Pascal Sandrez / MotorControl

Custom version for NXP cup car

Dependents:   NXPCUPcar

MotorControl.cpp@3:170c22171ec2, 2017-07-05 (annotated)

Committer:
Clarkk
Date:
Wed Jul 05 20:29:37 2017 +0000
Revision:
3:170c22171ec2
Parent:
2:0d5c994d8135
Child:
4:66154ae82263
Added fixed PWM mode

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