dev´s

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,10 +371,134 @@ 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
+  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
+// Function implementing Sine PWM algorithms
+// - space vector not implemented yet
+//
+
+int32_t float_to_q31(float input) {
+    int32_t q31 = (int32_t) (input * 2147483648.0f);
+    q31 = (q31 > 0) ? (q31 << 1) : (-q31 << 1);
+    return q31;
+}
+
+#define pi 3.1415926535897932384626433f
+
+int32_t q31_value;
+float value_f32_sine = 0;
+float value_f32_cosine = 0;
+q31_t cordic_cosine;
+q31_t cordic_sine;
+
+float wrap_to_1(float x) {
+    while (x > 1.0f) {
+        x -= 2.0f;
+    }
+    while (x < -1.0f) {
+        x += 2.0f;
+    }
+    return x;
+}
+/// @runger();
+/// @brief 
+/// @param Uq 
+/// @param Ud 
+/// @param angle_el 
+
+#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));
+
+  /* Write angle and start CORDIC execution */
+
+ // 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;
+
+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, value_f32_sine, value_f32_cosine);
+
   // set the voltages in hardware
   driver->setPwm(Ualpha, Ubeta);
+
+
+
 }
 
+
 // Function (iterative) generating open loop movement for target velocity
 // - target_velocity - rad/s
 // it uses voltage_limit variable

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
@@ -89,10 +91,19 @@ class StepperMotor: public FOCMotor
     */
     void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
 
-  private:
+    // CORDIC
+
+    void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) override;
+
+
+    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();
 
@@ -113,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

@@ -137,6 +137,13 @@ class FOCMotor
     */
     virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0;
 
+
+    // CORDIC 
+
+    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();
@@ -157,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

@@ -5,28 +5,36 @@
 
 class StepperDriver{
     public:
-
-        /** Initialise hardware */
-        virtual int init() = 0;
-        /** Enable hardware */
-        virtual void enable() = 0;
-        /** Disable hardware */
-        virtual void disable() = 0;
-
-        long pwm_frequency; //!< pwm frequency value in hertz
-        float voltage_power_supply; //!< power supply voltage 
-        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
-
-        /** 
-         * Set phase voltages to the harware 
-         * 
-         * @param Ua phase A voltage
-         * @param Ub phase B voltage
-        */
-        virtual void setPwm(float Ua, float Ub) = 0;
+       /** Initialise hardware */
+virtual int init() = 0;
+/** Enable hardware */
+virtual void enable() = 0;
+/** Disable hardware */
+virtual void disable() = 0;
+
+long pwm_frequency; //!< pwm frequency value in hertz
+float voltage_power_supply; //!< power supply voltage 
+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 
+ * 
+ * @param Ua phase A voltage
+ * @param Ub phase B voltage
+ */
+virtual void setPwm(float Ua, float Ub) = 0;
+
+
+
 };
 
 #endif

src/common/foc_utils.cpp

@@ -69,4 +69,4 @@ __attribute__((weak)) float _sqrtApprox(float number) {//low in fat
   y = * ( float * ) &i;
   // y = y * ( f - ( x * y * y ) ); // better precision
   return number * y;
-}
+}

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;