Dev. Changes

Field-Stack Work

Author: Juanduino

src/SimpleFOC.h

@@ -108,10 +108,11 @@ void loop() {
 #include "drivers/BLDCDriver3PWM.h"
 #include "drivers/BLDCDriver6PWM.h"
 #include "drivers/StepperDriver4PWM.h"
+#include "drivers/StepperDriver8PWM.h"
 #include "drivers/StepperDriver2PWM.h"
-#include "current_sense/InlineCurrentSense.h"
-#include "current_sense/LowsideCurrentSense.h"
-#include "current_sense/GenericCurrentSense.h"
+//#include "current_sense/InlineCurrentSense.h"
+//#include "current_sense/LowsideCurrentSense.h"
+//#include "current_sense/GenericCurrentSense.h"
 #include "communication/Commander.h"
 #include "communication/StepDirListener.h"
 #include "communication/SimpleFOCDebug.h"

src/StepperMotor.cpp

@@ -1,5 +1,7 @@
 #include "StepperMotor.h"
 #include "./communication/SimpleFOCDebug.h"
+#include "arm_math.h"
+
 
 
 // StepperMotor(int pp)
@@ -187,19 +189,19 @@ int StepperMotor::alignSensor() {
     // align the electrical phases of the motor and sensor
     // set angle -90(270 = 3PI/2) degrees
     setPhaseVoltage(voltage_sensor_align, 0,  _3PI_2);
-    _delay(700);
+    _delay(2000);
     // read the sensor
     sensor->update();
     // get the current zero electric angle
     zero_electric_angle = 0;
     zero_electric_angle = electricalAngle();
-    _delay(20);
+    _delay(2000);
     if(monitor_port){
       SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
     }
     // stop everything
     setPhaseVoltage(0, 0, 0);
-    _delay(200);
+    _delay(1000);
   }else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
   return exit_flag;
 }
@@ -257,7 +259,7 @@ void StepperMotor::loopFOC() {
   electrical_angle = electricalAngle();
 
   // set the phase voltage - FOC heart function :)
-  setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
+  setPhaseVoltageCORDIC_LL(voltage.q, voltage.d, electrical_angle);
 }
 
 // Iterative function running outer loop of the FOC algorithm
@@ -357,6 +359,8 @@ void StepperMotor::move(float new_target) {
 void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
   // Sinusoidal PWM modulation
   // Inverse Park transformation
+////SerialUSB.println("ange_el ->  ");
+//SerialUSB.println(angle_el);
 
   // angle normalization in between 0 and 2pi
   // only necessary if using _sin and _cos - approximation functions
@@ -367,8 +371,18 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
   Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
   Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;
 
-  // set the voltages in hardware
+  //set the voltages in hardware
   driver->setPwm(Ualpha, Ubeta);
+/*
+  Serial.print("Ualpha:");
+  Serial.print(Ualpha, 8);
+  Serial.print(",");
+  Serial.print("Ubeta:");
+  Serial.print(Ubeta, 8);  
+  Serial.println();
+
+  */
+
 }
 
 // Method using FOC to set Uq and Ud to the motor at the optimal angle
@@ -387,8 +401,8 @@ int32_t float_to_q31(float input) {
 int32_t q31_value;
 float value_f32_sine = 0;
 float value_f32_cosine = 0;
-float cordic_cosine = 0.0f;
-float cordic_sine = 0.0f;
+q31_t cordic_cosine;
+q31_t cordic_sine;
 
 float wrap_to_1(float x) {
     while (x > 1.0f) {
@@ -404,38 +418,84 @@ float wrap_to_1(float x) {
 /// @param Uq 
 /// @param Ud 
 /// @param angle_el 
-void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) {
-  // Sinusoidal PWM modulation
-  // Inverse Park transformation
 
-  // WHO U GONNA CALL? CORDIC ->
+#define PI32f 3.141592f
+
+float cordic_sin_value;
+float cordic_cos_value;
+
+void StepperMotor::setPhaseVoltageCORDIC_LL(float Uq, float Ud, float angle_el) {
+  
+ 
+ // convert angle flot to CORDICq31 format
+  uint32_t angle31 = (uint32_t)(angle_el * (1UL << 31) / (1.0f * PI));
 
-q31_value = float_to_q31(angle_el / (2.0f * pi));
+  /* Write angle and start CORDIC execution */
 
-CORDIC->WDATA = q31_value;
+ // WHO U GONNA CALL? CORDIC ->
+
+ CORDIC->WDATA = angle31;
+ q31_t cosOutput = (int32_t)CORDIC->RDATA;
+
+   // convert q31 result to float
+  cordic_cos_value = (float)cosOutput / (float)0x80000000;
+
+  /* Read sine */
+  q31_t sinOutput = (int32_t)CORDIC->RDATA;
+
+  // convert q31 results to float
+  cordic_sin_value = (float)sinOutput / (float)0x80000000;
+
+
+
+//value_f32_sine = wrap_to_1(value_f32_sine);
+//value_f32_cosine = wrap_to_1(value_f32_cosine);
+
+
+  // Inverse park transform
+  //Ualpha =  value_f32_cosine * Ud - value_f32_sine * Uq;  // -sin(angle) * Uq;
+  //Ubeta =  value_f32_sine * Ud + value_f32_cosine * Uq;    //  cos(angle) * Uq;
+
+  arm_inv_park_f32(Ud, Uq, &Ualpha, &Ubeta, cordic_sin_value, cordic_cos_value);
+
+ 
+
+  // set the voltages in hardware
+ driver->setPwm(Ualpha, Ubeta);
+
+ 
+  
+}
+
+ 
+void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) {
+  
+ 
+uint32_t q1_31_angle = (uint32_t)(angle_el * (1UL << 31) / (1.0f * PI));
+
+ // WHO U GONNA CALL? CORDIC ->
+CORDIC->WDATA = q1_31_angle;
 cordic_sine = CORDIC->RDATA;
 cordic_cosine = CORDIC->RDATA;
 
 value_f32_sine = (float)cordic_sine/(float)0x80000000;
 value_f32_cosine = (float)cordic_cosine/(float)0x80000000;
 
-if (angle_el < 0){
 value_f32_sine = wrap_to_1(value_f32_sine);
-value_f32_sine = value_f32_sine * -1;
-}
-
-if (angle_el > 0){
-value_f32_sine = wrap_to_1(value_f32_sine);
-}
-
 value_f32_cosine = wrap_to_1(value_f32_cosine);
 
+
   // Inverse park transform
-  Ualpha =  value_f32_cosine * Ud - value_f32_sine * Uq;  // -sin(angle) * Uq;
-  Ubeta =  value_f32_sine * Ud + value_f32_cosine * Uq;    //  cos(angle) * Uq;
+  //Ualpha =  value_f32_cosine * Ud - value_f32_sine * Uq;  // -sin(angle) * Uq;
+  //Ubeta =  value_f32_sine * Ud + value_f32_cosine * Uq;    //  cos(angle) * Uq;
+
+  arm_inv_park_f32(Ud, Uq, &Ualpha, &Ubeta, value_f32_sine, value_f32_cosine);
 
   // set the voltages in hardware
   driver->setPwm(Ualpha, Ubeta);
+
+ 
+  
 }
 
 

src/StepperMotor.h

@@ -79,6 +79,8 @@ class StepperMotor: public FOCMotor
 
     float	Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform
 
+    uint16_t countervalue; //!< counter value for the open loop control TIM3->CNT;
+
   /**
     * Method using FOC to set Uq to the motor at the optimal angle
     * Heart of the FOC algorithm
@@ -93,10 +95,15 @@ class StepperMotor: public FOCMotor
 
     void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) override;
 
-  private:
+
+    void setPhaseVoltageCORDIC_LL(float Uq, float Ud, float angle_el) override;
+
+    int alignSensor();
+
+ 
 
     /** Sensor alignment to electrical 0 angle of the motor */
-    int alignSensor();
+    
     /** Motor and sensor alignment to the sensors absolute 0 angle  */
     int absoluteZeroSearch();
 
@@ -117,6 +124,8 @@ class StepperMotor: public FOCMotor
     float angleOpenloop(float target_angle);
     // open loop variables
     long open_loop_timestamp;
+
+     private:
 };
 
 

src/common/base_classes/FOCMotor.h

@@ -142,6 +142,8 @@ class FOCMotor
 
     virtual void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el)=0;
 
+    virtual void setPhaseVoltageCORDIC_LL(float Uq, float Ud, float angle_el)=0;
+
     // State calculation methods 
     /** Shaft angle calculation in radians [rad] */
     float shaftAngle();
@@ -162,6 +164,7 @@ class FOCMotor
     float target; //!< current target value - depends of the controller
   	float shaft_angle;//!< current motor angle
   	float electrical_angle;//!< current electrical angle
+    float electrical_angle2;//!< current electrical angle
   	float shaft_velocity;//!< current motor velocity 
     float current_sp;//!< target current ( q current )
     float shaft_velocity_sp;//!< current target velocity

src/common/base_classes/Sensor.cpp

@@ -17,14 +17,14 @@ void Sensor::update() {
  /** get current angular velocity (rad/s) */
 float Sensor::getVelocity() {
     // calculate sample time
-    float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
-    // TODO handle overflow - we do need to reset vel_angle_prev_ts
-    if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
-
-    velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;
-    vel_angle_prev = angle_prev;
-    vel_full_rotations = full_rotations;
-    vel_angle_prev_ts = angle_prev_ts;
+   bool isCountingUpward = TIM3->CR1 & 0x0010 ? false : true;
+  
+   uint32_t timer2 = TIM2 -> CCR1;
+   float velocity = (4 * PI / 65536) * 168000000 / ((float)timer2 - 1.0f);
+
+   if (!isCountingUpward) {
+        velocity = -velocity;}
+  
     return velocity;
 }
 

src/common/base_classes/StepperDriver.h

@@ -19,6 +19,12 @@ float voltage_limit; //!< limiting voltage set to the motor
 bool initialized = false; // true if driver was successfully initialized
 void* params = 0; // pointer to hardware specific parameters of driver
 
+float dc_1a; //!< currently set duty cycle on phaseA
+float dc_1b; //!< currently set duty cycle on phaseB
+float dc_1c; //!< currently set duty cycle on phaseC
+float dc_1d; //!< currently set duty cycle on phaseC
+
+
 /** 
  * Set phase voltages to the hardware 
  * 

src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp

@@ -241,7 +241,6 @@ void RP2040ADCEngine::start() {
 
 void RP2040ADCEngine::stop() {
     adc_run(false);
-    irq_set_enabled(DMA_IRQ_0, false);
     dma_channel_abort(readDMAChannel);
     // if (triggerPWMSlice>=0)
     //     dma_channel_abort(triggerDMAChannel);

src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp

@@ -187,7 +187,7 @@ uint32_t _timerToRegularTRGO(HardwareTimer* timer){
 #endif
 #ifdef TIM8 // if defined timer 8
   else if(timer->getHandle()->Instance == TIM8) 
-    return ADC_EXTERNALTRIG_T8_TRGO;
+    return ADC_EXTERNALTRIG_T7_TRGO;
 #endif
 #ifdef TIM15 // if defined timer 15
   else if(timer->getHandle()->Instance == TIM15) 

src/drivers/BLDCDriver6PWM.cpp

@@ -55,7 +55,7 @@ int BLDCDriver6PWM::init() {
   pinMode(pwmB_l, OUTPUT);
   pinMode(pwmC_l, OUTPUT);
   if(_isset(enable_pin)) pinMode(enable_pin, OUTPUT);
-
+  Serial.println("Setting Pins to Output!");
 
   // sanity check for the voltage limit configuration
   if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
@@ -73,6 +73,12 @@ int BLDCDriver6PWM::init() {
   params = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
   initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);
   return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
+
+ // set phase state to enabled - if driver was successfully initialized
+  phase_state[0] = PhaseState::PHASE_ON;
+  phase_state[1] = PhaseState::PHASE_ON;
+  phase_state[2] = PhaseState::PHASE_ON;
+  
 }
 
 

src/drivers/BLDCDriver6PWM.h

@@ -36,6 +36,7 @@ class BLDCDriver6PWM: public BLDCDriver
   	int pwmA_h,pwmA_l; //!< phase A pwm pin number
   	int pwmB_h,pwmB_l; //!< phase B pwm pin number
   	int pwmC_h,pwmC_l; //!< phase C pwm pin number
+    
     int enable_pin; //!< enable pin number
     bool enable_active_high = true;