check for floating point error due to insufficient precision

Author: ATILIUS-REGULUS

src/BLDCMotor.cpp

@@ -29,7 +29,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
   driver = _driver;
 }
 
-// init hardware pins   
+// init hardware pins
 void BLDCMotor::init() {
   if(monitor_port) monitor_port->println("MOT: Initialise variables.");
   // sanity check for the voltage limit configuration
@@ -53,13 +53,13 @@ void BLDCMotor::disable()
 {
   // set zero to PWM
   driver->setPwm(0, 0, 0);
-  // disable the driver 
+  // disable the driver
   driver->disable();
 }
 // enable motor driver
 void BLDCMotor::enable()
 {
-  // enable the driver 
+  // enable the driver
   driver->enable();
   // set zero to PWM
   driver->setPwm(0, 0, 0);
@@ -93,7 +93,7 @@ int  BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction
 int BLDCMotor::alignSensor() {
   if(monitor_port) monitor_port->println("MOT: Align sensor.");
   // align the electrical phases of the motor and sensor
-  // set angle -90 degrees 
+  // set angle -90 degrees
 
   float start_angle = shaftAngle();
   for (int i = 0; i <=5; i++ ) {
@@ -134,19 +134,19 @@ int BLDCMotor::alignSensor() {
 }
 
 
-// Encoder alignment the absolute zero angle 
+// Encoder alignment the absolute zero angle
 // - to the index
 int BLDCMotor::absoluteZeroAlign() {
 
   if(monitor_port) monitor_port->println("MOT: Absolute zero align.");
     // if no absolute zero return
   if(!sensor->hasAbsoluteZero()) return 0;
-  
+
 
   if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching...");
   // search the absolute zero with small velocity
   while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){
-    loopFOC();   
+    loopFOC();
     voltage_q = PID_velocity(velocity_index_search - shaftVelocity());
   }
   voltage_q = 0;
@@ -167,9 +167,9 @@ int BLDCMotor::absoluteZeroAlign() {
 // Iterative function looping FOC algorithm, setting Uq on the Motor
 // The faster it can be run the better
 void BLDCMotor::loopFOC() {
-  // shaft angle 
+  // shaft angle
   shaft_angle = shaftAngle();
-  // set the phase voltage - FOC heart function :) 
+  // set the phase voltage - FOC heart function :)
   setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs));
 }
 
@@ -219,7 +219,7 @@ void BLDCMotor::move(float new_target) {
 
 // Method using FOC to set Uq and Ud to the motor at the optimal angle
 // Function implementing Space Vector PWM and Sine PWM algorithms
-// 
+//
 // Function using sine approximation
 // regular sin + cos ~300us    (no memory usaage)
 // approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
@@ -257,7 +257,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       };
       // 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;
@@ -272,7 +272,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
     break;
 
     case FOCModulationType::SinePWM :
-      // Sinusoidal PWM modulation 
+      // Sinusoidal PWM modulation
       // Inverse Park + Clarke transformation
 
       // angle normalization in between 0 and 2pi
@@ -299,10 +299,10 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       break;
 
     case FOCModulationType::SpaceVectorPWM :
-      // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm 
+      // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
       // https://www.youtube.com/watch?v=QMSWUMEAejg
 
-      // if negative voltages change inverse the phase 
+      // if negative voltages change inverse the phase
       // angle + 180degrees
       if(Uq < 0) angle_el += _PI;
       Uq = abs(Uq);
@@ -316,14 +316,14 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       // calculate the duty cycles
       float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit;
       float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit;
-      // two versions possible 
+      // two versions possible
       float T0 = 0; // pulled to 0 - better for low power supply voltage
-      if (centered) { 
+      if (centered) {
         T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2
-      } 
+      }
 
       // calculate the duty cycles(times)
-      float Ta,Tb,Tc; 
+      float Ta,Tb,Tc;
       switch(sector){
         case 1:
           Ta = T1 + T2 + T0/2;
@@ -369,7 +369,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       break;
 
   }
-  
+
   // set the voltages in driver
   driver->setPwm(Ua, Ub, Uc);
 }
@@ -386,7 +386,13 @@ void BLDCMotor::velocityOpenloop(float target_velocity){
   float Ts = (now_us - open_loop_timestamp) * 1e-6;
 
   // calculate the necessary angle to achieve target velocity
-  shaft_angle += target_velocity*Ts; 
+  shaft_angle += target_velocity*Ts;
+
+  // check if shaft_angle gets too large
+  if (shaft_angle >= (_2PI * 4))
+    {
+      shaft_angle = 0;
+    }
 
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
   setPhaseVoltage(voltage_limit,  0, _electricalAngle(shaft_angle, pole_pairs));
@@ -403,17 +409,17 @@ void BLDCMotor::angleOpenloop(float target_angle){
   unsigned long now_us = _micros();
   // calculate the sample time from last call
   float Ts = (now_us - open_loop_timestamp) * 1e-6;
-  
+
   // calculate the necessary angle to move from current position towards target angle
   // with maximal velocity (velocity_limit)
   if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts))
-    shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; 
+    shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts;
   else
     shaft_angle = target_angle;
-  
+
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
   setPhaseVoltage(voltage_limit,  0, _electricalAngle(shaft_angle, pole_pairs));
 
   // save timestamp for next call
   open_loop_timestamp = now_us;
-}
+}