Pascal Sandrez / Mbed 2 deprecated NXPCUPcar

NXP cup car code made by Pascal. "stable" version apr. 2016

Dependencies:   mbed FRDM-TFC PIDcontroller FreescaleIAP Vision MMA8451Q MotorControl

main.cpp@3:c21068661635, 2019-02-20 (annotated)

Committer:
Clarkk
Date:
Wed Feb 20 22:22:48 2019 +0000
Revision:
3:c21068661635
Parent:
2:eb79d6e3f701 
Stable version with speed control but without speed regulation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Clarkk 0:5e090fec6099 1 #include "mbed.h"
Clarkk 0:5e090fec6099 2 #include "TFC.h"
Clarkk 0:5e090fec6099 3 #include "MotorControl.h"
Clarkk 0:5e090fec6099 4 #include "Vision.h"
Clarkk 0:5e090fec6099 5 #include "MMA8451Q.h"
Clarkk 1:5402c62ad487 6 #include "PIDcontroller.h"
Clarkk 0:5e090fec6099 7
Clarkk 0:5e090fec6099 8 Serial pc(USBTX,USBRX);
Clarkk 0:5e090fec6099 9 Serial debug(PTD3,PTD2);
Clarkk 0:5e090fec6099 10 Ticker convTicker,batLevelTicker;
Clarkk 0:5e090fec6099 11 Timeout camExpoTimeout;
Clarkk 0:5e090fec6099 12 Timer t;
Clarkk 3:c21068661635 13 Ticker test;
Clarkk 0:5e090fec6099 14 Motors motors;
Clarkk 0:5e090fec6099 15 Vision vision;
Clarkk 1:5402c62ad487 16 PID steeringPID;
Clarkk 3:c21068661635 17 PID speedPID;
Clarkk 0:5e090fec6099 18
Clarkk 0:5e090fec6099 19 uint32_t convCounter = 0;
Clarkk 0:5e090fec6099 20
Clarkk 0:5e090fec6099 21 float getSteeringValue(float pixelIndex)
Clarkk 0:5e090fec6099 22 {
Clarkk 1:5402c62ad487 23 // return steering angle to send to servo function of pixel target
Clarkk 0:5e090fec6099 24 return ((-5e-6*pixelIndex+0.0009)*pixelIndex-0.0248)*pixelIndex-0.9129;
Clarkk 0:5e090fec6099 25 //return ((-5e-6*pixelIndex+0.0009)*pixelIndex-0.0224)*pixelIndex-0.9218;
Clarkk 0:5e090fec6099 26 }
Clarkk 0:5e090fec6099 27
Clarkk 0:5e090fec6099 28 float flt[4]={0,0,0,0};
Clarkk 0:5e090fec6099 29 float steeringFilter(float x)
Clarkk 0:5e090fec6099 30 {
Clarkk 0:5e090fec6099 31 return 2.0*x;
Clarkk 0:5e090fec6099 32
Clarkk 0:5e090fec6099 33 /* uint8_t i;
Clarkk 0:5e090fec6099 34
Clarkk 0:5e090fec6099 35 for(i=3;i>0;i--)
Clarkk 0:5e090fec6099 36 {
Clarkk 0:5e090fec6099 37 flt[i] = flt[i-1];
Clarkk 0:5e090fec6099 38 }
Clarkk 0:5e090fec6099 39 flt[0] = x;
Clarkk 0:5e090fec6099 40
Clarkk 0:5e090fec6099 41 return (flt[0]+flt[1]*0.75+flt[2]*0.5+flt[3]*0.25)/2.5;*/
Clarkk 0:5e090fec6099 42 }
Clarkk 0:5e090fec6099 43
Clarkk 0:5e090fec6099 44 void applySteeringCommand(float value)
Clarkk 0:5e090fec6099 45 {
Clarkk 0:5e090fec6099 46 float clamp = 0.55;
Clarkk 0:5e090fec6099 47 if(value > clamp)
Clarkk 0:5e090fec6099 48 value = clamp;
Clarkk 0:5e090fec6099 49 if(value < -clamp)
Clarkk 0:5e090fec6099 50 value = -clamp;
Clarkk 0:5e090fec6099 51 TFC_SetServo(0,value);
Clarkk 0:5e090fec6099 52 }
Clarkk 0:5e090fec6099 53
Clarkk 0:5e090fec6099 54 void stopCameraExposure()
Clarkk 0:5e090fec6099 55 {
Clarkk 0:5e090fec6099 56 }
Clarkk 0:5e090fec6099 57
Clarkk 2:eb79d6e3f701 58 void dataConvertionRoutine()
Clarkk 0:5e090fec6099 59 {
Clarkk 0:5e090fec6099 60 uint8_t mask;
Clarkk 0:5e090fec6099 61
Clarkk 0:5e090fec6099 62 // convertion of motor current is done at each tick
Clarkk 0:5e090fec6099 63 mask = ADC_MASK_CAPTURE_MOTOR_CURRENT;
Clarkk 0:5e090fec6099 64
Clarkk 0:5e090fec6099 65 // convertion of line scan is done each 5 ticks
Clarkk 0:5e090fec6099 66 if((convCounter % 5) == 0)
Clarkk 0:5e090fec6099 67 mask |= ADC_MASK_CAPTURE_LINE_SCAN;
Clarkk 0:5e090fec6099 68
Clarkk 0:5e090fec6099 69 // convertion of pot and battery is done each 5 ticks
Clarkk 0:5e090fec6099 70 if((convCounter % 5) == 2)
Clarkk 0:5e090fec6099 71 mask |= ADC_MASK_CAPTURE_POT_0 | ADC_MASK_CAPTURE_POT_1 | ADC_MASK_CAPTURE_BATTERY_LEVEL;
Clarkk 0:5e090fec6099 72
Clarkk 0:5e090fec6099 73 TFC_StartDataAcquisition(mask);
Clarkk 0:5e090fec6099 74
Clarkk 0:5e090fec6099 75 convCounter++;
Clarkk 3:c21068661635 76 if(convCounter >= 4000000000)
Clarkk 0:5e090fec6099 77 convCounter = 0;
Clarkk 0:5e090fec6099 78
Clarkk 0:5e090fec6099 79 //TFC_StartDataAcquisition(ADC_MASK_CAPTURE_ALL);
Clarkk 0:5e090fec6099 80 //camExpoTimeout.attach(&stopCameraExposure,camExposure);
Clarkk 0:5e090fec6099 81 }
Clarkk 0:5e090fec6099 82
Clarkk 0:5e090fec6099 83 void updateBatteryLevel()
Clarkk 0:5e090fec6099 84 {
Clarkk 0:5e090fec6099 85 float batteryVoltage;
Clarkk 0:5e090fec6099 86 batteryVoltage = motors.getAverageBatteryVoltage();
Clarkk 0:5e090fec6099 87 TFC_SetBatteryLED_Level(batteryVoltage*3.3-18.8);
Clarkk 0:5e090fec6099 88 }
Clarkk 0:5e090fec6099 89
Clarkk 0:5e090fec6099 90 void saveADCMeas()
Clarkk 0:5e090fec6099 91 {
Clarkk 0:5e090fec6099 92 //TFC_BAT_LED2_TOGGLE;
Clarkk 0:5e090fec6099 93 if(TFC_MotorCurrentReady)
Clarkk 0:5e090fec6099 94 {
Clarkk 0:5e090fec6099 95 TFC_MotorCurrentReady = 0;
Clarkk 0:5e090fec6099 96 motors.saveMotorCurrentMeasure(TFC_ReadMotACurrentRaw(),TFC_ReadMotBCurrentRaw());
Clarkk 0:5e090fec6099 97 }
Clarkk 0:5e090fec6099 98 if(TFC_LineScanImageReady)
Clarkk 0:5e090fec6099 99 {
Clarkk 0:5e090fec6099 100 TFC_LineScanImageReady = 0; // clear counter to be able to detect next frame
Clarkk 0:5e090fec6099 101 vision.saveData((uint16_t*)TFC_LineScanImage0);
Clarkk 0:5e090fec6099 102 }
Clarkk 0:5e090fec6099 103 if(TFC_BatteryVoltageReady)
Clarkk 0:5e090fec6099 104 {
Clarkk 0:5e090fec6099 105 TFC_BatteryVoltageReady = 0;
Clarkk 0:5e090fec6099 106 motors.saveBatteryVoltageMeasure(TFC_ReadBatteryVoltageRaw());
Clarkk 0:5e090fec6099 107 }
Clarkk 0:5e090fec6099 108 }
Clarkk 0:5e090fec6099 109
Clarkk 3:c21068661635 110 void printCarSpeed(Serial& output, Motors motors, uint32_t index)
Clarkk 3:c21068661635 111 {
Clarkk 3:c21068661635 112 output.printf("%d %f %f\r\n",index,motors.getCarSpeed(),motors.getSpeedError());
Clarkk 3:c21068661635 113 }
Clarkk 3:c21068661635 114
Clarkk 3:c21068661635 115 void printAll(Serial& output, Motors motors, float PWM, float steering, uint32_t index)
Clarkk 3:c21068661635 116 {
Clarkk 3:c21068661635 117 output.printf("%d %f %f %f\n",index,motors.getCarSpeed(), PWM ,steering);
Clarkk 3:c21068661635 118 }
Clarkk 3:c21068661635 119
Clarkk 3:c21068661635 120 void printEdges(Serial& output, struct lineScanData lineData)
Clarkk 3:c21068661635 121 {
Clarkk 3:c21068661635 122 uint8_t i;
Clarkk 3:c21068661635 123
Clarkk 3:c21068661635 124 output.printf("[");
Clarkk 3:c21068661635 125 for(i=0;i<lineData.leftEdgeIndex;i++) output.printf(" ");
Clarkk 3:c21068661635 126 output.printf("^");
Clarkk 3:c21068661635 127 for(i=lineData.leftEdgeIndex;i<lineData.mostWhiteIndex;i++) output.printf(" ");
Clarkk 3:c21068661635 128 output.printf("|");
Clarkk 3:c21068661635 129 for(i=lineData.mostWhiteIndex;i<lineData.rightEdgeIndex;i++) output.printf(" ");
Clarkk 3:c21068661635 130 output.printf("^");
Clarkk 3:c21068661635 131 for(i=lineData.rightEdgeIndex;i<128;i++) output.printf(" ");
Clarkk 3:c21068661635 132 output.printf("]\r\n");
Clarkk 3:c21068661635 133 }
Clarkk 3:c21068661635 134
Clarkk 3:c21068661635 135 void printLine(Serial& output)
Clarkk 3:c21068661635 136 {
Clarkk 3:c21068661635 137 uint8_t i;
Clarkk 3:c21068661635 138
Clarkk 3:c21068661635 139 for(i=0;i<128;i++)
Clarkk 3:c21068661635 140 {
Clarkk 3:c21068661635 141 output.printf("%d ",TFC_LineScanImage0[i]);
Clarkk 3:c21068661635 142 }
Clarkk 3:c21068661635 143 output.printf("\r\n");
Clarkk 3:c21068661635 144 }
Clarkk 3:c21068661635 145
Clarkk 3:c21068661635 146 bool dir = false;
Clarkk 3:c21068661635 147 void invertMot()
Clarkk 3:c21068661635 148 {
Clarkk 3:c21068661635 149 if(dir)
Clarkk 3:c21068661635 150 motors.setFixedPWMValue(1.0);
Clarkk 3:c21068661635 151 else
Clarkk 3:c21068661635 152 motors.setFixedPWMValue(-1.0);
Clarkk 3:c21068661635 153 dir = !dir;
Clarkk 3:c21068661635 154 }
Clarkk 3:c21068661635 155
Clarkk 0:5e090fec6099 156 int main()
Clarkk 0:5e090fec6099 157 {
Clarkk 0:5e090fec6099 158 uint8_t r,l;
Clarkk 2:eb79d6e3f701 159 float motorPWM,steering,steeringFiltered;
Clarkk 0:5e090fec6099 160 float pixelTarget;
Clarkk 0:5e090fec6099 161 struct lineScanData lineData;
Clarkk 0:5e090fec6099 162 uint32_t lastcount = 0;
Clarkk 0:5e090fec6099 163 bool debugon = false;
Clarkk 0:5e090fec6099 164
Clarkk 0:5e090fec6099 165 //MMA8451Q acc(PTE25, PTE24, (0x1d<<1));
Clarkk 0:5e090fec6099 166
Clarkk 0:5e090fec6099 167 TFC_Init();
Clarkk 0:5e090fec6099 168
Clarkk 0:5e090fec6099 169 pc.baud(115200);
Clarkk 0:5e090fec6099 170 debug.baud(115200);
Clarkk 0:5e090fec6099 171
Clarkk 3:c21068661635 172 // test.attach(&invertMot,2);
Clarkk 3:c21068661635 173
Clarkk 2:eb79d6e3f701 174 // run convertion routine each 2ms
Clarkk 2:eb79d6e3f701 175 convTicker.attach_us(&dataConvertionRoutine, 2000);
Clarkk 2:eb79d6e3f701 176 // record data when convertion done
Clarkk 0:5e090fec6099 177 TFC_ConvertionReadyCallback.attach(&saveADCMeas);
Clarkk 0:5e090fec6099 178
Clarkk 0:5e090fec6099 179 batLevelTicker.attach(&updateBatteryLevel,0.2);
Clarkk 0:5e090fec6099 180
Clarkk 2:eb79d6e3f701 181 motors.setFixedPWMMode();
Clarkk 0:5e090fec6099 182 motors.setSpeedTargetValue(2.0);
Clarkk 3:c21068661635 183 motors.setFixedPWMValue(0.45);
Clarkk 1:5402c62ad487 184
Clarkk 1:5402c62ad487 185 steeringPID.setReference(0);
Clarkk 3:c21068661635 186 steeringPID.setCoefficients(1.2,0,0);
Clarkk 1:5402c62ad487 187 steeringPID.setOutputLimits(-0.6,0.6);
Clarkk 3:c21068661635 188
Clarkk 3:c21068661635 189 speedPID.setReference(1.0);
Clarkk 3:c21068661635 190 speedPID.setCoefficients(0.6,3,0);
Clarkk 3:c21068661635 191 speedPID.setOutputLimits(-1.0,1.0);
Clarkk 0:5e090fec6099 192
Clarkk 0:5e090fec6099 193 while(1)
Clarkk 0:5e090fec6099 194 {
Clarkk 0:5e090fec6099 195 if(TFC_ReadPushButton(0))
Clarkk 0:5e090fec6099 196 {
Clarkk 0:5e090fec6099 197 motors.stop();
Clarkk 0:5e090fec6099 198 debugon = false;
Clarkk 0:5e090fec6099 199 }
Clarkk 0:5e090fec6099 200
Clarkk 0:5e090fec6099 201 if(TFC_ReadPushButton(1))
Clarkk 0:5e090fec6099 202 {
Clarkk 0:5e090fec6099 203 motors.start();
Clarkk 0:5e090fec6099 204 debugon = true;
Clarkk 0:5e090fec6099 205 }
Clarkk 0:5e090fec6099 206
Clarkk 0:5e090fec6099 207 vision.processTasks();
Clarkk 0:5e090fec6099 208 lineData = vision.getRoadData();
Clarkk 0:5e090fec6099 209 r = lineData.rightEdgeIndex;
Clarkk 0:5e090fec6099 210 //if(r>=115)
Clarkk 0:5e090fec6099 211 // r = 128;
Clarkk 0:5e090fec6099 212 l = lineData.leftEdgeIndex;
Clarkk 0:5e090fec6099 213 //if(l<=13)
Clarkk 0:5e090fec6099 214 // l = 0;
Clarkk 0:5e090fec6099 215 pixelTarget = (r+l)/2.0;
Clarkk 1:5402c62ad487 216
Clarkk 0:5e090fec6099 217 steering = getSteeringValue(pixelTarget);
Clarkk 1:5402c62ad487 218
Clarkk 1:5402c62ad487 219 // previous
Clarkk 1:5402c62ad487 220 //applySteeringCommand(steeringFilter(steering));
Clarkk 1:5402c62ad487 221
Clarkk 1:5402c62ad487 222
Clarkk 1:5402c62ad487 223 // new
Clarkk 3:c21068661635 224 steeringFiltered = steeringPID.processNewValue(-steering);
Clarkk 2:eb79d6e3f701 225 applySteeringCommand(steeringFiltered);
Clarkk 3:c21068661635 226
Clarkk 3:c21068661635 227 // drive proportional to steering
Clarkk 3:c21068661635 228 motorPWM = 0.5-abs(steeringFiltered)/2.0;
Clarkk 3:c21068661635 229 motors.setFixedPWMValue(motorPWM);
Clarkk 3:c21068661635 230
Clarkk 3:c21068661635 231 // speed is regulated
Clarkk 3:c21068661635 232 //speedPID.setReference(1.8-abs(steeringFiltered)*2.5); // set speed to 2m/s max and decrease to 0.5m/s if steering is at max
Clarkk 3:c21068661635 233 //motorPWM = speedPID.processNewValue(motors.getCarSpeed());
Clarkk 3:c21068661635 234 //motors.setFixedPWMValue(motorPWM);
Clarkk 0:5e090fec6099 235
Clarkk 0:5e090fec6099 236 motors.processTasks();
Clarkk 0:5e090fec6099 237 //motorPWM = TFC_ReadPot(0);
Clarkk 0:5e090fec6099 238 //motorPWM = (TFC_GetDIP_Switch()*0.05+0.25);
Clarkk 0:5e090fec6099 239 //motors.setFixedPWMValue(motorPWM);
Clarkk 0:5e090fec6099 240
Clarkk 0:5e090fec6099 241 /*if(TFC_LineScanImageReady)
Clarkk 0:5e090fec6099 242 {
Clarkk 0:5e090fec6099 243 TFC_LineScanImageReady = 0; // clear counter to be able to detect next frame
Clarkk 0:5e090fec6099 244
Clarkk 0:5e090fec6099 245 // calculation time measure
Clarkk 0:5e090fec6099 246 t.reset();
Clarkk 0:5e090fec6099 247 t.start();
Clarkk 0:5e090fec6099 248
Clarkk 0:5e090fec6099 249 // stop calculation time measurement
Clarkk 0:5e090fec6099 250 t.stop();
Clarkk 0:5e090fec6099 251
Clarkk 0:5e090fec6099 252 //pc.printf("Bat: %fV, PWM:%f, MA:%frps, MB:%frps\r",motors.getAverageBatteryVoltage(),motorPWM,motors.getWheelRPS('A'),motors.getWheelRPS('B'));
Clarkk 0:5e090fec6099 253 //pc.printf("Bat: %fV, PWM:%f, MA:%fV, MB:%fV\r",motors.getAverageBatteryVoltage(),motorPWM,motors.getAverageMotCurrent('A'),motors.getAverageMotCurrent('B'));
Clarkk 0:5e090fec6099 254 //pc.printf("Bat: %fV, PWM:%f, MotA:%f, MotB:%f, speedA:%f, speedB:%f\r",TFC_ReadBatteryVoltage(),motorPWM,TFC_ReadMotACurrent(),TFC_ReadMotBCurrent(),getMotSpeed('A',motorPWM),getMotSpeed('B',motorPWM));
Clarkk 0:5e090fec6099 255 //pc.printf("Bat: %fV, d=%f, time:%dus max light:%d @ %d \r",TFC_ReadBatteryVoltage(),steering,t.read_us(),lineData.maxLightValue,lineData.mostWhiteIndex);
Clarkk 0:5e090fec6099 256 }*/
Clarkk 0:5e090fec6099 257
Clarkk 1:5402c62ad487 258 if((convCounter % 100) == 0) // each 0.2 second
Clarkk 0:5e090fec6099 259 {
Clarkk 3:c21068661635 260 //printEdges(pc);
Clarkk 3:c21068661635 261 //printLine(pc);
Clarkk 0:5e090fec6099 262
Clarkk 3:c21068661635 263 //pc.printf("%d %d => %f\r\n",l,r,steering);
Clarkk 3:c21068661635 264 //pc.printf("%f %f\r",motors.getAverageMotCurrent('A'),motors.getAverageMotCurrent('B'));
Clarkk 1:5402c62ad487 265 }
Clarkk 0:5e090fec6099 266
Clarkk 0:5e090fec6099 267 //pc.printf("%d %f %f %f \r",lastcount,acc.getAccX(),acc.getAccY(),acc.getAccZ());
Clarkk 0:5e090fec6099 268 if(debugon && (convCounter != lastcount))
Clarkk 0:5e090fec6099 269 {
Clarkk 0:5e090fec6099 270 lastcount = convCounter;
Clarkk 3:c21068661635 271 //printAll(debug, motors, motorPWM, steeringFiltered, lastcount);
Clarkk 3:c21068661635 272 //printCarSpeed(debug, motors, lastcount);
Clarkk 3:c21068661635 273 //pc.printf("%d %f %f %f\n",lastcount,motors.getMotPWM('A'),motors.getMotCurrent('A'),motors.getWheelSpeed('A'));
Clarkk 3:c21068661635 274 //pc.printf("%f\r",speedPID.getIntegralTerm());
Clarkk 3:c21068661635 275 //debug.printf("%d %f %f %f\n",lastcount,motors.getMotCurrent('A'),motors.getMotCurrent('B'),motors.getCarSpeed(),);
Clarkk 0:5e090fec6099 276 //debug.printf("%d %f %f %f\n",lastcount,motors.getMotPWM('A'),motors.getMotPWM('B'),steering);
Clarkk 0:5e090fec6099 277 //debug.printf("%d %f %f %f\n",lastcount,acc.getAccX(),acc.getAccY(),steering);
Clarkk 0:5e090fec6099 278 }
Clarkk 0:5e090fec6099 279 }
Clarkk 0:5e090fec6099 280 }