Bram Stoelinga / Mbed 2 deprecated ProjectVroemba

Projectvroemba

Dependencies:   mbed

main.cpp@0:dc7bdd0a1b32, 2016-12-14 (annotated)

Committer:
bramstoelinga
Date:
Wed Dec 14 21:18:17 2016 +0000
Revision:
0:dc7bdd0a1b32
ii

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bramstoelinga 0:dc7bdd0a1b32 1 #include "mbed.h"
bramstoelinga 0:dc7bdd0a1b32 2 #include <algorithm>
bramstoelinga 0:dc7bdd0a1b32 3
bramstoelinga 0:dc7bdd0a1b32 4 double voltageToDistance(uint16_t);
bramstoelinga 0:dc7bdd0a1b32 5 void setPwmBoth(float);
bramstoelinga 0:dc7bdd0a1b32 6
bramstoelinga 0:dc7bdd0a1b32 7 double FilterRechts(double);
bramstoelinga 0:dc7bdd0a1b32 8 double FilterVoor(double);
bramstoelinga 0:dc7bdd0a1b32 9 double FilterBeneden(double);
bramstoelinga 0:dc7bdd0a1b32 10 double FilterLinks(double);
bramstoelinga 0:dc7bdd0a1b32 11
bramstoelinga 0:dc7bdd0a1b32 12 AnalogIn sensorA1(A2); //Sensor 1
bramstoelinga 0:dc7bdd0a1b32 13 AnalogIn sensorA2(A3); //Sensor 2
bramstoelinga 0:dc7bdd0a1b32 14 AnalogIn sensorA3(A4); //Sensor 3
bramstoelinga 0:dc7bdd0a1b32 15 AnalogIn sensorA4(A5); //Sensor 4
bramstoelinga 0:dc7bdd0a1b32 16
bramstoelinga 0:dc7bdd0a1b32 17 //H-brug poorten (pwm)
bramstoelinga 0:dc7bdd0a1b32 18 PwmOut pwmL1(PC_8);
bramstoelinga 0:dc7bdd0a1b32 19 PwmOut pwmL3(PB_4);
bramstoelinga 0:dc7bdd0a1b32 20
bramstoelinga 0:dc7bdd0a1b32 21 //Duty cycle
bramstoelinga 0:dc7bdd0a1b32 22 float dutyCycle1 = 0.0f;
bramstoelinga 0:dc7bdd0a1b32 23 float dutyCycle2 = 0.0f;
bramstoelinga 0:dc7bdd0a1b32 24
bramstoelinga 0:dc7bdd0a1b32 25 void setPwmBoth(float pwmFloat) {
bramstoelinga 0:dc7bdd0a1b32 26 dutyCycle1 = pwmFloat;
bramstoelinga 0:dc7bdd0a1b32 27 dutyCycle2 = pwmFloat;
bramstoelinga 0:dc7bdd0a1b32 28 pwmL1 = dutyCycle1;
bramstoelinga 0:dc7bdd0a1b32 29 pwmL3 = dutyCycle2;
bramstoelinga 0:dc7bdd0a1b32 30 }
bramstoelinga 0:dc7bdd0a1b32 31
bramstoelinga 0:dc7bdd0a1b32 32 double FilterRechts(double sensorValue) {
bramstoelinga 0:dc7bdd0a1b32 33 static double sensorArray[10] = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20};
bramstoelinga 0:dc7bdd0a1b32 34 static int arrayPosition = 0;
bramstoelinga 0:dc7bdd0a1b32 35 double average = 0;
bramstoelinga 0:dc7bdd0a1b32 36 sensorArray[arrayPosition] = sensorValue;
bramstoelinga 0:dc7bdd0a1b32 37 arrayPosition ++;
bramstoelinga 0:dc7bdd0a1b32 38 for(int i = 0; i < 10; i++) {
bramstoelinga 0:dc7bdd0a1b32 39 average += sensorArray[i];
bramstoelinga 0:dc7bdd0a1b32 40 }
bramstoelinga 0:dc7bdd0a1b32 41 average /= 10;
bramstoelinga 0:dc7bdd0a1b32 42 if(arrayPosition == 10) {
bramstoelinga 0:dc7bdd0a1b32 43 arrayPosition = 0;
bramstoelinga 0:dc7bdd0a1b32 44 }
bramstoelinga 0:dc7bdd0a1b32 45 printf("%f\n", average);
bramstoelinga 0:dc7bdd0a1b32 46 return average;
bramstoelinga 0:dc7bdd0a1b32 47 }
bramstoelinga 0:dc7bdd0a1b32 48 double FilterVoor(double sensorValue) {
bramstoelinga 0:dc7bdd0a1b32 49 static double sensorArray[10] = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20};
bramstoelinga 0:dc7bdd0a1b32 50 static int arrayPosition = 0;
bramstoelinga 0:dc7bdd0a1b32 51 double average = 0;
bramstoelinga 0:dc7bdd0a1b32 52 sensorArray[arrayPosition] = sensorValue;
bramstoelinga 0:dc7bdd0a1b32 53 arrayPosition ++;
bramstoelinga 0:dc7bdd0a1b32 54 for(int i = 0; i < 10; i++)
bramstoelinga 0:dc7bdd0a1b32 55 {
bramstoelinga 0:dc7bdd0a1b32 56 average += sensorArray[i];
bramstoelinga 0:dc7bdd0a1b32 57 }
bramstoelinga 0:dc7bdd0a1b32 58 average /= 10;
bramstoelinga 0:dc7bdd0a1b32 59 if(arrayPosition == 10)
bramstoelinga 0:dc7bdd0a1b32 60 {
bramstoelinga 0:dc7bdd0a1b32 61 arrayPosition = 0;
bramstoelinga 0:dc7bdd0a1b32 62 }
bramstoelinga 0:dc7bdd0a1b32 63 printf("%f\n", average);
bramstoelinga 0:dc7bdd0a1b32 64 return average;
bramstoelinga 0:dc7bdd0a1b32 65 }
bramstoelinga 0:dc7bdd0a1b32 66 double FilterBeneden(double sensorValue) {
bramstoelinga 0:dc7bdd0a1b32 67 static double sensorArray[10] = {14, 14, 14, 14, 14, 14, 14, 14, 14, 14};
bramstoelinga 0:dc7bdd0a1b32 68 static int arrayPosition = 0;
bramstoelinga 0:dc7bdd0a1b32 69 double average = 0;
bramstoelinga 0:dc7bdd0a1b32 70 sensorArray[arrayPosition] = sensorValue;
bramstoelinga 0:dc7bdd0a1b32 71 arrayPosition ++;
bramstoelinga 0:dc7bdd0a1b32 72 for(int i = 0; i < 10; i++)
bramstoelinga 0:dc7bdd0a1b32 73 {
bramstoelinga 0:dc7bdd0a1b32 74 average += sensorArray[i];
bramstoelinga 0:dc7bdd0a1b32 75 }
bramstoelinga 0:dc7bdd0a1b32 76 average /= 10;
bramstoelinga 0:dc7bdd0a1b32 77 if(arrayPosition == 10)
bramstoelinga 0:dc7bdd0a1b32 78 {
bramstoelinga 0:dc7bdd0a1b32 79 arrayPosition = 0;
bramstoelinga 0:dc7bdd0a1b32 80 }
bramstoelinga 0:dc7bdd0a1b32 81 printf("%f\n", average);
bramstoelinga 0:dc7bdd0a1b32 82 return average;
bramstoelinga 0:dc7bdd0a1b32 83 }
bramstoelinga 0:dc7bdd0a1b32 84 double FilterLinks(double sensorValue) {
bramstoelinga 0:dc7bdd0a1b32 85 static double sensorArray[10] = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20};
bramstoelinga 0:dc7bdd0a1b32 86 static int arrayPosition = 0;
bramstoelinga 0:dc7bdd0a1b32 87 double average = 0;
bramstoelinga 0:dc7bdd0a1b32 88 sensorArray[arrayPosition] = sensorValue;
bramstoelinga 0:dc7bdd0a1b32 89 arrayPosition ++;
bramstoelinga 0:dc7bdd0a1b32 90 for(int i = 0; i < 10; i++)
bramstoelinga 0:dc7bdd0a1b32 91 {
bramstoelinga 0:dc7bdd0a1b32 92 average += sensorArray[i];
bramstoelinga 0:dc7bdd0a1b32 93 }
bramstoelinga 0:dc7bdd0a1b32 94 average /= 10;
bramstoelinga 0:dc7bdd0a1b32 95 if(arrayPosition == 10)
bramstoelinga 0:dc7bdd0a1b32 96 {
bramstoelinga 0:dc7bdd0a1b32 97 arrayPosition = 0;
bramstoelinga 0:dc7bdd0a1b32 98 }
bramstoelinga 0:dc7bdd0a1b32 99 printf("%f\n", average);
bramstoelinga 0:dc7bdd0a1b32 100 return average;
bramstoelinga 0:dc7bdd0a1b32 101 }
bramstoelinga 0:dc7bdd0a1b32 102
bramstoelinga 0:dc7bdd0a1b32 103 double voltageToDistance(uint16_t voltage) {
bramstoelinga 0:dc7bdd0a1b32 104 if (voltage < 16)
bramstoelinga 0:dc7bdd0a1b32 105 {
bramstoelinga 0:dc7bdd0a1b32 106 voltage = 16;
bramstoelinga 0:dc7bdd0a1b32 107 }
bramstoelinga 0:dc7bdd0a1b32 108 double distance = 0;
bramstoelinga 0:dc7bdd0a1b32 109 distance = 2076.0 / (voltage - 11.0);
bramstoelinga 0:dc7bdd0a1b32 110 return distance * 100;
bramstoelinga 0:dc7bdd0a1b32 111 }
bramstoelinga 0:dc7bdd0a1b32 112
bramstoelinga 0:dc7bdd0a1b32 113 int main(){
bramstoelinga 0:dc7bdd0a1b32 114 enum State
bramstoelinga 0:dc7bdd0a1b32 115 {
bramstoelinga 0:dc7bdd0a1b32 116 initialize,
bramstoelinga 0:dc7bdd0a1b32 117 wait,
bramstoelinga 0:dc7bdd0a1b32 118 autonomous,
bramstoelinga 0:dc7bdd0a1b32 119 HmoveForward,
bramstoelinga 0:dc7bdd0a1b32 120 HturnLeft,
bramstoelinga 0:dc7bdd0a1b32 121 HturnRight,
bramstoelinga 0:dc7bdd0a1b32 122 Hreverse,
bramstoelinga 0:dc7bdd0a1b32 123 neutral,
bramstoelinga 0:dc7bdd0a1b32 124 AmoveForward,
bramstoelinga 0:dc7bdd0a1b32 125 Aleft,
bramstoelinga 0:dc7bdd0a1b32 126 Aright,
bramstoelinga 0:dc7bdd0a1b32 127 AturnLeft,
bramstoelinga 0:dc7bdd0a1b32 128 AturnRight
bramstoelinga 0:dc7bdd0a1b32 129 };
bramstoelinga 0:dc7bdd0a1b32 130
bramstoelinga 0:dc7bdd0a1b32 131 //Digitale H-brug poorten
bramstoelinga 0:dc7bdd0a1b32 132 DigitalOut L2(PB_8);
bramstoelinga 0:dc7bdd0a1b32 133 DigitalOut E1(PC_9);
bramstoelinga 0:dc7bdd0a1b32 134 DigitalOut L4(PB_14);
bramstoelinga 0:dc7bdd0a1b32 135 DigitalOut E2(PB_15);
bramstoelinga 0:dc7bdd0a1b32 136
bramstoelinga 0:dc7bdd0a1b32 137 //Statemachine
bramstoelinga 0:dc7bdd0a1b32 138 State stateMachine;
bramstoelinga 0:dc7bdd0a1b32 139 stateMachine = initialize;
bramstoelinga 0:dc7bdd0a1b32 140
bramstoelinga 0:dc7bdd0a1b32 141 //Serieele poorten
bramstoelinga 0:dc7bdd0a1b32 142 Serial pc(USBTX, USBRX);
bramstoelinga 0:dc7bdd0a1b32 143 Serial bluetooth(PA_11, PA_12); // TX, RX
bramstoelinga 0:dc7bdd0a1b32 144
bramstoelinga 0:dc7bdd0a1b32 145 //Booleans om afgelopen statussen te onthouden
bramstoelinga 0:dc7bdd0a1b32 146 bool fromForward = false;
bramstoelinga 0:dc7bdd0a1b32 147 bool fromReverse = false;
bramstoelinga 0:dc7bdd0a1b32 148 bool status = false; //status false = autonoom, true = handmatig
bramstoelinga 0:dc7bdd0a1b32 149
bramstoelinga 0:dc7bdd0a1b32 150 //Timer voor het uitrijden van de bluetooth
bramstoelinga 0:dc7bdd0a1b32 151 Timer bluetoothTimer;
bramstoelinga 0:dc7bdd0a1b32 152 Timer autoTimer;
bramstoelinga 0:dc7bdd0a1b32 153
bramstoelinga 0:dc7bdd0a1b32 154 int movingDelay = 25;
bramstoelinga 0:dc7bdd0a1b32 155 int cutoff = 30;
bramstoelinga 0:dc7bdd0a1b32 156 int detection = 25;
bramstoelinga 0:dc7bdd0a1b32 157
bramstoelinga 0:dc7bdd0a1b32 158 while (1)
bramstoelinga 0:dc7bdd0a1b32 159 {
bramstoelinga 0:dc7bdd0a1b32 160 switch(stateMachine)
bramstoelinga 0:dc7bdd0a1b32 161 {
bramstoelinga 0:dc7bdd0a1b32 162 case initialize:
bramstoelinga 0:dc7bdd0a1b32 163 pwmL1.period_ms(10);
bramstoelinga 0:dc7bdd0a1b32 164 pwmL3.period_ms(10);
bramstoelinga 0:dc7bdd0a1b32 165 pwmL1 = dutyCycle1;
bramstoelinga 0:dc7bdd0a1b32 166 pwmL3 = dutyCycle2;
bramstoelinga 0:dc7bdd0a1b32 167 bluetooth.baud(115200);
bramstoelinga 0:dc7bdd0a1b32 168 pc.baud(115200);
bramstoelinga 0:dc7bdd0a1b32 169 pc.printf("Bluetooth Start\r\n");
bramstoelinga 0:dc7bdd0a1b32 170 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 171 break;
bramstoelinga 0:dc7bdd0a1b32 172
bramstoelinga 0:dc7bdd0a1b32 173 case wait:
bramstoelinga 0:dc7bdd0a1b32 174 if (bluetooth.readable())
bramstoelinga 0:dc7bdd0a1b32 175 {
bramstoelinga 0:dc7bdd0a1b32 176 switch(bluetooth.getc())
bramstoelinga 0:dc7bdd0a1b32 177 {
bramstoelinga 0:dc7bdd0a1b32 178 case 30:
bramstoelinga 0:dc7bdd0a1b32 179 status = true;
bramstoelinga 0:dc7bdd0a1b32 180 bluetoothTimer.start();
bramstoelinga 0:dc7bdd0a1b32 181 break;
bramstoelinga 0:dc7bdd0a1b32 182
bramstoelinga 0:dc7bdd0a1b32 183 case 40:
bramstoelinga 0:dc7bdd0a1b32 184 status = false;
bramstoelinga 0:dc7bdd0a1b32 185 stateMachine = autonomous;
bramstoelinga 0:dc7bdd0a1b32 186 break;
bramstoelinga 0:dc7bdd0a1b32 187
bramstoelinga 0:dc7bdd0a1b32 188 case 49:
bramstoelinga 0:dc7bdd0a1b32 189 bluetoothTimer.reset();
bramstoelinga 0:dc7bdd0a1b32 190 if(status == true)
bramstoelinga 0:dc7bdd0a1b32 191 {
bramstoelinga 0:dc7bdd0a1b32 192 stateMachine = HmoveForward;
bramstoelinga 0:dc7bdd0a1b32 193 }
bramstoelinga 0:dc7bdd0a1b32 194 break;
bramstoelinga 0:dc7bdd0a1b32 195
bramstoelinga 0:dc7bdd0a1b32 196 case 47:
bramstoelinga 0:dc7bdd0a1b32 197 bluetoothTimer.reset();
bramstoelinga 0:dc7bdd0a1b32 198 if(status == true)
bramstoelinga 0:dc7bdd0a1b32 199 {
bramstoelinga 0:dc7bdd0a1b32 200 stateMachine = HturnLeft;
bramstoelinga 0:dc7bdd0a1b32 201 }
bramstoelinga 0:dc7bdd0a1b32 202 break;
bramstoelinga 0:dc7bdd0a1b32 203
bramstoelinga 0:dc7bdd0a1b32 204 case 51:
bramstoelinga 0:dc7bdd0a1b32 205 bluetoothTimer.reset();
bramstoelinga 0:dc7bdd0a1b32 206 if(status == true)
bramstoelinga 0:dc7bdd0a1b32 207 {
bramstoelinga 0:dc7bdd0a1b32 208 stateMachine = HturnRight;
bramstoelinga 0:dc7bdd0a1b32 209 }
bramstoelinga 0:dc7bdd0a1b32 210 break;
bramstoelinga 0:dc7bdd0a1b32 211
bramstoelinga 0:dc7bdd0a1b32 212 case 55:
bramstoelinga 0:dc7bdd0a1b32 213 bluetoothTimer.reset();
bramstoelinga 0:dc7bdd0a1b32 214 if(status == true) {
bramstoelinga 0:dc7bdd0a1b32 215 stateMachine = Hreverse;
bramstoelinga 0:dc7bdd0a1b32 216 }
bramstoelinga 0:dc7bdd0a1b32 217 break;
bramstoelinga 0:dc7bdd0a1b32 218 }
bramstoelinga 0:dc7bdd0a1b32 219 }
bramstoelinga 0:dc7bdd0a1b32 220 else if (status == false)
bramstoelinga 0:dc7bdd0a1b32 221 {
bramstoelinga 0:dc7bdd0a1b32 222 stateMachine = autonomous;
bramstoelinga 0:dc7bdd0a1b32 223 }
bramstoelinga 0:dc7bdd0a1b32 224 else if (status == true)
bramstoelinga 0:dc7bdd0a1b32 225 {
bramstoelinga 0:dc7bdd0a1b32 226 stateMachine = neutral;
bramstoelinga 0:dc7bdd0a1b32 227 }
bramstoelinga 0:dc7bdd0a1b32 228 break;
bramstoelinga 0:dc7bdd0a1b32 229
bramstoelinga 0:dc7bdd0a1b32 230 case autonomous:
bramstoelinga 0:dc7bdd0a1b32 231 double SensorRechts = voltageToDistance(sensorA1.read_u16());
bramstoelinga 0:dc7bdd0a1b32 232 double SensorVoor = voltageToDistance(sensorA2.read_u16());
bramstoelinga 0:dc7bdd0a1b32 233 double SensorBeneden = voltageToDistance(sensorA3.read_u16());
bramstoelinga 0:dc7bdd0a1b32 234 double SensorLinks = voltageToDistance(sensorA4.read_u16());
bramstoelinga 0:dc7bdd0a1b32 235
bramstoelinga 0:dc7bdd0a1b32 236 if(SensorRechts > cutoff) {SensorRechts = cutoff;}
bramstoelinga 0:dc7bdd0a1b32 237 if(SensorVoor > cutoff) {SensorVoor = cutoff;}
bramstoelinga 0:dc7bdd0a1b32 238 if(SensorBeneden > cutoff) {SensorBeneden = cutoff;}
bramstoelinga 0:dc7bdd0a1b32 239 if(SensorLinks > cutoff) {SensorLinks = cutoff;}
bramstoelinga 0:dc7bdd0a1b32 240
bramstoelinga 0:dc7bdd0a1b32 241 SensorRechts = FilterRechts(SensorRechts);
bramstoelinga 0:dc7bdd0a1b32 242 SensorVoor = FilterVoor(SensorVoor);
bramstoelinga 0:dc7bdd0a1b32 243 SensorBeneden = FilterBeneden(SensorBeneden);
bramstoelinga 0:dc7bdd0a1b32 244 SensorLinks = FilterLinks(SensorLinks);
bramstoelinga 0:dc7bdd0a1b32 245
bramstoelinga 0:dc7bdd0a1b32 246 if(SensorBeneden > 17 || SensorBeneden < 11)
bramstoelinga 0:dc7bdd0a1b32 247 {
bramstoelinga 0:dc7bdd0a1b32 248 stateMachine = AturnLeft; //Draai naar links als er een afgrond is of een obstakel ligt
bramstoelinga 0:dc7bdd0a1b32 249 }
bramstoelinga 0:dc7bdd0a1b32 250 else if(SensorRechts > detection && SensorVoor > detection && SensorLinks > detection)
bramstoelinga 0:dc7bdd0a1b32 251 {
bramstoelinga 0:dc7bdd0a1b32 252 stateMachine = AmoveForward; //Naar voren als sensoren niks zien.
bramstoelinga 0:dc7bdd0a1b32 253 }
bramstoelinga 0:dc7bdd0a1b32 254 else if(SensorRechts < detection && SensorVoor > detection && SensorLinks > detection)
bramstoelinga 0:dc7bdd0a1b32 255 {
bramstoelinga 0:dc7bdd0a1b32 256 stateMachine = Aleft; //Naar links afbuigen als een schuine sensor een object ziet.
bramstoelinga 0:dc7bdd0a1b32 257 }
bramstoelinga 0:dc7bdd0a1b32 258 else if(SensorRechts > detection && SensorVoor > detection && SensorLinks < detection)
bramstoelinga 0:dc7bdd0a1b32 259 {
bramstoelinga 0:dc7bdd0a1b32 260 stateMachine = Aright; //Naar rechts afbuigen als een schuine sensor een object ziet.
bramstoelinga 0:dc7bdd0a1b32 261 }
bramstoelinga 0:dc7bdd0a1b32 262 else if(SensorVoor < detection && SensorLinks > detection)
bramstoelinga 0:dc7bdd0a1b32 263 {
bramstoelinga 0:dc7bdd0a1b32 264 stateMachine = AturnLeft; //Naar links draaien als de sensor naar voren iets ziet (rechts maakt niet uit)
bramstoelinga 0:dc7bdd0a1b32 265 }
bramstoelinga 0:dc7bdd0a1b32 266 else if(SensorVoor < detection && SensorLinks < detection)
bramstoelinga 0:dc7bdd0a1b32 267 {
bramstoelinga 0:dc7bdd0a1b32 268 stateMachine = AturnRight; //Naar rechts draaien als de sensoren naar voor en links iets zien
bramstoelinga 0:dc7bdd0a1b32 269 }
bramstoelinga 0:dc7bdd0a1b32 270 break;
bramstoelinga 0:dc7bdd0a1b32 271
bramstoelinga 0:dc7bdd0a1b32 272 case HmoveForward:
bramstoelinga 0:dc7bdd0a1b32 273 fromForward = true;
bramstoelinga 0:dc7bdd0a1b32 274 if(fromReverse == true)
bramstoelinga 0:dc7bdd0a1b32 275 {
bramstoelinga 0:dc7bdd0a1b32 276 setPwmBoth(0.0f);
bramstoelinga 0:dc7bdd0a1b32 277 fromReverse = false;
bramstoelinga 0:dc7bdd0a1b32 278 }
bramstoelinga 0:dc7bdd0a1b32 279 if(dutyCycle1 <= 0.99f && status == true)
bramstoelinga 0:dc7bdd0a1b32 280 {
bramstoelinga 0:dc7bdd0a1b32 281 E2 = true;
bramstoelinga 0:dc7bdd0a1b32 282 L4 = false;
bramstoelinga 0:dc7bdd0a1b32 283 E1 = true;
bramstoelinga 0:dc7bdd0a1b32 284 L2 = false;
bramstoelinga 0:dc7bdd0a1b32 285 dutyCycle1 += 0.05f;
bramstoelinga 0:dc7bdd0a1b32 286 dutyCycle2 += 0.05f;
bramstoelinga 0:dc7bdd0a1b32 287 pwmL1 = dutyCycle1;
bramstoelinga 0:dc7bdd0a1b32 288 pwmL3 = dutyCycle2;
bramstoelinga 0:dc7bdd0a1b32 289 }
bramstoelinga 0:dc7bdd0a1b32 290 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 291 break;
bramstoelinga 0:dc7bdd0a1b32 292
bramstoelinga 0:dc7bdd0a1b32 293 case HturnLeft:
bramstoelinga 0:dc7bdd0a1b32 294 E1 = true;
bramstoelinga 0:dc7bdd0a1b32 295 L2 = true;
bramstoelinga 0:dc7bdd0a1b32 296 E2 = true;
bramstoelinga 0:dc7bdd0a1b32 297 L4 = false;
bramstoelinga 0:dc7bdd0a1b32 298 dutyCycle1 = dutyCycle2 = 0.5f;
bramstoelinga 0:dc7bdd0a1b32 299 pwmL1 = pwmL3 = dutyCycle1;
bramstoelinga 0:dc7bdd0a1b32 300 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 301 break;
bramstoelinga 0:dc7bdd0a1b32 302
bramstoelinga 0:dc7bdd0a1b32 303 case HturnRight:
bramstoelinga 0:dc7bdd0a1b32 304 E1 = true;
bramstoelinga 0:dc7bdd0a1b32 305 L2 = false;
bramstoelinga 0:dc7bdd0a1b32 306 E2 = true;
bramstoelinga 0:dc7bdd0a1b32 307 L4 = true;
bramstoelinga 0:dc7bdd0a1b32 308 dutyCycle1 = dutyCycle2 = 0.5f;
bramstoelinga 0:dc7bdd0a1b32 309 pwmL1 = pwmL3 = dutyCycle1;
bramstoelinga 0:dc7bdd0a1b32 310 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 311 break;
bramstoelinga 0:dc7bdd0a1b32 312
bramstoelinga 0:dc7bdd0a1b32 313 case Hreverse:
bramstoelinga 0:dc7bdd0a1b32 314 fromReverse = true;
bramstoelinga 0:dc7bdd0a1b32 315 E2 = true;
bramstoelinga 0:dc7bdd0a1b32 316 L4 = true;
bramstoelinga 0:dc7bdd0a1b32 317 E1 = true;
bramstoelinga 0:dc7bdd0a1b32 318 L2 = true;
bramstoelinga 0:dc7bdd0a1b32 319 if(fromForward == true)
bramstoelinga 0:dc7bdd0a1b32 320 {
bramstoelinga 0:dc7bdd0a1b32 321 setPwmBoth(1.0f);
bramstoelinga 0:dc7bdd0a1b32 322 fromForward = false;
bramstoelinga 0:dc7bdd0a1b32 323 }
bramstoelinga 0:dc7bdd0a1b32 324 dutyCycle1 -= 0.05f;
bramstoelinga 0:dc7bdd0a1b32 325 dutyCycle2 -= 0.05f;
bramstoelinga 0:dc7bdd0a1b32 326 pwmL1 = dutyCycle1;
bramstoelinga 0:dc7bdd0a1b32 327 pwmL3 = dutyCycle2;
bramstoelinga 0:dc7bdd0a1b32 328 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 329 break;
bramstoelinga 0:dc7bdd0a1b32 330
bramstoelinga 0:dc7bdd0a1b32 331 case neutral:
bramstoelinga 0:dc7bdd0a1b32 332 if(bluetoothTimer.read_ms() > 100) {
bramstoelinga 0:dc7bdd0a1b32 333 E2 = false;
bramstoelinga 0:dc7bdd0a1b32 334 L4 = false;
bramstoelinga 0:dc7bdd0a1b32 335 pwmL1 = false;
bramstoelinga 0:dc7bdd0a1b32 336 pwmL3 = false;
bramstoelinga 0:dc7bdd0a1b32 337 E1 = false;
bramstoelinga 0:dc7bdd0a1b32 338 L2 = false;
bramstoelinga 0:dc7bdd0a1b32 339 if (fromForward == true){setPwmBoth(0.0f);}
bramstoelinga 0:dc7bdd0a1b32 340 else if(fromReverse == true){setPwmBoth(1.0f);}
bramstoelinga 0:dc7bdd0a1b32 341 }
bramstoelinga 0:dc7bdd0a1b32 342 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 343 break;
bramstoelinga 0:dc7bdd0a1b32 344
bramstoelinga 0:dc7bdd0a1b32 345 case AmoveForward:
bramstoelinga 0:dc7bdd0a1b32 346 autoTimer.start();
bramstoelinga 0:dc7bdd0a1b32 347 while(autoTimer.read_ms() < movingDelay)
bramstoelinga 0:dc7bdd0a1b32 348 {
bramstoelinga 0:dc7bdd0a1b32 349 E1 = true;
bramstoelinga 0:dc7bdd0a1b32 350 L2 = false;
bramstoelinga 0:dc7bdd0a1b32 351 E2 = true;
bramstoelinga 0:dc7bdd0a1b32 352 L4 = false;
bramstoelinga 0:dc7bdd0a1b32 353 setPwmBoth(0.60f);
bramstoelinga 0:dc7bdd0a1b32 354 }
bramstoelinga 0:dc7bdd0a1b32 355 autoTimer.stop();
bramstoelinga 0:dc7bdd0a1b32 356 autoTimer.reset();
bramstoelinga 0:dc7bdd0a1b32 357 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 358 break;
bramstoelinga 0:dc7bdd0a1b32 359
bramstoelinga 0:dc7bdd0a1b32 360 case Aright:
bramstoelinga 0:dc7bdd0a1b32 361 autoTimer.start();
bramstoelinga 0:dc7bdd0a1b32 362 while(autoTimer.read_ms() < movingDelay)
bramstoelinga 0:dc7bdd0a1b32 363 {
bramstoelinga 0:dc7bdd0a1b32 364 E1 = true;
bramstoelinga 0:dc7bdd0a1b32 365 L2 = false;
bramstoelinga 0:dc7bdd0a1b32 366 E2 = false;
bramstoelinga 0:dc7bdd0a1b32 367 L4 = false;
bramstoelinga 0:dc7bdd0a1b32 368 pwmL3 = false;
bramstoelinga 0:dc7bdd0a1b32 369 dutyCycle1 = 0.60f;
bramstoelinga 0:dc7bdd0a1b32 370 pwmL1 = dutyCycle1;
bramstoelinga 0:dc7bdd0a1b32 371 }
bramstoelinga 0:dc7bdd0a1b32 372 autoTimer.stop();
bramstoelinga 0:dc7bdd0a1b32 373 autoTimer.reset();
bramstoelinga 0:dc7bdd0a1b32 374 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 375 break;
bramstoelinga 0:dc7bdd0a1b32 376
bramstoelinga 0:dc7bdd0a1b32 377 case Aleft:
bramstoelinga 0:dc7bdd0a1b32 378 autoTimer.start();
bramstoelinga 0:dc7bdd0a1b32 379 while(autoTimer.read_ms() < movingDelay)
bramstoelinga 0:dc7bdd0a1b32 380 {
bramstoelinga 0:dc7bdd0a1b32 381 E1 = false;
bramstoelinga 0:dc7bdd0a1b32 382 L2 = false;
bramstoelinga 0:dc7bdd0a1b32 383 pwmL1 = false;
bramstoelinga 0:dc7bdd0a1b32 384 E2 = true;
bramstoelinga 0:dc7bdd0a1b32 385 L4 = false;
bramstoelinga 0:dc7bdd0a1b32 386 dutyCycle2 = 0.60f;
bramstoelinga 0:dc7bdd0a1b32 387 pwmL3 = dutyCycle2;
bramstoelinga 0:dc7bdd0a1b32 388 }
bramstoelinga 0:dc7bdd0a1b32 389 autoTimer.stop();
bramstoelinga 0:dc7bdd0a1b32 390 autoTimer.reset();
bramstoelinga 0:dc7bdd0a1b32 391 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 392 break;
bramstoelinga 0:dc7bdd0a1b32 393
bramstoelinga 0:dc7bdd0a1b32 394 case AturnRight:
bramstoelinga 0:dc7bdd0a1b32 395 autoTimer.start();
bramstoelinga 0:dc7bdd0a1b32 396 while(autoTimer.read_ms() < movingDelay)
bramstoelinga 0:dc7bdd0a1b32 397 {
bramstoelinga 0:dc7bdd0a1b32 398 E1 = true;
bramstoelinga 0:dc7bdd0a1b32 399 L2 = false;
bramstoelinga 0:dc7bdd0a1b32 400 E2 = true;
bramstoelinga 0:dc7bdd0a1b32 401 L4 = true;
bramstoelinga 0:dc7bdd0a1b32 402 setPwmBoth(0.50f);
bramstoelinga 0:dc7bdd0a1b32 403 }
bramstoelinga 0:dc7bdd0a1b32 404 autoTimer.stop();
bramstoelinga 0:dc7bdd0a1b32 405 autoTimer.reset();
bramstoelinga 0:dc7bdd0a1b32 406 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 407 break;
bramstoelinga 0:dc7bdd0a1b32 408
bramstoelinga 0:dc7bdd0a1b32 409 case AturnLeft:
bramstoelinga 0:dc7bdd0a1b32 410 autoTimer.start();
bramstoelinga 0:dc7bdd0a1b32 411 while(autoTimer.read_ms() < movingDelay)
bramstoelinga 0:dc7bdd0a1b32 412 {
bramstoelinga 0:dc7bdd0a1b32 413 E1 = true;
bramstoelinga 0:dc7bdd0a1b32 414 L2 = true;
bramstoelinga 0:dc7bdd0a1b32 415 E2 = true;
bramstoelinga 0:dc7bdd0a1b32 416 L4 = false;
bramstoelinga 0:dc7bdd0a1b32 417 setPwmBoth(0.50f);
bramstoelinga 0:dc7bdd0a1b32 418 }
bramstoelinga 0:dc7bdd0a1b32 419 autoTimer.stop();
bramstoelinga 0:dc7bdd0a1b32 420 autoTimer.reset();
bramstoelinga 0:dc7bdd0a1b32 421 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 422 break;
bramstoelinga 0:dc7bdd0a1b32 423
bramstoelinga 0:dc7bdd0a1b32 424 default:
bramstoelinga 0:dc7bdd0a1b32 425 stateMachine = wait;
bramstoelinga 0:dc7bdd0a1b32 426 break;
bramstoelinga 0:dc7bdd0a1b32 427 }
bramstoelinga 0:dc7bdd0a1b32 428 }
bramstoelinga 0:dc7bdd0a1b32 429 }