MotorControl.cpp

00001 #include "TFC.h"
00002 #include "MotorControl.h"
00003 //#include "limits.h"
00004 
00005 #define INT_MAX 32767
00006 #define INT_MIN -32768
00007 
00008 Motors::Motors()
00009 {
00010     uint8_t i;
00011     
00012     PWMRegulatedMode = false;
00013     
00014     motorCurrentIndex = 0;
00015     batVoltageIndex = 0;
00016     
00017     for(i=0;i<MOTSAMPLECOUNT;i++)
00018     {
00019         motorACurrent[i] = 0;
00020         motorBCurrent[i] = 0;
00021         motorAPWM[i] = 0;
00022         motorAPWM[i] = 0;
00023     }
00024 }
00025 
00026 void Motors::start()
00027 {
00028     TFC_HBRIDGE_ENABLE;
00029 }
00030 
00031 void Motors::stop()
00032 {
00033     TFC_HBRIDGE_DISABLE;
00034 }
00035 
00036 void Motors::saveBatteryVoltageMeasure(uint16_t ADCresult)
00037 {
00038     batVoltageIndex++;
00039     if(batVoltageIndex >= BATSAMPLECOUNT)
00040         batVoltageIndex = 0;
00041     
00042     batteryVoltage[batVoltageIndex] = ADCresult;
00043 }
00044 
00045 void Motors::saveMotorCurrentMeasure(uint16_t MotA_ADCresult, uint16_t MotB_ADCresult)
00046 {
00047 //    uint8_t i,index;
00048 //    int16_t sum = 0;
00049     
00050     motorCurrentIndex++;
00051     if(motorCurrentIndex >= MOTSAMPLECOUNT)
00052         motorCurrentIndex = 0;
00053     
00054     // current is always measured as positive. Need to check direction of motor drive to know the sign
00055     // current value stored is directly value in volt at input of ADC, not real current in Amp
00056     if(currentMotAPWM >= 0)
00057         motorACurrent[motorCurrentIndex] = (float)MotA_ADCresult/1240.9090; // *3.3/4095
00058     else
00059         motorACurrent[motorCurrentIndex] = -(float)MotA_ADCresult/1240.9090; // *3.3/4095
00060     
00061     if(currentMotBPWM >= 0)
00062         motorBCurrent[motorCurrentIndex] = (float)MotB_ADCresult/1240.9090; // *3.3/4095
00063     else
00064         motorBCurrent[motorCurrentIndex] = -(float)MotB_ADCresult/1240.9090; // *3.3/4095
00065     
00066     motorAPWM[motorCurrentIndex] = currentMotAPWM;
00067     motorBPWM[motorCurrentIndex] = currentMotBPWM;
00068 /*    index = motorCurrentIndex;
00069     for(i=0;i<10;i++)
00070     {
00071         sum += motorACurrent[index] - motorBCurrent[index];
00072         if(index == 0)
00073             index = MOTSAMPLECOUNT;
00074         index--;
00075     }
00076     
00077     torqueDiffAvg = sum;*/
00078 }
00079 
00080 float Motors::getAverageSpeed()
00081 {
00082     float currentA = 0, currentB = 0, meanVoltageA = 0, meanVoltageB = 0;
00083     uint8_t index = motorCurrentIndex;
00084     uint8_t i,avg = 10;
00085     
00086     for(i=0;i<avg;i++)
00087     {
00088         currentA += motorACurrent[index];
00089         meanVoltageA += ((float)motorAPWM[index])*(float)batteryVoltage[batVoltageIndex]; // *3.3/4095*5.7
00090         currentB += motorBCurrent[index];
00091         meanVoltageB += ((float)motorBPWM[index])*(float)batteryVoltage[batVoltageIndex]; // *3.3/4095*5.7
00092         if(index == 0)
00093             index = MOTSAMPLECOUNT;
00094         index--;
00095     }
00096 
00097     // freeSpeed = 9.8004*meanVoltage-7.0023
00098     // speed = (freeSpeed - (2.255*meanVoltage+10.51)*current)
00099     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
00100 }
00101 
00102 float Motors::getWheelRPS(char mot)
00103 {
00104     float current,meanVoltage;
00105     uint8_t index = motorCurrentIndex;
00106         
00107     // TODO: optimise computation time
00108     if(mot == 'A')
00109     {
00110         meanVoltage = ((float)motorAPWM[index])/INT_MAX*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
00111         return ((9.829651257*meanVoltage)-(19.33709258*motorACurrent[index])-6.435176945);
00112     }
00113     else
00114     {
00115         meanVoltage = ((float)motorBPWM[index])/INT_MAX*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
00116         return ((9.829651257*meanVoltage)-(19.33709258*motorBCurrent[index])-6.435176945);
00117     }
00118  }
00119 
00120 float Motors::getWheelSpeed(char mot)
00121 {
00122     return getWheelRPS(mot)*0.15707963267; // in m/s (pi*0.05)
00123 }
00124 
00125 float Motors::getCarSpeed()
00126 {
00127     return (getWheelSpeed('A')+getWheelSpeed('B'))/2.0;
00128 }
00129 
00130 void Motors::setFixedPWMMode(void)
00131 {
00132     PWMRegulatedMode = false;
00133 }
00134 
00135 void Motors::setRegulatedPWMMode(void)
00136 {
00137     PWMRegulatedMode = true;
00138 }
00139 
00140 void Motors::processTasks()
00141 {
00142     int16_t nextDelta;
00143     int32_t torqueDiffAvg,PWMSumAvg,motorSumAvg,meanVoltage,nextp;
00144     uint8_t i,index;
00145 
00146     if(motorDriveIndex != ((motorCurrentIndex+1)%MOTSAMPLECOUNT)) // process data only if a new data is available
00147     {
00148         motorDriveIndex = motorCurrentIndex+1; // current is saved with the PWM, compute the next PWM now
00149         
00150         torqueDiffAvg = 0;
00151         PWMSumAvg = 0;
00152         motorSumAvg = 0;
00153         index = motorDriveIndex;
00154         // TODO: verify indexes
00155         for(i=0;i<10;i++)
00156         {
00157             // compute average values over 10 last measures
00158             torqueDiffAvg += motorACurrent[index] - motorBCurrent[index];
00159             PWMSumAvg += motorAPWM[index]+motorBPWM[index];
00160             motorSumAvg += motorACurrent[index]+motorBCurrent[index];
00161             if(index == 0)
00162                 index = MOTSAMPLECOUNT;
00163             index--;
00164         }
00165         
00166         if(PWMRegulatedMode)
00167         {
00168 /*
00169             a0 = 1000000;
00170             a1 = 93;        // 10/a0/9.8004/pi/0.05*2*INT_MAX/3.3*4095/5.7;
00171             a2 = 1099919;   // a0*7.0023*pi*0.05;
00172             a3 = 9996209;   // 10/a0/2.255/pi/0.05*2*INT_MAX/3.3*4095/5.7/3.3*4095*2*10;
00173             a4 = 67;        // a0*10.51*pi*0.05*3.3/4095/2/10;
00174 */
00175             // compute current speed to get error when compared to target
00176             meanVoltage = PWMSumAvg*batteryVoltage[batVoltageIndex];
00177 //          speederror1000000 = meanVoltage/a1-a2-(meanVoltage/a3+a4)*motorSumAvg - speedTarget*a0;
00178             speederror1000000 = meanVoltage/93-1099919-(meanVoltage/9996209+67)*motorSumAvg - speedTarget;
00179         
00180             /*nextp = speederror1000000/-20;
00181             if(nextp > INT_MAX)
00182                 nextPWM = INT_MAX;
00183             else if (nextp < INT_MIN)
00184                 nextPWM = INT_MIN;
00185             else
00186                 nextPWM = (int16_t)nextp;
00187             nextDelta = 0;*/
00188         }
00189         else
00190         {
00191             nextPWM = fixedPWM; // 14745
00192         }
00193         
00194         nextDelta = (int16_t)(torqueDiffAvg/2); // delta to apply is about half the sum (0x7fff/(10*1240.9)/5)
00195         nextDelta = 0;
00196         
00197         // compute MotA PWM
00198 /*        if ((nextDelta > 0 && nextPWM < INT_MIN + nextDelta) ||
00199            (nextDelta < 0 && nextPWM > INT_MAX + nextDelta))
00200             currentMotAPWM = INT_MIN;
00201         else
00202             currentMotAPWM = nextPWM - nextDelta;*/
00203         currentMotAPWM = nextPWM-nextDelta;
00204     
00205         // compute MotB PWM
00206 /*        if (((nextDelta > 0) && (nextPWM > (INT_MAX - nextDelta))) ||
00207            ((nextDelta < 0) && (nextPWM < (INT_MIN - nextDelta))))
00208             currentMotBPWM = INT_MAX;
00209         else
00210             currentMotBPWM = nextPWM + nextDelta;*/
00211         currentMotBPWM = nextPWM+nextDelta;
00212 
00213         TFC_SetMotorPWM(getMotPWM('A'),getMotPWM('B'));
00214     }
00215 }
00216 
00217 float Motors::getAverageMotCurrent(char mot)
00218 {
00219     uint8_t i;
00220     float sum = 0;
00221     
00222     if(mot == 'A')
00223     {
00224         for(i=0;i<MOTSAMPLECOUNT;i++)
00225         {
00226             sum += motorACurrent[i];
00227         }
00228     }
00229     else
00230     {
00231         for(i=0;i<MOTSAMPLECOUNT;i++)
00232         {
00233             sum += motorBCurrent[i];
00234         }
00235     }
00236     
00237     return sum/MOTSAMPLECOUNT;
00238 }
00239 
00240 float Motors::getMotCurrent(char mot)
00241 {
00242     if(mot == 'A')
00243         return motorACurrent[motorCurrentIndex];
00244     else
00245         return motorBCurrent[motorCurrentIndex];
00246 }
00247 
00248 float Motors::getMotPWM(char mot)
00249 {
00250     if(mot == 'A')
00251         return ((float)currentMotAPWM)/INT_MAX;
00252     else
00253         return ((float)currentMotBPWM)/INT_MAX;
00254 }
00255 
00256 float Motors::getAverageBatteryVoltage()
00257 {
00258     uint8_t i;
00259     uint32_t sum = 0;
00260     
00261     for(i=0;i<BATSAMPLECOUNT;i++)
00262     {
00263         sum += batteryVoltage[i];
00264     }
00265     return ((float)sum)/BATSAMPLECOUNT/217.7;
00266 }
00267 
00268 void Motors::setFixedPWMValue(float pwm)
00269 {
00270     fixedPWM = (int16_t)(pwm*INT_MAX);
00271 }
00272 
00273 void Motors::setSpeedTargetValue(float speed)
00274 {
00275     speedTarget = (int32_t)(speed*1e6);
00276 }
00277 
00278 float Motors::getSpeedError()
00279 {
00280     return ((float)speederror1000000)/1e6;
00281 }