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@4:85f05101889e, 2019-02-20 (annotated)

Committer:
Clarkk
Date:
Wed Feb 20 22:26:15 2019 +0000
Revision:
4:85f05101889e
Parent:
3:c21068661635 
Some attempt to do speed regulation but not fully implemented. However car is running well using speed modulation only.

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 }