Diff: MotorControl.cpp

Revision:

3:170c22171ec2

Parent:

2:0d5c994d8135

Child:

4:66154ae82263

--- a/MotorControl.cpp	Thu Jun 01 13:16:50 2017 +0000
+++ b/MotorControl.cpp	Wed Jul 05 20:29:37 2017 +0000
@@ -9,6 +9,8 @@
 {
     uint8_t i;
     
+    PWMRegulatedMode = false;
+    
     motorCurrentIndex = 0;
     batVoltageIndex = 0;
     
@@ -97,13 +99,28 @@
     return (getWheelSpeed('A')+getWheelSpeed('B'))/2.0;
 }
 
+void Motors::setPWM(float value)
+{
+    fixedPWM = (int16_t)(INT_MAX*value);
+}
+
+void Motors::setFixedPWMMode(void)
+{
+    PWMRegulatedMode = false;
+}
+
+void Motors::setRegulatedPWMMode(void)
+{
+    PWMRegulatedMode = true;
+}
+
 void Motors::processTasks()
 {
     int16_t nextDelta;
     int32_t torqueDiffAvg,PWMSumAvg,motorSumAvg,meanVoltage,nextp;
     uint8_t i,index;
 
-    if(motorDriveIndex != motorCurrentIndex)
+    if(motorDriveIndex != motorCurrentIndex) // process data only if a new data is available
     {
         motorDriveIndex = motorCurrentIndex;
         
@@ -114,6 +131,7 @@
         // TODO: verify indexes
         for(i=0;i<10;i++)
         {
+            // compute average values over 10 last measures
             torqueDiffAvg += motorACurrent[index] - motorBCurrent[index];
             PWMSumAvg += motorAPWM[index]+motorBPWM[index];
             motorSumAvg += motorACurrent[index]+motorBCurrent[index];
@@ -122,28 +140,36 @@
             index--;
         }
         
-/*        a0 = 1000000;
-        a1 = 93;        // 10/a0/9.8004/pi/0.05*2*INT_MAX/3.3*4095/5.7;
-        a2 = 1099919;   // a0*7.0023*pi*0.05;
-        a3 = 9996209;   // 10/a0/2.255/pi/0.05*2*INT_MAX/3.3*4095/5.7/3.3*4095*2*10;
-        a4 = 67;        // a0*10.51*pi*0.05*3.3/4095/2/10;
+        if(PWMRegulatedMode)
+        {
+/*
+            a0 = 1000000;
+            a1 = 93;        // 10/a0/9.8004/pi/0.05*2*INT_MAX/3.3*4095/5.7;
+            a2 = 1099919;   // a0*7.0023*pi*0.05;
+            a3 = 9996209;   // 10/a0/2.255/pi/0.05*2*INT_MAX/3.3*4095/5.7/3.3*4095*2*10;
+            a4 = 67;        // a0*10.51*pi*0.05*3.3/4095/2/10;
 */
-        meanVoltage = PWMSumAvg*batteryVoltage[batVoltageIndex];
-//        speederror1000000 = meanVoltage/a1-a2-(meanVoltage/a3+a4)*motorSumAvg - speedTarget*a0;
-        speederror1000000 = meanVoltage/93-1099919-(meanVoltage/9996209+67)*motorSumAvg - speedTarget;
+            // compute current speed to get error when compared to target
+            meanVoltage = PWMSumAvg*batteryVoltage[batVoltageIndex];
+//          speederror1000000 = meanVoltage/a1-a2-(meanVoltage/a3+a4)*motorSumAvg - speedTarget*a0;
+            speederror1000000 = meanVoltage/93-1099919-(meanVoltage/9996209+67)*motorSumAvg - speedTarget;
         
-        /*nextp = speederror1000000/-20;
-        if(nextp > INT_MAX)
-            nextPWM = INT_MAX;
-        else if (nextp < INT_MIN)
-            nextPWM = INT_MIN;
+            /*nextp = speederror1000000/-20;
+            if(nextp > INT_MAX)
+                nextPWM = INT_MAX;
+            else if (nextp < INT_MIN)
+                nextPWM = INT_MIN;
+            else
+                nextPWM = (int16_t)nextp;
+            nextDelta = 0;*/
+        }
         else
-            nextPWM = (int16_t)nextp;
-        nextDelta = 0;*/
+        {
+            nextPWM = fixedPWM; // 14745
+        }
         
-        nextPWM = 14745;
         nextDelta = (int16_t)(torqueDiffAvg/2); // delta to apply is about half the sum (0x7fff/(10*1240.9)/5)
-
+        
         // compute MotA PWM
 /*        if ((nextDelta > 0 && nextPWM < INT_MIN + nextDelta) ||
            (nextDelta < 0 && nextPWM > INT_MAX + nextDelta))