moving the case statements to avoid cross initialization

Author: owennewo

src/BLDCMotor.cpp

@@ -226,9 +226,50 @@ void BLDCMotor::move(float new_target) {
 void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
 
   const bool centered = true;
+  int sector;
 
   switch (foc_modulation)
   {
+    case FOCModulationType::Trapezoid_120 :
+      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
+      static int trap_120_map[6][3] = {
+        {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+      };
+      // static int trap_120_state = 0;
+      sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes
+
+      Ua = Uq + trap_120_map[sector][0] * Uq;
+      Ub = Uq + trap_120_map[sector][1] * Uq;
+      Uc = Uq + trap_120_map[sector][2] * Uq;
+
+      if (centered) {
+        Ua += (voltage_power_supply)/2 -Uq;
+        Ub += (voltage_power_supply)/2 -Uq;
+        Uc += (voltage_power_supply)/2 -Uq;
+      }
+    break;
+
+    case FOCModulationType::Trapezoid_150 :
+      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
+      static int trap_150_map[12][3] = {
+        {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+      };
+      // static int trap_150_state = 0;
+      sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes
+      
+      Ua = Uq + trap_150_map[sector][0] * Uq;
+      Ub = Uq + trap_150_map[sector][1] * Uq;
+      Uc = Uq + trap_150_map[sector][2] * Uq;
+
+      //center
+      if (centered) {
+        Ua += (voltage_power_supply)/2 -Uq;
+        Ub += (voltage_power_supply)/2 -Uq;
+        Uc += (voltage_power_supply)/2 -Uq;
+      }
+
+    break;
+
     case FOCModulationType::SinePWM :
       // Sinusoidal PWM modulation 
       // Inverse Park + Clarke transformation
@@ -268,7 +309,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2);
 
       // find the sector we are in currently
-      int sector = floor(angle_el / _PI_3) + 1;
+      sector = floor(angle_el / _PI_3) + 1;
       // calculate the duty cycles
       float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_power_supply;
       float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_power_supply;
@@ -324,45 +365,6 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       Uc = Tc*driver->voltage_power_supply;
       break;
 
-    case FOCModulationType::Trapezoid_120 :
-      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
-      static int trap_120_map[6][3] = {
-        {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
-      };
-      static int trap_120_state = 0;
-      trap_120_state = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes
-
-      Ua = Uq + trap_120_map[trap_120_state][0] * Uq;
-      Ub = Uq + trap_120_map[trap_120_state][1] * Uq;
-      Uc = Uq + trap_120_map[trap_120_state][2] * Uq;
-
-      if (centered) {
-        Ua += (voltage_power_supply)/2 -Uq;
-        Ub += (voltage_power_supply)/2 -Uq;
-        Uc += (voltage_power_supply)/2 -Uq;
-      }
-    break;
-
-    case FOCModulationType::Trapezoid_150 :
-      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
-      static int trap_150_map[12][3] = {
-        {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
-      };
-      static int trap_150_state = 0;
-      trap_150_state = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes
-      
-      Ua = Uq + trap_150_map[trap_150_state][0] * Uq;
-      Ub = Uq + trap_150_map[trap_150_state][1] * Uq;
-      Uc = Uq + trap_150_map[trap_150_state][2] * Uq;
-
-      //center
-      if (centered) {
-        Ua += (voltage_power_supply)/2 -Uq;
-        Ub += (voltage_power_supply)/2 -Uq;
-        Uc += (voltage_power_supply)/2 -Uq;
-      }
-
-    break;
   }
 
   // set the voltages in driver