Initial 8PWM

Author: Juanduino

src/StepperMotor.cpp

@@ -371,6 +371,70 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
   driver->setPwm(Ualpha, Ubeta);
 }
 
+// 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;
+float cordic_cosine = 0.0f;
+float cordic_sine = 0.0f;
+
+float wrap_to_1(float x) {
+    while (x > 1.0f) {
+        x -= 2.0f;
+    }
+    while (x < -1.0f) {
+        x += 2.0f;
+    }
+    return x;
+}
+
+void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) {
+  // Sinusoidal PWM modulation
+  // Inverse Park transformation
+
+  // WHO U GONNA CALL? CORDIC ->
+
+q31_value = float_to_q31(angle_el / (2.0f * pi));
+
+CORDIC->WDATA = q31_value;
+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 < 0){
+value_f32_sine = wrap_to_1(value_f32_sine);
+value_f32_sine = value_f32_sine * -1;
+}
+
+if (angle > 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 - cordic_sine * Uq;  // -sin(angle) * Uq;
+  Ubeta =  cordic_sine * Ud + value_f32_cosine * Uq;    //  cos(angle) * Uq;
+
+  // 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

@@ -89,6 +89,10 @@ class StepperMotor: public FOCMotor
     */
     void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
 
+    // CORDIC
+
+    void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) override;
+
   private:
 
     /** Sensor alignment to electrical 0 angle of the motor */

src/common/base_classes/StepperDriver.h

@@ -5,28 +5,30 @@
 
 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
+
+/** 
+ * 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/drivers/hardware_api.h

@@ -36,7 +36,7 @@
 // will be returned as a void pointer from the _configurexPWM functions
 // will be provided to the _writeDutyCyclexPWM() as a void pointer
 typedef struct GenericDriverParams {
-  int pins[6];
+  int pins[8];
   long pwm_frequency;
   float dead_zone;
 } GenericDriverParams;
@@ -114,6 +114,27 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const
  */
 void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l);
 
+/** 
+ * Configuring PWM frequency, resolution and alignment
+ * - Stepper driver - 8PWM setting
+ * - hardware specific
+ * 
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pin1A pin1A stepper driver
+ * @param pin1B pin1B stepper driver
+ * @param pin2A pin2A stepper driver
+ * @param pin2B pin2B stepper driver
+ * @param pin3A pin3A stepper driver
+ * @param pin3B pin3B stepper driver
+ * @param pin4A pin4A stepper driver
+ * @param pin4B pin4B stepper driver
+ * 
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
+ */
+void* _configure8PWM(long pwm_frequency, float dead_zone, const int pin1A, const int pin1B, const int pin2A, const int pin2B,
+                     const int pin3A, const int pin3B, const int pin4A, const int pin4B);
+
+
 /** 
  * Function setting the duty cycle to the pwm pin (ex. analogWrite())
  * - Stepper driver - 2PWM setting
@@ -177,4 +198,23 @@ void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, vo
 void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params);
 
 
+/**
+
+*Function setting the duty cycle to the pwm pin (ex. analogWrite())
+* - Stepper driver - 8PWM setting
+* - hardware specific
+*@param dc_1a duty cycle phase 1A [0, 1]
+*@param dc_1b duty cycle phase 1B [0, 1]
+*@param dc_1c duty cycle phase 1C [0, 1]
+*@param dc_1d duty cycle phase 1D [0, 1]
+*@param dc_2a duty cycle phase 2A [0, 1]
+*@param dc_2b duty cycle phase 2B [0, 1]
+*@param dc_2c duty cycle phase 2C [0, 1]
+*@param dc_2d duty cycle phase 2D [0, 1]
+*@param params the driver parameters
+*/
+
+void _writeDutyCycle8PWM(float dc_1a, float dc_1b, float dc_1c, float dc_1d, float dc_2a, float dc_2b, float dc_2c, float dc_2d, void* params);
+
+
 #endif