Pascal Sandrez / MotorControl

Custom version for NXP cup car

Dependents:   NXPCUPcar

MotorControl.cpp@2:0d5c994d8135, 2017-06-01 (annotated)

Committer:
Clarkk
Date:
Thu Jun 01 13:16:50 2017 +0000
Revision:
2:0d5c994d8135
Parent:
0:a1bb4583940a
Child:
3:170c22171ec2
Few changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Clarkk 0:a1bb4583940a 1 #include "TFC.h"
Clarkk 0:a1bb4583940a 2 #include "MotorControl.h"
Clarkk 2:0d5c994d8135 3 //#include "limits.h"
Clarkk 2:0d5c994d8135 4
Clarkk 2:0d5c994d8135 5 #define INT_MAX 32767
Clarkk 2:0d5c994d8135 6 #define INT_MIN -32768
Clarkk 0:a1bb4583940a 7
Clarkk 0:a1bb4583940a 8 Motors::Motors()
Clarkk 0:a1bb4583940a 9 {
Clarkk 2:0d5c994d8135 10 uint8_t i;
Clarkk 2:0d5c994d8135 11
Clarkk 0:a1bb4583940a 12 motorCurrentIndex = 0;
Clarkk 0:a1bb4583940a 13 batVoltageIndex = 0;
Clarkk 0:a1bb4583940a 14
Clarkk 2:0d5c994d8135 15 for(i=0;i<MOTSAMPLECOUNT;i++)
Clarkk 2:0d5c994d8135 16 {
Clarkk 2:0d5c994d8135 17 motorACurrent[i] = 0;
Clarkk 2:0d5c994d8135 18 motorBCurrent[i] = 0;
Clarkk 2:0d5c994d8135 19 motorAPWM[i] = 0;
Clarkk 2:0d5c994d8135 20 motorAPWM[i] = 0;
Clarkk 2:0d5c994d8135 21 }
Clarkk 0:a1bb4583940a 22 }
Clarkk 0:a1bb4583940a 23
Clarkk 0:a1bb4583940a 24 void Motors::start()
Clarkk 0:a1bb4583940a 25 {
Clarkk 0:a1bb4583940a 26 TFC_HBRIDGE_ENABLE;
Clarkk 0:a1bb4583940a 27 }
Clarkk 0:a1bb4583940a 28
Clarkk 0:a1bb4583940a 29 void Motors::stop()
Clarkk 0:a1bb4583940a 30 {
Clarkk 0:a1bb4583940a 31 TFC_HBRIDGE_DISABLE;
Clarkk 0:a1bb4583940a 32 }
Clarkk 0:a1bb4583940a 33
Clarkk 0:a1bb4583940a 34 void Motors::saveBatteryVoltageMeasure(uint16_t ADCresult)
Clarkk 0:a1bb4583940a 35 {
Clarkk 0:a1bb4583940a 36 batVoltageIndex++;
Clarkk 0:a1bb4583940a 37 if(batVoltageIndex >= BATSAMPLECOUNT)
Clarkk 0:a1bb4583940a 38 batVoltageIndex = 0;
Clarkk 0:a1bb4583940a 39
Clarkk 0:a1bb4583940a 40 batteryVoltage[batVoltageIndex] = ADCresult;
Clarkk 0:a1bb4583940a 41 }
Clarkk 0:a1bb4583940a 42
Clarkk 0:a1bb4583940a 43 void Motors::saveMotorCurrentMeasure(uint16_t MotA_ADCresult, uint16_t MotB_ADCresult)
Clarkk 0:a1bb4583940a 44 {
Clarkk 2:0d5c994d8135 45 // uint8_t i,index;
Clarkk 2:0d5c994d8135 46 // int16_t sum = 0;
Clarkk 2:0d5c994d8135 47
Clarkk 0:a1bb4583940a 48 motorCurrentIndex++;
Clarkk 0:a1bb4583940a 49 if(motorCurrentIndex >= MOTSAMPLECOUNT)
Clarkk 0:a1bb4583940a 50 motorCurrentIndex = 0;
Clarkk 0:a1bb4583940a 51
Clarkk 0:a1bb4583940a 52 motorACurrent[motorCurrentIndex] = MotA_ADCresult;
Clarkk 0:a1bb4583940a 53 motorBCurrent[motorCurrentIndex] = MotB_ADCresult;
Clarkk 2:0d5c994d8135 54
Clarkk 2:0d5c994d8135 55 /* index = motorCurrentIndex;
Clarkk 2:0d5c994d8135 56 for(i=0;i<10;i++)
Clarkk 2:0d5c994d8135 57 {
Clarkk 2:0d5c994d8135 58 sum += motorACurrent[index] - motorBCurrent[index];
Clarkk 2:0d5c994d8135 59 if(index == 0)
Clarkk 2:0d5c994d8135 60 index = MOTSAMPLECOUNT;
Clarkk 2:0d5c994d8135 61 index--;
Clarkk 2:0d5c994d8135 62 }
Clarkk 2:0d5c994d8135 63
Clarkk 2:0d5c994d8135 64 torqueDiffAvg = sum;*/
Clarkk 0:a1bb4583940a 65 }
Clarkk 0:a1bb4583940a 66
Clarkk 0:a1bb4583940a 67 float Motors::getWheelRPS(char mot)
Clarkk 0:a1bb4583940a 68 {
Clarkk 0:a1bb4583940a 69 float current,meanVoltage;
Clarkk 2:0d5c994d8135 70 uint8_t index = motorCurrentIndex;
Clarkk 0:a1bb4583940a 71
Clarkk 2:0d5c994d8135 72
Clarkk 2:0d5c994d8135 73 // TODO: optimise computation time
Clarkk 0:a1bb4583940a 74 if(mot == 'A')
Clarkk 2:0d5c994d8135 75 {
Clarkk 2:0d5c994d8135 76 current = (float)motorACurrent[index]/1240.9; // *3.3/4095
Clarkk 2:0d5c994d8135 77 meanVoltage = ((float)motorAPWM[index])/INT_MAX*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
Clarkk 2:0d5c994d8135 78 }
Clarkk 0:a1bb4583940a 79 else
Clarkk 2:0d5c994d8135 80 {
Clarkk 2:0d5c994d8135 81 current = (float)motorBCurrent[index]/1240.9; // *3.3/4095
Clarkk 2:0d5c994d8135 82 meanVoltage = ((float)motorBPWM[index])/INT_MAX*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
Clarkk 2:0d5c994d8135 83 }
Clarkk 0:a1bb4583940a 84
Clarkk 0:a1bb4583940a 85 // freeSpeed = 9.8004*meanVoltage-7.0023
Clarkk 0:a1bb4583940a 86 // speed = (freeSpeed - (2.255*meanVoltage+10.51)*current)
Clarkk 0:a1bb4583940a 87 return ((9.8004*meanVoltage-7.0023)-(2.255*meanVoltage+10.51)*current); // rps
Clarkk 0:a1bb4583940a 88 }
Clarkk 0:a1bb4583940a 89
Clarkk 0:a1bb4583940a 90 float Motors::getWheelSpeed(char mot)
Clarkk 0:a1bb4583940a 91 {
Clarkk 2:0d5c994d8135 92 return getWheelRPS(mot)*0.157; // in m/s (pi*0.05)
Clarkk 2:0d5c994d8135 93 }
Clarkk 2:0d5c994d8135 94
Clarkk 2:0d5c994d8135 95 float Motors::getCarSpeed()
Clarkk 2:0d5c994d8135 96 {
Clarkk 2:0d5c994d8135 97 return (getWheelSpeed('A')+getWheelSpeed('B'))/2.0;
Clarkk 0:a1bb4583940a 98 }
Clarkk 0:a1bb4583940a 99
Clarkk 0:a1bb4583940a 100 void Motors::processTasks()
Clarkk 0:a1bb4583940a 101 {
Clarkk 2:0d5c994d8135 102 int16_t nextDelta;
Clarkk 2:0d5c994d8135 103 int32_t torqueDiffAvg,PWMSumAvg,motorSumAvg,meanVoltage,nextp;
Clarkk 2:0d5c994d8135 104 uint8_t i,index;
Clarkk 2:0d5c994d8135 105
Clarkk 2:0d5c994d8135 106 if(motorDriveIndex != motorCurrentIndex)
Clarkk 2:0d5c994d8135 107 {
Clarkk 2:0d5c994d8135 108 motorDriveIndex = motorCurrentIndex;
Clarkk 2:0d5c994d8135 109
Clarkk 2:0d5c994d8135 110 torqueDiffAvg = 0;
Clarkk 2:0d5c994d8135 111 PWMSumAvg = 0;
Clarkk 2:0d5c994d8135 112 motorSumAvg = 0;
Clarkk 2:0d5c994d8135 113 index = motorDriveIndex;
Clarkk 2:0d5c994d8135 114 // TODO: verify indexes
Clarkk 2:0d5c994d8135 115 for(i=0;i<10;i++)
Clarkk 2:0d5c994d8135 116 {
Clarkk 2:0d5c994d8135 117 torqueDiffAvg += motorACurrent[index] - motorBCurrent[index];
Clarkk 2:0d5c994d8135 118 PWMSumAvg += motorAPWM[index]+motorBPWM[index];
Clarkk 2:0d5c994d8135 119 motorSumAvg += motorACurrent[index]+motorBCurrent[index];
Clarkk 2:0d5c994d8135 120 if(index == 0)
Clarkk 2:0d5c994d8135 121 index = MOTSAMPLECOUNT;
Clarkk 2:0d5c994d8135 122 index--;
Clarkk 2:0d5c994d8135 123 }
Clarkk 2:0d5c994d8135 124
Clarkk 2:0d5c994d8135 125 /* a0 = 1000000;
Clarkk 2:0d5c994d8135 126 a1 = 93; // 10/a0/9.8004/pi/0.05*2*INT_MAX/3.3*4095/5.7;
Clarkk 2:0d5c994d8135 127 a2 = 1099919; // a0*7.0023*pi*0.05;
Clarkk 2:0d5c994d8135 128 a3 = 9996209; // 10/a0/2.255/pi/0.05*2*INT_MAX/3.3*4095/5.7/3.3*4095*2*10;
Clarkk 2:0d5c994d8135 129 a4 = 67; // a0*10.51*pi*0.05*3.3/4095/2/10;
Clarkk 2:0d5c994d8135 130 */
Clarkk 2:0d5c994d8135 131 meanVoltage = PWMSumAvg*batteryVoltage[batVoltageIndex];
Clarkk 2:0d5c994d8135 132 // speederror1000000 = meanVoltage/a1-a2-(meanVoltage/a3+a4)*motorSumAvg - speedTarget*a0;
Clarkk 2:0d5c994d8135 133 speederror1000000 = meanVoltage/93-1099919-(meanVoltage/9996209+67)*motorSumAvg - speedTarget;
Clarkk 2:0d5c994d8135 134
Clarkk 2:0d5c994d8135 135 /*nextp = speederror1000000/-20;
Clarkk 2:0d5c994d8135 136 if(nextp > INT_MAX)
Clarkk 2:0d5c994d8135 137 nextPWM = INT_MAX;
Clarkk 2:0d5c994d8135 138 else if (nextp < INT_MIN)
Clarkk 2:0d5c994d8135 139 nextPWM = INT_MIN;
Clarkk 2:0d5c994d8135 140 else
Clarkk 2:0d5c994d8135 141 nextPWM = (int16_t)nextp;
Clarkk 2:0d5c994d8135 142 nextDelta = 0;*/
Clarkk 2:0d5c994d8135 143
Clarkk 2:0d5c994d8135 144 nextPWM = 14745;
Clarkk 2:0d5c994d8135 145 nextDelta = (int16_t)(torqueDiffAvg/2); // delta to apply is about half the sum (0x7fff/(10*1240.9)/5)
Clarkk 2:0d5c994d8135 146
Clarkk 2:0d5c994d8135 147 // compute MotA PWM
Clarkk 2:0d5c994d8135 148 /* if ((nextDelta > 0 && nextPWM < INT_MIN + nextDelta) ||
Clarkk 2:0d5c994d8135 149 (nextDelta < 0 && nextPWM > INT_MAX + nextDelta))
Clarkk 2:0d5c994d8135 150 motorAPWM[motorDriveIndex] = INT_MIN;
Clarkk 2:0d5c994d8135 151 else
Clarkk 2:0d5c994d8135 152 motorAPWM[motorDriveIndex] = nextPWM - nextDelta;*/
Clarkk 2:0d5c994d8135 153 motorAPWM[motorDriveIndex] = nextPWM-nextDelta;
Clarkk 0:a1bb4583940a 154
Clarkk 2:0d5c994d8135 155 // compute MotB PWM
Clarkk 2:0d5c994d8135 156 /* if (((nextDelta > 0) && (nextPWM > (INT_MAX - nextDelta))) ||
Clarkk 2:0d5c994d8135 157 ((nextDelta < 0) && (nextPWM < (INT_MIN - nextDelta))))
Clarkk 2:0d5c994d8135 158 motorBPWM[motorDriveIndex] = INT_MAX;
Clarkk 2:0d5c994d8135 159 else
Clarkk 2:0d5c994d8135 160 motorBPWM[motorDriveIndex] = nextPWM + nextDelta;*/
Clarkk 2:0d5c994d8135 161 motorBPWM[motorDriveIndex] = nextPWM+nextDelta;
Clarkk 2:0d5c994d8135 162
Clarkk 2:0d5c994d8135 163 currentMotAPWM = ((float)motorAPWM[motorDriveIndex])/INT_MAX;
Clarkk 2:0d5c994d8135 164 currentMotBPWM = ((float)motorBPWM[motorDriveIndex])/INT_MAX;
Clarkk 2:0d5c994d8135 165 TFC_SetMotorPWM(currentMotAPWM,currentMotBPWM);
Clarkk 2:0d5c994d8135 166 }
Clarkk 0:a1bb4583940a 167 }
Clarkk 0:a1bb4583940a 168
Clarkk 0:a1bb4583940a 169 float Motors::getAverageMotCurrent(char mot)
Clarkk 0:a1bb4583940a 170 {
Clarkk 0:a1bb4583940a 171 uint8_t i;
Clarkk 0:a1bb4583940a 172 uint32_t sum = 0;
Clarkk 0:a1bb4583940a 173
Clarkk 0:a1bb4583940a 174 if(mot == 'A')
Clarkk 0:a1bb4583940a 175 {
Clarkk 0:a1bb4583940a 176 for(i=0;i<MOTSAMPLECOUNT;i++)
Clarkk 0:a1bb4583940a 177 {
Clarkk 0:a1bb4583940a 178 sum += motorACurrent[i];
Clarkk 0:a1bb4583940a 179 }
Clarkk 0:a1bb4583940a 180 }
Clarkk 0:a1bb4583940a 181 else
Clarkk 0:a1bb4583940a 182 {
Clarkk 0:a1bb4583940a 183 for(i=0;i<MOTSAMPLECOUNT;i++)
Clarkk 0:a1bb4583940a 184 {
Clarkk 0:a1bb4583940a 185 sum += motorBCurrent[i];
Clarkk 0:a1bb4583940a 186 }
Clarkk 0:a1bb4583940a 187 }
Clarkk 0:a1bb4583940a 188
Clarkk 0:a1bb4583940a 189 return ((float)sum)/MOTSAMPLECOUNT/1240.9;
Clarkk 0:a1bb4583940a 190 }
Clarkk 0:a1bb4583940a 191
Clarkk 2:0d5c994d8135 192 float Motors::getMotCurrent(char mot)
Clarkk 2:0d5c994d8135 193 {
Clarkk 2:0d5c994d8135 194 if(mot == 'A')
Clarkk 2:0d5c994d8135 195 return (float)motorACurrent[motorCurrentIndex]/1240.9;
Clarkk 2:0d5c994d8135 196 else
Clarkk 2:0d5c994d8135 197 return (float)motorBCurrent[motorCurrentIndex]/1240.9;
Clarkk 2:0d5c994d8135 198 }
Clarkk 2:0d5c994d8135 199
Clarkk 2:0d5c994d8135 200 float Motors::getMotPWM(char mot)
Clarkk 2:0d5c994d8135 201 {
Clarkk 2:0d5c994d8135 202 if(mot == 'A')
Clarkk 2:0d5c994d8135 203 return currentMotAPWM;
Clarkk 2:0d5c994d8135 204 else
Clarkk 2:0d5c994d8135 205 return currentMotBPWM;
Clarkk 2:0d5c994d8135 206 }
Clarkk 2:0d5c994d8135 207
Clarkk 0:a1bb4583940a 208 float Motors::getAverageBatteryVoltage()
Clarkk 0:a1bb4583940a 209 {
Clarkk 0:a1bb4583940a 210 uint8_t i;
Clarkk 0:a1bb4583940a 211 uint32_t sum = 0;
Clarkk 0:a1bb4583940a 212
Clarkk 0:a1bb4583940a 213 for(i=0;i<BATSAMPLECOUNT;i++)
Clarkk 0:a1bb4583940a 214 {
Clarkk 0:a1bb4583940a 215 sum += batteryVoltage[i];
Clarkk 0:a1bb4583940a 216 }
Clarkk 0:a1bb4583940a 217 return ((float)sum)/BATSAMPLECOUNT/217.7;
Clarkk 0:a1bb4583940a 218 }
Clarkk 0:a1bb4583940a 219
Clarkk 0:a1bb4583940a 220 void Motors::setFixedPWMValue(float pwm)
Clarkk 0:a1bb4583940a 221 {
Clarkk 2:0d5c994d8135 222 nextPWM = (int16_t)(pwm*INT_MAX);
Clarkk 0:a1bb4583940a 223 }
Clarkk 2:0d5c994d8135 224
Clarkk 2:0d5c994d8135 225 void Motors::setSpeedTargetValue(float speed)
Clarkk 2:0d5c994d8135 226 {
Clarkk 2:0d5c994d8135 227 speedTarget = (int32_t)(speed*1e6);
Clarkk 2:0d5c994d8135 228 }
Clarkk 2:0d5c994d8135 229
Clarkk 2:0d5c994d8135 230 float Motors::getSpeedError()
Clarkk 2:0d5c994d8135 231 {
Clarkk 2:0d5c994d8135 232 return ((float)speederror1000000)/1e6;
Clarkk 2:0d5c994d8135 233 }