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@2:eb79d6e3f701, 2017-07-05 (annotated)

Committer:
Clarkk
Date:
Wed Jul 05 20:31:26 2017 +0000
Revision:
2:eb79d6e3f701
Parent:
1:5402c62ad487
Child:
3:c21068661635
Nice version with some code cleaning.; Car is quite fast, slow down when error is high and accelerate then.

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