Diff: MotorControl.cpp

Revision:

4:66154ae82263

Parent:

3:170c22171ec2

--- a/MotorControl.cpp	Wed Jul 05 20:29:37 2017 +0000
+++ b/MotorControl.cpp	Wed Feb 20 22:14:07 2019 +0000
@@ -51,9 +51,20 @@
     if(motorCurrentIndex >= MOTSAMPLECOUNT)
         motorCurrentIndex = 0;
     
-    motorACurrent[motorCurrentIndex] = MotA_ADCresult;
-    motorBCurrent[motorCurrentIndex] = MotB_ADCresult;
+    // current is always measured as positive. Need to check direction of motor drive to know the sign
+    // current value stored is directly value in volt at input of ADC, not real current in Amp
+    if(currentMotAPWM >= 0)
+        motorACurrent[motorCurrentIndex] = (float)MotA_ADCresult/1240.9090; // *3.3/4095
+    else
+        motorACurrent[motorCurrentIndex] = -(float)MotA_ADCresult/1240.9090; // *3.3/4095
     
+    if(currentMotBPWM >= 0)
+        motorBCurrent[motorCurrentIndex] = (float)MotB_ADCresult/1240.9090; // *3.3/4095
+    else
+        motorBCurrent[motorCurrentIndex] = -(float)MotB_ADCresult/1240.9090; // *3.3/4095
+    
+    motorAPWM[motorCurrentIndex] = currentMotAPWM;
+    motorBPWM[motorCurrentIndex] = currentMotBPWM;
 /*    index = motorCurrentIndex;
     for(i=0;i<10;i++)
     {
@@ -66,32 +77,49 @@
     torqueDiffAvg = sum;*/
 }
 
+float Motors::getAverageSpeed()
+{
+    float currentA = 0, currentB = 0, meanVoltageA = 0, meanVoltageB = 0;
+    uint8_t index = motorCurrentIndex;
+    uint8_t i,avg = 10;
+    
+    for(i=0;i<avg;i++)
+    {
+        currentA += motorACurrent[index];
+        meanVoltageA += ((float)motorAPWM[index])*(float)batteryVoltage[batVoltageIndex]; // *3.3/4095*5.7
+        currentB += motorBCurrent[index];
+        meanVoltageB += ((float)motorBPWM[index])*(float)batteryVoltage[batVoltageIndex]; // *3.3/4095*5.7
+        if(index == 0)
+            index = MOTSAMPLECOUNT;
+        index--;
+    }
+
+    // freeSpeed = 9.8004*meanVoltage-7.0023
+    // speed = (freeSpeed - (2.255*meanVoltage+10.51)*current)
+    return (((9.8004/INT_MAX/217.7*meanVoltageA-7.0023)-(2.255/INT_MAX/217.7*meanVoltageA+10.51)*currentA)+((9.8004/INT_MAX/217.7*meanVoltageB-7.0023)-(2.255/INT_MAX/217.7*meanVoltageB+10.51)*currentB))/avg/2*0.157; // m/s
+}
+
 float Motors::getWheelRPS(char mot)
 {
     float current,meanVoltage;
     uint8_t index = motorCurrentIndex;
-    
-    
+        
     // TODO: optimise computation time
     if(mot == 'A')
     {
-        current = (float)motorACurrent[index]/1240.9; // *3.3/4095
         meanVoltage = ((float)motorAPWM[index])/INT_MAX*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
+        return ((9.829651257*meanVoltage)-(19.33709258*motorACurrent[index])-6.435176945);
     }
     else
     {
-        current = (float)motorBCurrent[index]/1240.9; // *3.3/4095
         meanVoltage = ((float)motorBPWM[index])/INT_MAX*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
+        return ((9.829651257*meanVoltage)-(19.33709258*motorBCurrent[index])-6.435176945);
     }
-    
-    // freeSpeed = 9.8004*meanVoltage-7.0023
-    // speed = (freeSpeed - (2.255*meanVoltage+10.51)*current)
-    return ((9.8004*meanVoltage-7.0023)-(2.255*meanVoltage+10.51)*current); // rps
-}
+ }
 
 float Motors::getWheelSpeed(char mot)
 {
-    return getWheelRPS(mot)*0.157; // in m/s (pi*0.05)
+    return getWheelRPS(mot)*0.15707963267; // in m/s (pi*0.05)
 }
 
 float Motors::getCarSpeed()
@@ -99,11 +127,6 @@
     return (getWheelSpeed('A')+getWheelSpeed('B'))/2.0;
 }
 
-void Motors::setPWM(float value)
-{
-    fixedPWM = (int16_t)(INT_MAX*value);
-}
-
 void Motors::setFixedPWMMode(void)
 {
     PWMRegulatedMode = false;
@@ -120,9 +143,9 @@
     int32_t torqueDiffAvg,PWMSumAvg,motorSumAvg,meanVoltage,nextp;
     uint8_t i,index;
 
-    if(motorDriveIndex != motorCurrentIndex) // process data only if a new data is available
+    if(motorDriveIndex != ((motorCurrentIndex+1)%MOTSAMPLECOUNT)) // process data only if a new data is available
     {
-        motorDriveIndex = motorCurrentIndex;
+        motorDriveIndex = motorCurrentIndex+1; // current is saved with the PWM, compute the next PWM now
         
         torqueDiffAvg = 0;
         PWMSumAvg = 0;
@@ -169,33 +192,32 @@
         }
         
         nextDelta = (int16_t)(torqueDiffAvg/2); // delta to apply is about half the sum (0x7fff/(10*1240.9)/5)
+        nextDelta = 0;
         
         // compute MotA PWM
 /*        if ((nextDelta > 0 && nextPWM < INT_MIN + nextDelta) ||
            (nextDelta < 0 && nextPWM > INT_MAX + nextDelta))
-            motorAPWM[motorDriveIndex] = INT_MIN;
+            currentMotAPWM = INT_MIN;
         else
-            motorAPWM[motorDriveIndex] = nextPWM - nextDelta;*/
-        motorAPWM[motorDriveIndex] = nextPWM-nextDelta;
+            currentMotAPWM = nextPWM - nextDelta;*/
+        currentMotAPWM = nextPWM-nextDelta;
     
         // compute MotB PWM
 /*        if (((nextDelta > 0) && (nextPWM > (INT_MAX - nextDelta))) ||
            ((nextDelta < 0) && (nextPWM < (INT_MIN - nextDelta))))
-            motorBPWM[motorDriveIndex] = INT_MAX;
+            currentMotBPWM = INT_MAX;
         else
-            motorBPWM[motorDriveIndex] = nextPWM + nextDelta;*/
-        motorBPWM[motorDriveIndex] = nextPWM+nextDelta;
+            currentMotBPWM = nextPWM + nextDelta;*/
+        currentMotBPWM = nextPWM+nextDelta;
 
-        currentMotAPWM = ((float)motorAPWM[motorDriveIndex])/INT_MAX;
-        currentMotBPWM = ((float)motorBPWM[motorDriveIndex])/INT_MAX;
-        TFC_SetMotorPWM(currentMotAPWM,currentMotBPWM);
+        TFC_SetMotorPWM(getMotPWM('A'),getMotPWM('B'));
     }
 }
 
 float Motors::getAverageMotCurrent(char mot)
 {
     uint8_t i;
-    uint32_t sum = 0;
+    float sum = 0;
     
     if(mot == 'A')
     {
@@ -212,23 +234,23 @@
         }
     }
     
-    return ((float)sum)/MOTSAMPLECOUNT/1240.9;
+    return sum/MOTSAMPLECOUNT;
 }
 
 float Motors::getMotCurrent(char mot)
 {
     if(mot == 'A')
-        return (float)motorACurrent[motorCurrentIndex]/1240.9;
+        return motorACurrent[motorCurrentIndex];
     else
-        return (float)motorBCurrent[motorCurrentIndex]/1240.9;
+        return motorBCurrent[motorCurrentIndex];
 }
 
 float Motors::getMotPWM(char mot)
 {
     if(mot == 'A')
-        return currentMotAPWM;
+        return ((float)currentMotAPWM)/INT_MAX;
     else
-        return currentMotBPWM;
+        return ((float)currentMotBPWM)/INT_MAX;
 }
 
 float Motors::getAverageBatteryVoltage()
@@ -245,7 +267,7 @@
 
 void Motors::setFixedPWMValue(float pwm)
 {
-    nextPWM = (int16_t)(pwm*INT_MAX);
+    fixedPWM = (int16_t)(pwm*INT_MAX);
 }
 
 void Motors::setSpeedTargetValue(float speed)