Diff: MotorControl.cpp

Revision:

2:0d5c994d8135

Parent:

0:a1bb4583940a

Child:

3:170c22171ec2

--- a/MotorControl.cpp	Fri Mar 25 13:03:59 2016 +0000
+++ b/MotorControl.cpp	Thu Jun 01 13:16:50 2017 +0000
@@ -1,12 +1,24 @@
 #include "TFC.h"
 #include "MotorControl.h"
+//#include "limits.h"
+
+#define INT_MAX 32767
+#define INT_MIN -32768
 
 Motors::Motors()
 {
+    uint8_t i;
+    
     motorCurrentIndex = 0;
     batVoltageIndex = 0;
     
-    steeringAngle = 0;
+    for(i=0;i<MOTSAMPLECOUNT;i++)
+    {
+        motorACurrent[i] = 0;
+        motorBCurrent[i] = 0;
+        motorAPWM[i] = 0;
+        motorAPWM[i] = 0;
+    }
 }
 
 void Motors::start()
@@ -30,29 +42,45 @@
 
 void Motors::saveMotorCurrentMeasure(uint16_t MotA_ADCresult, uint16_t MotB_ADCresult)
 {
+//    uint8_t i,index;
+//    int16_t sum = 0;
+    
     motorCurrentIndex++;
     if(motorCurrentIndex >= MOTSAMPLECOUNT)
         motorCurrentIndex = 0;
     
     motorACurrent[motorCurrentIndex] = MotA_ADCresult;
     motorBCurrent[motorCurrentIndex] = MotB_ADCresult;
-}
-
-void Motors::saveSteering(float angle)
-{
-    steeringAngle = angle;
+    
+/*    index = motorCurrentIndex;
+    for(i=0;i<10;i++)
+    {
+        sum += motorACurrent[index] - motorBCurrent[index];
+        if(index == 0)
+            index = MOTSAMPLECOUNT;
+        index--;
+    }
+    
+    torqueDiffAvg = sum;*/
 }
 
 float Motors::getWheelRPS(char mot)
 {
     float current,meanVoltage;
+    uint8_t index = motorCurrentIndex;
     
+    
+    // TODO: optimise computation time
     if(mot == 'A')
-        current = (float)motorACurrent[motorCurrentIndex]/1240.9; // *3.3/4095
+    {
+        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
+    }
     else
-        current = (float)motorBCurrent[motorCurrentIndex]/1240.9; // *3.3/4095
-    
-    meanVoltage = currentPWM*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
+    {
+        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
+    }
     
     // freeSpeed = 9.8004*meanVoltage-7.0023
     // speed = (freeSpeed - (2.255*meanVoltage+10.51)*current)
@@ -61,12 +89,81 @@
 
 float Motors::getWheelSpeed(char mot)
 {
-    return getWheelRPS(mot)*0.157;
+    return getWheelRPS(mot)*0.157; // in m/s (pi*0.05)
+}
+
+float Motors::getCarSpeed()
+{
+    return (getWheelSpeed('A')+getWheelSpeed('B'))/2.0;
 }
 
 void Motors::processTasks()
 {
+    int16_t nextDelta;
+    int32_t torqueDiffAvg,PWMSumAvg,motorSumAvg,meanVoltage,nextp;
+    uint8_t i,index;
+
+    if(motorDriveIndex != motorCurrentIndex)
+    {
+        motorDriveIndex = motorCurrentIndex;
+        
+        torqueDiffAvg = 0;
+        PWMSumAvg = 0;
+        motorSumAvg = 0;
+        index = motorDriveIndex;
+        // TODO: verify indexes
+        for(i=0;i<10;i++)
+        {
+            torqueDiffAvg += motorACurrent[index] - motorBCurrent[index];
+            PWMSumAvg += motorAPWM[index]+motorBPWM[index];
+            motorSumAvg += motorACurrent[index]+motorBCurrent[index];
+            if(index == 0)
+                index = MOTSAMPLECOUNT;
+            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;
+*/
+        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;
+        else
+            nextPWM = (int16_t)nextp;
+        nextDelta = 0;*/
+        
+        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))
+            motorAPWM[motorDriveIndex] = INT_MIN;
+        else
+            motorAPWM[motorDriveIndex] = nextPWM - nextDelta;*/
+        motorAPWM[motorDriveIndex] = nextPWM-nextDelta;
     
+        // compute MotB PWM
+/*        if (((nextDelta > 0) && (nextPWM > (INT_MAX - nextDelta))) ||
+           ((nextDelta < 0) && (nextPWM < (INT_MIN - nextDelta))))
+            motorBPWM[motorDriveIndex] = INT_MAX;
+        else
+            motorBPWM[motorDriveIndex] = nextPWM + nextDelta;*/
+        motorBPWM[motorDriveIndex] = nextPWM+nextDelta;
+
+        currentMotAPWM = ((float)motorAPWM[motorDriveIndex])/INT_MAX;
+        currentMotBPWM = ((float)motorBPWM[motorDriveIndex])/INT_MAX;
+        TFC_SetMotorPWM(currentMotAPWM,currentMotBPWM);
+    }
 }
 
 float Motors::getAverageMotCurrent(char mot)
@@ -92,6 +189,22 @@
     return ((float)sum)/MOTSAMPLECOUNT/1240.9;
 }
 
+float Motors::getMotCurrent(char mot)
+{
+    if(mot == 'A')
+        return (float)motorACurrent[motorCurrentIndex]/1240.9;
+    else
+        return (float)motorBCurrent[motorCurrentIndex]/1240.9;
+}
+
+float Motors::getMotPWM(char mot)
+{
+    if(mot == 'A')
+        return currentMotAPWM;
+    else
+        return currentMotBPWM;
+}
+
 float Motors::getAverageBatteryVoltage()
 {
     uint8_t i;
@@ -104,9 +217,17 @@
     return ((float)sum)/BATSAMPLECOUNT/217.7;
 }
 
-
 void Motors::setFixedPWMValue(float pwm)
 {
-    currentPWM = pwm;
-    TFC_SetMotorPWM(currentPWM,currentPWM);
+    nextPWM = (int16_t)(pwm*INT_MAX);
 }
+
+void Motors::setSpeedTargetValue(float speed)
+{
+    speedTarget = (int32_t)(speed*1e6);
+}
+
+float Motors::getSpeedError()
+{
+    return ((float)speederror1000000)/1e6;
+}