Jamie Bignell / Mbed 2 deprecated Nucleo_blink_led

test

Dependencies:   mbed

Dependents:   MouseHybridSenseCode

main.cpp@7:11dd5581c763, 2018-04-28 (annotated)

Committer:
JamieBignell
Date:
Sat Apr 28 21:34:55 2018 +0000
Revision:
7:11dd5581c763
Parent:
6:a652deaae134 
Hybrid Mouse Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JamieBignell 5:dfea493f7a12 1 #include <mbed.h>
JamieBignell 5:dfea493f7a12 2 #include <funcdef.h>
JamieBignell 5:dfea493f7a12 3 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
JamieBignell 5:dfea493f7a12 4 //Initialise Global Variables
JamieBignell 5:dfea493f7a12 5
JamieBignell 5:dfea493f7a12 6 int sensorValue[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 7
JamieBignell 5:dfea493f7a12 8 int sensorMinValue[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 9 int sensorMaxValue[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 10
JamieBignell 5:dfea493f7a12 11 int sensorMinValue1[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 12 int sensorMaxValue1[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 13
JamieBignell 5:dfea493f7a12 14 int sensorMinValue2[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 15 int sensorMaxValue2[] = {0,0,0,0,0,0,0,0};
JamieBignell 5:dfea493f7a12 16
JamieBignell 5:dfea493f7a12 17 int sensorThreshold[] = {0,0,0,0,0,0,0,0};
JamieBignell 7:11dd5581c763 18 int sensorErrorsdiscrete[] = {0,0,0,0,0,0,0,0};
JamieBignell 6:a652deaae134 19 float sensorErrors[] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
JamieBignell 5:dfea493f7a12 20
JamieBignell 7:11dd5581c763 21 float linearOffset[106][2]={
JamieBignell 7:11dd5581c763 22 { 6.8275 , -26.0300 },
JamieBignell 7:11dd5581c763 23 { 6.6497 , -25.4700 },
JamieBignell 7:11dd5581c763 24 { 6.4960 , -24.9800 },
JamieBignell 7:11dd5581c763 25 { 6.2632 , -24.4900 },
JamieBignell 7:11dd5581c763 26 { 5.9921 , -23.9900 },
JamieBignell 7:11dd5581c763 27 { 5.8196 , -23.4700 },
JamieBignell 7:11dd5581c763 28 { 5.7159 , -22.9600 },
JamieBignell 7:11dd5581c763 29 { 5.5201 , -22.4600 },
JamieBignell 7:11dd5581c763 30 { 5.4829 , -22.0000 },
JamieBignell 7:11dd5581c763 31 { 5.3919 , -21.4900 },
JamieBignell 7:11dd5581c763 32 { 5.2843 , -20.9700 },
JamieBignell 7:11dd5581c763 33 { 5.2686 , -20.4500 },
JamieBignell 7:11dd5581c763 34 { 5.2052 , -19.9900 },
JamieBignell 7:11dd5581c763 35 { 5.1198 , -19.5200 },
JamieBignell 7:11dd5581c763 36 { 5.0410 , -19.0000 },
JamieBignell 7:11dd5581c763 37 { 4.9476 , -18.4500 },
JamieBignell 7:11dd5581c763 38 { 4.8565 , -18.0300 },
JamieBignell 7:11dd5581c763 39 { 4.6842 , -17.5100 },
JamieBignell 7:11dd5581c763 40 { 4.5375 , -16.9800 },
JamieBignell 7:11dd5581c763 41 { 4.4060 , -16.4400 },
JamieBignell 7:11dd5581c763 42 { 4.3064 , -16.0200 },
JamieBignell 7:11dd5581c763 43 { 4.1894 , -15.4500 },
JamieBignell 7:11dd5581c763 44 { 3.9800 , -14.9600 },
JamieBignell 7:11dd5581c763 45 { 3.8762 , -14.5200 },
JamieBignell 7:11dd5581c763 46 { 3.7609 , -13.9800 },
JamieBignell 7:11dd5581c763 47 { 3.6261 , -13.4800 },
JamieBignell 7:11dd5581c763 48 { 3.5013 , -13.0300 },
JamieBignell 7:11dd5581c763 49 { 3.3357 , -12.4800 },
JamieBignell 7:11dd5581c763 50 { 3.3057 , -12.0000 },
JamieBignell 7:11dd5581c763 51 { 3.2587 , -11.4900 },
JamieBignell 7:11dd5581c763 52 { 3.2009 , -11.0000 },
JamieBignell 7:11dd5581c763 53 { 3.1255 , -10.4100 },
JamieBignell 7:11dd5581c763 54 { 3.0409 , -9.9600 },
JamieBignell 7:11dd5581c763 55 { 2.9758 , -9.4600 },
JamieBignell 7:11dd5581c763 56 { 2.8237 , -9.0000 },
JamieBignell 7:11dd5581c763 57 { 2.7329 , -8.4500 },
JamieBignell 7:11dd5581c763 58 { 2.5944 , -7.9500 },
JamieBignell 7:11dd5581c763 59 { 2.4453 , -7.4300 },
JamieBignell 7:11dd5581c763 60 { 2.1612 , -6.9500 },
JamieBignell 7:11dd5581c763 61 { 2.1333 , -6.5100 },
JamieBignell 7:11dd5581c763 62 { 1.9384 , -6.0200 },
JamieBignell 7:11dd5581c763 63 { 1.7274 , -5.4800 },
JamieBignell 7:11dd5581c763 64 { 1.4727 , -4.9600 },
JamieBignell 7:11dd5581c763 65 { 1.2838 , -4.4900 },
JamieBignell 7:11dd5581c763 66 { 1.0132 , -3.9800 },
JamieBignell 7:11dd5581c763 67 { 0.7237 , -3.3500 },
JamieBignell 7:11dd5581c763 68 { 0.6065 , -3.0000 },
JamieBignell 7:11dd5581c763 69 { 0.4716 , -2.4700 },
JamieBignell 7:11dd5581c763 70 { 0.3420 , -1.9700 },
JamieBignell 7:11dd5581c763 71 { 0.2302 , -1.4900 },
JamieBignell 7:11dd5581c763 72 { 0.1598 , -0.9700 },
JamieBignell 7:11dd5581c763 73 { 0.0632 , -0.4900 },
JamieBignell 7:11dd5581c763 74 { -0.0137 , 0.0000 },
JamieBignell 7:11dd5581c763 75 { -0.0542 , 0.4800 },
JamieBignell 7:11dd5581c763 76 { -0.1547 , 1.0000 },
JamieBignell 7:11dd5581c763 77 { -0.1795 , 1.4900 },
JamieBignell 7:11dd5581c763 78 { -0.2608 , 1.9900 },
JamieBignell 7:11dd5581c763 79 { -0.3267 , 2.5000 },
JamieBignell 7:11dd5581c763 80 { -0.4297 , 2.9900 },
JamieBignell 7:11dd5581c763 81 { -0.5448 , 3.5600 },
JamieBignell 7:11dd5581c763 82 { -0.6416 , 4.0900 },
JamieBignell 7:11dd5581c763 83 { -0.8621 , 4.5000 },
JamieBignell 7:11dd5581c763 84 { -1.0357 , 5.0000 },
JamieBignell 7:11dd5581c763 85 { -1.2421 , 5.4700 },
JamieBignell 7:11dd5581c763 86 { -1.5076 , 6.0300 },
JamieBignell 7:11dd5581c763 87 { -1.8054 , 6.5500 },
JamieBignell 7:11dd5581c763 88 { -2.0865 , 7.0300 },
JamieBignell 7:11dd5581c763 89 { -2.2763 , 7.4700 },
JamieBignell 7:11dd5581c763 90 { -2.5454 , 8.0000 },
JamieBignell 7:11dd5581c763 91 { -2.7537 , 8.5600 },
JamieBignell 7:11dd5581c763 92 { -2.8835 , 8.9900 },
JamieBignell 7:11dd5581c763 93 { -3.0262 , 9.5000 },
JamieBignell 7:11dd5581c763 94 { -3.2092 , 10.0000 },
JamieBignell 7:11dd5581c763 95 { -3.3651 , 10.5600 },
JamieBignell 7:11dd5581c763 96 { -3.4420 , 10.9900 },
JamieBignell 7:11dd5581c763 97 { -3.5348 , 11.5000 },
JamieBignell 7:11dd5581c763 98 { -3.5697 , 11.9900 },
JamieBignell 7:11dd5581c763 99 { -3.6499 , 12.5300 },
JamieBignell 7:11dd5581c763 100 { -3.7035 , 13.0000 },
JamieBignell 7:11dd5581c763 101 { -3.7264 , 13.4700 },
JamieBignell 7:11dd5581c763 102 { -3.8294 , 14.0100 },
JamieBignell 7:11dd5581c763 103 { -3.9171 , 14.5100 },
JamieBignell 7:11dd5581c763 104 { -4.0666 , 15.1100 },
JamieBignell 7:11dd5581c763 105 { -4.1025 , 15.4700 },
JamieBignell 7:11dd5581c763 106 { -4.3145 , 16.0300 },
JamieBignell 7:11dd5581c763 107 { -4.4127 , 16.4800 },
JamieBignell 7:11dd5581c763 108 { -4.5864 , 17.0500 },
JamieBignell 7:11dd5581c763 109 { -4.6950 , 17.4700 },
JamieBignell 7:11dd5581c763 110 { -4.8164 , 18.0300 },
JamieBignell 7:11dd5581c763 111 { -4.9033 , 18.5300 },
JamieBignell 7:11dd5581c763 112 { -5.0180 , 19.0200 },
JamieBignell 7:11dd5581c763 113 { -5.1011 , 19.5400 },
JamieBignell 7:11dd5581c763 114 { -5.2195 , 20.0200 },
JamieBignell 7:11dd5581c763 115 { -5.3131 , 20.4700 },
JamieBignell 7:11dd5581c763 116 { -5.3823 , 21.0200 },
JamieBignell 7:11dd5581c763 117 { -5.4816 , 21.5200 },
JamieBignell 7:11dd5581c763 118 { -5.5606 , 21.9800 },
JamieBignell 7:11dd5581c763 119 { -5.6365 , 22.5000 },
JamieBignell 7:11dd5581c763 120 { -5.7055 , 23.0900 },
JamieBignell 7:11dd5581c763 121 { -5.7591 , 23.4700 },
JamieBignell 7:11dd5581c763 122 { -5.9464 , 23.9900 },
JamieBignell 7:11dd5581c763 123 { -6.1486 , 24.5400 },
JamieBignell 7:11dd5581c763 124 { -6.3176 , 25.0000 },
JamieBignell 7:11dd5581c763 125 { -6.4417 , 25.5000 },
JamieBignell 7:11dd5581c763 126 { -6.6916 , 25.9800 },
JamieBignell 7:11dd5581c763 127 { -6.9624 , 26.5600 }};
JamieBignell 7:11dd5581c763 128
JamieBignell 7:11dd5581c763 129 float motorLookup[11][2]={
JamieBignell 7:11dd5581c763 130 { 0.000 , 0.000 },
JamieBignell 7:11dd5581c763 131 { 20.000 , 100.000 },
JamieBignell 7:11dd5581c763 132 { 300.000 , 200.000 },
JamieBignell 7:11dd5581c763 133 { 520.000 , 300.000 },
JamieBignell 7:11dd5581c763 134 { 660.000 , 400.000 },
JamieBignell 7:11dd5581c763 135 { 750.000 , 500.000 },
JamieBignell 7:11dd5581c763 136 { 820.000 , 600.000 },
JamieBignell 7:11dd5581c763 137 { 860.000 , 700.000 },
JamieBignell 7:11dd5581c763 138 { 920.000 , 800.000 },
JamieBignell 7:11dd5581c763 139 { 940.000 , 900.000 },
JamieBignell 7:11dd5581c763 140 { 1000.000, 1000.000}};
JamieBignell 7:11dd5581c763 141
JamieBignell 6:a652deaae134 142 float Prop=0;
JamieBignell 6:a652deaae134 143 float Inte=0;
JamieBignell 6:a652deaae134 144 float Dere=0;
JamieBignell 5:dfea493f7a12 145
JamieBignell 6:a652deaae134 146 float errorVar=0;
JamieBignell 7:11dd5581c763 147 float largestpositiveerrorvar=0;
JamieBignell 7:11dd5581c763 148 float largestnegativeerrorvar=0;
JamieBignell 7:11dd5581c763 149 int errorVardiscrete=0;
JamieBignell 7:11dd5581c763 150 int previousErrordiscrete=0;
JamieBignell 6:a652deaae134 151 float previousError=0;
JamieBignell 6:a652deaae134 152 float PIDs=0;
JamieBignell 6:a652deaae134 153 int newPIDs=0;
JamieBignell 6:a652deaae134 154 int accumulator=0;
JamieBignell 7:11dd5581c763 155
JamieBignell 6:a652deaae134 156 bool stopMotors=false;
JamieBignell 6:a652deaae134 157
JamieBignell 6:a652deaae134 158 int leftMotorValTemp=0;
JamieBignell 6:a652deaae134 159 int rightMotorValTemp=0;
JamieBignell 6:a652deaae134 160
JamieBignell 7:11dd5581c763 161 int initialMotorspeed = 550;//IF THIS IS CHANGED, CHANGE IT IN TICKER
JamieBignell 7:11dd5581c763 162 int errorDetectthres=100;//IF THIS IS CHANGED, CHANGE IT IN BLACK ENDGE DETECTION
JamieBignell 5:dfea493f7a12 163
JamieBignell 7:11dd5581c763 164 Ticker ticker1;
JamieBignell 7:11dd5581c763 165 Ticker ticker2;
JamieBignell 5:dfea493f7a12 166 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
JamieBignell 5:dfea493f7a12 167 //Main Code
JamieBignell 0:c0c329729a7a 168
JamieBignell 6:a652deaae134 169 void tracker() {
JamieBignell 6:a652deaae134 170 //IF THE ERROR IS EQUAL TO THE LAST ERROR, START A COUNTING VARIABLE AND ADD THIS TO THE NOMINAL MOTOR SPEED (IE WHEN ERROR OF 0), IF CASE NOT TRUE, DO NOTHING/RETURN TO NORMAL SPEED FOR CORNER.
JamieBignell 7:11dd5581c763 171 if (abs(PIDs)<=errorDetectthres)
JamieBignell 6:a652deaae134 172 {
JamieBignell 6:a652deaae134 173 accumulator++;
JamieBignell 6:a652deaae134 174 LEDVal=1;
JamieBignell 6:a652deaae134 175 }
JamieBignell 6:a652deaae134 176 else
JamieBignell 6:a652deaae134 177 {
JamieBignell 6:a652deaae134 178 accumulator=0;
JamieBignell 7:11dd5581c763 179 errorDetectthres=65;
JamieBignell 6:a652deaae134 180 LEDVal=0;
JamieBignell 6:a652deaae134 181 }
JamieBignell 6:a652deaae134 182
JamieBignell 7:11dd5581c763 183 if (accumulator>=14)
JamieBignell 7:11dd5581c763 184 {
JamieBignell 7:11dd5581c763 185 initialMotorspeed = 975;
JamieBignell 7:11dd5581c763 186 }
JamieBignell 7:11dd5581c763 187 else if (accumulator>=12)
JamieBignell 7:11dd5581c763 188 {
JamieBignell 7:11dd5581c763 189 initialMotorspeed = 975;
JamieBignell 7:11dd5581c763 190 errorDetectthres=300;
JamieBignell 7:11dd5581c763 191 }
JamieBignell 7:11dd5581c763 192 else if (accumulator>=10)
JamieBignell 6:a652deaae134 193 {
JamieBignell 7:11dd5581c763 194 initialMotorspeed = 950;
JamieBignell 7:11dd5581c763 195 }
JamieBignell 7:11dd5581c763 196 else if (accumulator>=8)
JamieBignell 7:11dd5581c763 197 {
JamieBignell 7:11dd5581c763 198 initialMotorspeed = 900;
JamieBignell 7:11dd5581c763 199 errorDetectthres=150;
JamieBignell 7:11dd5581c763 200 }
JamieBignell 7:11dd5581c763 201 else if (accumulator>=6)
JamieBignell 7:11dd5581c763 202 {
JamieBignell 7:11dd5581c763 203 initialMotorspeed = 600;
JamieBignell 7:11dd5581c763 204 errorDetectthres=125;
JamieBignell 7:11dd5581c763 205 }
JamieBignell 7:11dd5581c763 206 else if (accumulator>=4)
JamieBignell 7:11dd5581c763 207 {
JamieBignell 7:11dd5581c763 208 initialMotorspeed = 550;
JamieBignell 6:a652deaae134 209 }
JamieBignell 6:a652deaae134 210 else if (accumulator>=2)
JamieBignell 6:a652deaae134 211 {
JamieBignell 7:11dd5581c763 212 initialMotorspeed = 550;
JamieBignell 6:a652deaae134 213 }
JamieBignell 6:a652deaae134 214 else
JamieBignell 6:a652deaae134 215 {
JamieBignell 7:11dd5581c763 216 initialMotorspeed = 550;
JamieBignell 6:a652deaae134 217 }
JamieBignell 6:a652deaae134 218 }
JamieBignell 6:a652deaae134 219
JamieBignell 7:11dd5581c763 220 void calculations()
JamieBignell 7:11dd5581c763 221 {
JamieBignell 7:11dd5581c763 222 //USE ERROR TO CALCULATE PID VALUES
JamieBignell 7:11dd5581c763 223 Prop = errorVar;
JamieBignell 7:11dd5581c763 224 Inte = Inte + errorVar;
JamieBignell 7:11dd5581c763 225 Dere = ((errorVar) - (previousError));
JamieBignell 7:11dd5581c763 226
JamieBignell 7:11dd5581c763 227 //BECAUSE THE CURRENT ERROR IS CALCULATED IN A FUNCTION BEFORE THIS, ON THE NEXT ITERATION WE WANT TO REMEMBER THE PREVIOUS VARIABLE
JamieBignell 7:11dd5581c763 228 previousError=errorVar;
JamieBignell 7:11dd5581c763 229 previousErrordiscrete=errorVardiscrete;
JamieBignell 7:11dd5581c763 230
JamieBignell 7:11dd5581c763 231 //FORMULATE OFFSET VALUE FROM ERROR, PID VALUES AND CONSTANTS
JamieBignell 7:11dd5581c763 232 PIDs = (Kp*Prop)+(Kd*Dere)+(Ki*Inte);
JamieBignell 7:11dd5581c763 233 newPIDs=PIDs;
JamieBignell 7:11dd5581c763 234 }
JamieBignell 7:11dd5581c763 235
JamieBignell 3:7f44ab64e96b 236 int main()
JamieBignell 5:dfea493f7a12 237 {
JamieBignell 7:11dd5581c763 238 //CONFIGURE MOTORS & TURN OFF LED
JamieBignell 4:f1670eec4681 239 LEDVal=0;
JamieBignell 5:dfea493f7a12 240 leftMotorVal.period_us(1000);
JamieBignell 5:dfea493f7a12 241 rightMotorVal.period_us(1000);
JamieBignell 5:dfea493f7a12 242 leftMotorVal.pulsewidth_us(0);
JamieBignell 5:dfea493f7a12 243 rightMotorVal.pulsewidth_us(0);
JamieBignell 4:f1670eec4681 244
JamieBignell 5:dfea493f7a12 245 wait_ms(1000);
JamieBignell 4:f1670eec4681 246
JamieBignell 6:a652deaae134 247 //TURN LED ON
JamieBignell 6:a652deaae134 248 LEDVal=1;
JamieBignell 6:a652deaae134 249
JamieBignell 5:dfea493f7a12 250 //BEGIN CALIBRATION
JamieBignell 5:dfea493f7a12 251 CalibrateFunc();
JamieBignell 6:a652deaae134 252
JamieBignell 5:dfea493f7a12 253 //TURN OFF LED AND SET PULSES TO 2 HZ
JamieBignell 5:dfea493f7a12 254 LEDVal=0;
JamieBignell 6:a652deaae134 255
JamieBignell 5:dfea493f7a12 256 //FIND LINE
JamieBignell 6:a652deaae134 257 findLineFunc();
JamieBignell 7:11dd5581c763 258
JamieBignell 5:dfea493f7a12 259 //ONCE FOUND, HAND OVER TO MAIN ALGORITHM
JamieBignell 7:11dd5581c763 260 ticker1.attach(&tracker, 0.2);
JamieBignell 7:11dd5581c763 261 ticker2.attach(&calculations, 0.002);
JamieBignell 5:dfea493f7a12 262 while(1)
JamieBignell 5:dfea493f7a12 263 {
JamieBignell 5:dfea493f7a12 264 senseFunc();
JamieBignell 6:a652deaae134 265 CountingVar=!CountingVar;
JamieBignell 7:11dd5581c763 266 //calculateFunc();
JamieBignell 5:dfea493f7a12 267 driveFunc();
JamieBignell 4:f1670eec4681 268 }
JamieBignell 6:a652deaae134 269
JamieBignell 3:7f44ab64e96b 270 return 0;
JamieBignell 3:7f44ab64e96b 271 }