Sungwoo Han / Mbed 2 deprecated 4180_Final_halleffectSensor_Fin

4180 final project slave mbed code

Dependencies:   4DGL-uLCD-SE Motor mbed-rtos mbed

main.cpp@0:6d4adda58ec2, 2016-12-09 (annotated)

Committer:
ihansw
Date:
Fri Dec 09 15:31:23 2016 +0000
Revision:
0:6d4adda58ec2
ece4180 final project slave mbed code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ihansw 0:6d4adda58ec2 1 #include "mbed.h"
ihansw 0:6d4adda58ec2 2 #include "uLCD_4DGL.h"
ihansw 0:6d4adda58ec2 3 #include "Motor.h"
ihansw 0:6d4adda58ec2 4 #include "rtos.h"
ihansw 0:6d4adda58ec2 5 #include "stdio.h"
ihansw 0:6d4adda58ec2 6
ihansw 0:6d4adda58ec2 7 //Motors
ihansw 0:6d4adda58ec2 8 Motor m_R(p21, p5, p6); // pwm, fwd, rev _ right wheel
ihansw 0:6d4adda58ec2 9 Motor m_L(p22, p7, p8); // pwm, fwd, rev _ left wheel
ihansw 0:6d4adda58ec2 10
ihansw 0:6d4adda58ec2 11 //Serial communication between 2 mbeds
ihansw 0:6d4adda58ec2 12 Serial pc(USBTX, USBRX);
ihansw 0:6d4adda58ec2 13 Serial uart(p9, p10);
ihansw 0:6d4adda58ec2 14
ihansw 0:6d4adda58ec2 15 // Timer and halleffect sensor interrupts
ihansw 0:6d4adda58ec2 16 Timer t;
ihansw 0:6d4adda58ec2 17 InterruptIn risingEdge_L(p24);
ihansw 0:6d4adda58ec2 18 InterruptIn risingEdge_R(p25);
ihansw 0:6d4adda58ec2 19
ihansw 0:6d4adda58ec2 20 // uLCD for testing
ihansw 0:6d4adda58ec2 21 uLCD_4DGL uLCD(p28,p27,p29);
ihansw 0:6d4adda58ec2 22
ihansw 0:6d4adda58ec2 23 // LEDs for testing
ihansw 0:6d4adda58ec2 24 DigitalOut myled1(LED1);
ihansw 0:6d4adda58ec2 25 DigitalOut myled2(LED2);
ihansw 0:6d4adda58ec2 26 DigitalOut myled3(LED3);
ihansw 0:6d4adda58ec2 27 DigitalOut myled4(LED4);
ihansw 0:6d4adda58ec2 28
ihansw 0:6d4adda58ec2 29 //global variables for the halleffect sensors
ihansw 0:6d4adda58ec2 30 volatile long int count_L;
ihansw 0:6d4adda58ec2 31 volatile long int count_nr_L;
ihansw 0:6d4adda58ec2 32
ihansw 0:6d4adda58ec2 33 volatile long int count_R;
ihansw 0:6d4adda58ec2 34 volatile long int count_nr_R;
ihansw 0:6d4adda58ec2 35
ihansw 0:6d4adda58ec2 36 volatile int count;
ihansw 0:6d4adda58ec2 37
ihansw 0:6d4adda58ec2 38 volatile char ms[1];
ihansw 0:6d4adda58ec2 39 volatile char i ;
ihansw 0:6d4adda58ec2 40
ihansw 0:6d4adda58ec2 41 /*
ihansw 0:6d4adda58ec2 42 volatile int speed_h = 6;
ihansw 0:6d4adda58ec2 43 volatile int speed_m = 5;
ihansw 0:6d4adda58ec2 44 volatile int speed_l = 4;
ihansw 0:6d4adda58ec2 45 */
ihansw 0:6d4adda58ec2 46
ihansw 0:6d4adda58ec2 47
ihansw 0:6d4adda58ec2 48 void pulses_L()
ihansw 0:6d4adda58ec2 49 {
ihansw 0:6d4adda58ec2 50 if(myled2 == 1)
ihansw 0:6d4adda58ec2 51 {
ihansw 0:6d4adda58ec2 52 myled2 = 0;
ihansw 0:6d4adda58ec2 53 }
ihansw 0:6d4adda58ec2 54 else
ihansw 0:6d4adda58ec2 55 {
ihansw 0:6d4adda58ec2 56 myled2 = 1;
ihansw 0:6d4adda58ec2 57 }
ihansw 0:6d4adda58ec2 58 count_L++;
ihansw 0:6d4adda58ec2 59 count_nr_L++;
ihansw 0:6d4adda58ec2 60 }
ihansw 0:6d4adda58ec2 61
ihansw 0:6d4adda58ec2 62 void pulses_R()
ihansw 0:6d4adda58ec2 63 {
ihansw 0:6d4adda58ec2 64 if(myled4 == 1)
ihansw 0:6d4adda58ec2 65 {
ihansw 0:6d4adda58ec2 66 myled4 = 0;
ihansw 0:6d4adda58ec2 67 }
ihansw 0:6d4adda58ec2 68 else
ihansw 0:6d4adda58ec2 69 {
ihansw 0:6d4adda58ec2 70 myled4 = 1;
ihansw 0:6d4adda58ec2 71 }
ihansw 0:6d4adda58ec2 72 count_R++;
ihansw 0:6d4adda58ec2 73 count_nr_R++;
ihansw 0:6d4adda58ec2 74 }
ihansw 0:6d4adda58ec2 75
ihansw 0:6d4adda58ec2 76 int main() {
ihansw 0:6d4adda58ec2 77 uart.baud(9600);
ihansw 0:6d4adda58ec2 78
ihansw 0:6d4adda58ec2 79 double circumference = 6.5 * 3.1416; // 6 cm wheel diameter * pi
ihansw 0:6d4adda58ec2 80 long int temp_L;
ihansw 0:6d4adda58ec2 81 long int temp_R;
ihansw 0:6d4adda58ec2 82 double sA = 0.52;
ihansw 0:6d4adda58ec2 83 double sB = 0.55;
ihansw 0:6d4adda58ec2 84
ihansw 0:6d4adda58ec2 85 double dist_L;
ihansw 0:6d4adda58ec2 86 double dist_R;
ihansw 0:6d4adda58ec2 87
ihansw 0:6d4adda58ec2 88 count = 0;
ihansw 0:6d4adda58ec2 89
ihansw 0:6d4adda58ec2 90 //risingEdge.mode(Pullup);
ihansw 0:6d4adda58ec2 91 risingEdge_L.rise(&pulses_L);
ihansw 0:6d4adda58ec2 92 risingEdge_L.fall(&pulses_L);
ihansw 0:6d4adda58ec2 93
ihansw 0:6d4adda58ec2 94 risingEdge_R.rise(&pulses_R);
ihansw 0:6d4adda58ec2 95 risingEdge_R.fall(&pulses_R);
ihansw 0:6d4adda58ec2 96
ihansw 0:6d4adda58ec2 97 while(1) {
ihansw 0:6d4adda58ec2 98
ihansw 0:6d4adda58ec2 99 myled2 = 1;
ihansw 0:6d4adda58ec2 100 count++;
ihansw 0:6d4adda58ec2 101 uLCD.locate(0,1);
ihansw 0:6d4adda58ec2 102 uLCD.printf("it's Ready, %d",count);
ihansw 0:6d4adda58ec2 103
ihansw 0:6d4adda58ec2 104 if(uart.readable())
ihansw 0:6d4adda58ec2 105 {
ihansw 0:6d4adda58ec2 106 uLCD.locate(5,2);
ihansw 0:6d4adda58ec2 107 i = uart.getc();
ihansw 0:6d4adda58ec2 108 uLCD.printf("%c\n", i);
ihansw 0:6d4adda58ec2 109
ihansw 0:6d4adda58ec2 110 /////////////////////////////////////// Go forward ////////////////////////////
ihansw 0:6d4adda58ec2 111 if(i == 'f')
ihansw 0:6d4adda58ec2 112 {
ihansw 0:6d4adda58ec2 113 //speedA = speed_h;
ihansw 0:6d4adda58ec2 114
ihansw 0:6d4adda58ec2 115 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 116 uLCD.printf("Go Forward!");
ihansw 0:6d4adda58ec2 117
ihansw 0:6d4adda58ec2 118 m_L.speed(sA);
ihansw 0:6d4adda58ec2 119 m_R.speed(sB);
ihansw 0:6d4adda58ec2 120
ihansw 0:6d4adda58ec2 121 count_L = 0;
ihansw 0:6d4adda58ec2 122 count_R = 0;
ihansw 0:6d4adda58ec2 123
ihansw 0:6d4adda58ec2 124 while(!(uart.readable()))
ihansw 0:6d4adda58ec2 125 {
ihansw 0:6d4adda58ec2 126 temp_L = count_L;
ihansw 0:6d4adda58ec2 127 temp_R = count_R;
ihansw 0:6d4adda58ec2 128
ihansw 0:6d4adda58ec2 129 uLCD.locate(0,4);
ihansw 0:6d4adda58ec2 130 uLCD.printf("Count_L: %d", temp_L);
ihansw 0:6d4adda58ec2 131
ihansw 0:6d4adda58ec2 132 uLCD.locate(0,5);
ihansw 0:6d4adda58ec2 133 uLCD.printf("Count_R: %d", temp_R);
ihansw 0:6d4adda58ec2 134
ihansw 0:6d4adda58ec2 135 dist_L = (circumference/8)*((double)temp_L);
ihansw 0:6d4adda58ec2 136 dist_R = (circumference/8)*((double)temp_R);
ihansw 0:6d4adda58ec2 137
ihansw 0:6d4adda58ec2 138 uLCD.locate(0,7);
ihansw 0:6d4adda58ec2 139 uLCD.printf("distance_L: %0.2f cm", dist_L);
ihansw 0:6d4adda58ec2 140 uLCD.locate(0,8);
ihansw 0:6d4adda58ec2 141 uLCD.printf("distance_R: %0.2f cm", dist_R);
ihansw 0:6d4adda58ec2 142 }
ihansw 0:6d4adda58ec2 143
ihansw 0:6d4adda58ec2 144
ihansw 0:6d4adda58ec2 145
ihansw 0:6d4adda58ec2 146 }
ihansw 0:6d4adda58ec2 147 /////////////////////////////////////// Go Backward ////////////////////////////
ihansw 0:6d4adda58ec2 148 if(i == 'b')
ihansw 0:6d4adda58ec2 149 {
ihansw 0:6d4adda58ec2 150 //speedA = speed_h;
ihansw 0:6d4adda58ec2 151
ihansw 0:6d4adda58ec2 152 // uLCD.cls();
ihansw 0:6d4adda58ec2 153 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 154 uLCD.printf("Go Backward!");
ihansw 0:6d4adda58ec2 155 m_L.speed(-sA);
ihansw 0:6d4adda58ec2 156 m_R.speed(-sB);
ihansw 0:6d4adda58ec2 157
ihansw 0:6d4adda58ec2 158 }
ihansw 0:6d4adda58ec2 159 /////////////////////////////////////// Turn left ////////////////////////////
ihansw 0:6d4adda58ec2 160 if(i == 'l')
ihansw 0:6d4adda58ec2 161 {
ihansw 0:6d4adda58ec2 162 // speedA = speed_m;
ihansw 0:6d4adda58ec2 163
ihansw 0:6d4adda58ec2 164 uLCD.cls();
ihansw 0:6d4adda58ec2 165 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 166 uLCD.printf("Turn Left!");
ihansw 0:6d4adda58ec2 167 m_L.speed(-0.65);
ihansw 0:6d4adda58ec2 168 m_R.speed(0.67);
ihansw 0:6d4adda58ec2 169
ihansw 0:6d4adda58ec2 170
ihansw 0:6d4adda58ec2 171 count_L = 0;
ihansw 0:6d4adda58ec2 172 count_R = 0;
ihansw 0:6d4adda58ec2 173
ihansw 0:6d4adda58ec2 174 while(!(uart.readable()))
ihansw 0:6d4adda58ec2 175 {
ihansw 0:6d4adda58ec2 176 if((count_L < 4) || (count_R <4))
ihansw 0:6d4adda58ec2 177 {
ihansw 0:6d4adda58ec2 178 /*
ihansw 0:6d4adda58ec2 179 temp_L = count_L;
ihansw 0:6d4adda58ec2 180 temp_R = count_R;
ihansw 0:6d4adda58ec2 181
ihansw 0:6d4adda58ec2 182 uLCD.locate(0,4);
ihansw 0:6d4adda58ec2 183 uLCD.printf("Count_L: %d", temp_L);
ihansw 0:6d4adda58ec2 184
ihansw 0:6d4adda58ec2 185 uLCD.locate(0,5);
ihansw 0:6d4adda58ec2 186 uLCD.printf("Count_R: %d", temp_R);
ihansw 0:6d4adda58ec2 187
ihansw 0:6d4adda58ec2 188 dist_L = (circumference/8)*((double)temp_L);
ihansw 0:6d4adda58ec2 189 dist_R = (circumference/8)*((double)temp_R);
ihansw 0:6d4adda58ec2 190
ihansw 0:6d4adda58ec2 191 uLCD.locate(0,7);
ihansw 0:6d4adda58ec2 192 uLCD.printf("distance_L: %0.2f cm", dist_L);
ihansw 0:6d4adda58ec2 193 uLCD.locate(0,8);
ihansw 0:6d4adda58ec2 194 uLCD.printf("distance_R: %0.2f cm", dist_R);
ihansw 0:6d4adda58ec2 195 */
ihansw 0:6d4adda58ec2 196 }
ihansw 0:6d4adda58ec2 197 else
ihansw 0:6d4adda58ec2 198 {
ihansw 0:6d4adda58ec2 199 m_L.speed(0);
ihansw 0:6d4adda58ec2 200 m_R.speed(0);
ihansw 0:6d4adda58ec2 201 wait(0.5);
ihansw 0:6d4adda58ec2 202 //m_L.speed(sA);
ihansw 0:6d4adda58ec2 203 //m_R.speed(sB);
ihansw 0:6d4adda58ec2 204 //wait(4);
ihansw 0:6d4adda58ec2 205 }
ihansw 0:6d4adda58ec2 206 }
ihansw 0:6d4adda58ec2 207 }
ihansw 0:6d4adda58ec2 208
ihansw 0:6d4adda58ec2 209 /////////////////////////////////////// Turn Right ////////////////////////////
ihansw 0:6d4adda58ec2 210
ihansw 0:6d4adda58ec2 211 if(i == 'r')
ihansw 0:6d4adda58ec2 212 {
ihansw 0:6d4adda58ec2 213 //speedA = speed_m;
ihansw 0:6d4adda58ec2 214
ihansw 0:6d4adda58ec2 215 uLCD.cls();
ihansw 0:6d4adda58ec2 216 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 217 uLCD.printf("Turn Right!");
ihansw 0:6d4adda58ec2 218 m_L.speed(0.65);
ihansw 0:6d4adda58ec2 219 m_R.speed(-0.67);
ihansw 0:6d4adda58ec2 220
ihansw 0:6d4adda58ec2 221 count_L = 0;
ihansw 0:6d4adda58ec2 222 count_R = 0;
ihansw 0:6d4adda58ec2 223
ihansw 0:6d4adda58ec2 224 while(!(uart.readable()))
ihansw 0:6d4adda58ec2 225 {
ihansw 0:6d4adda58ec2 226 if((count_L < 5)||(count_R < 5))
ihansw 0:6d4adda58ec2 227 {
ihansw 0:6d4adda58ec2 228 /*
ihansw 0:6d4adda58ec2 229 temp_L = count_L;
ihansw 0:6d4adda58ec2 230 temp_R = count_R;
ihansw 0:6d4adda58ec2 231
ihansw 0:6d4adda58ec2 232 uLCD.locate(0,4);
ihansw 0:6d4adda58ec2 233 uLCD.printf("Count_L: %d", temp_L);
ihansw 0:6d4adda58ec2 234
ihansw 0:6d4adda58ec2 235 uLCD.locate(0,5);
ihansw 0:6d4adda58ec2 236 uLCD.printf("Count_R: %d", temp_R);
ihansw 0:6d4adda58ec2 237
ihansw 0:6d4adda58ec2 238 dist_L = (circumference/8)*((double)temp_L);
ihansw 0:6d4adda58ec2 239 dist_R = (circumference/8)*((double)temp_R);
ihansw 0:6d4adda58ec2 240
ihansw 0:6d4adda58ec2 241 uLCD.locate(0,7);
ihansw 0:6d4adda58ec2 242 uLCD.printf("distance_L: %0.2f cm", dist_L);
ihansw 0:6d4adda58ec2 243 uLCD.locate(0,8);
ihansw 0:6d4adda58ec2 244 uLCD.printf("distance_R: %0.2f cm", dist_R);
ihansw 0:6d4adda58ec2 245 */
ihansw 0:6d4adda58ec2 246 }
ihansw 0:6d4adda58ec2 247 else
ihansw 0:6d4adda58ec2 248 {
ihansw 0:6d4adda58ec2 249 m_L.speed(0);
ihansw 0:6d4adda58ec2 250 m_R.speed(0);
ihansw 0:6d4adda58ec2 251 wait(0.5);
ihansw 0:6d4adda58ec2 252 //m_L.speed(sA);
ihansw 0:6d4adda58ec2 253 //m_R.speed(sB);
ihansw 0:6d4adda58ec2 254 //wait(4);
ihansw 0:6d4adda58ec2 255 }
ihansw 0:6d4adda58ec2 256 }
ihansw 0:6d4adda58ec2 257 }
ihansw 0:6d4adda58ec2 258
ihansw 0:6d4adda58ec2 259
ihansw 0:6d4adda58ec2 260 ////////////////////////////Tilt to the left //////////////////////////////
ihansw 0:6d4adda58ec2 261
ihansw 0:6d4adda58ec2 262 if(i == 'a')
ihansw 0:6d4adda58ec2 263 {
ihansw 0:6d4adda58ec2 264 //speedB = speedB - 0.1;
ihansw 0:6d4adda58ec2 265
ihansw 0:6d4adda58ec2 266 //uLCD.cls();
ihansw 0:6d4adda58ec2 267 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 268 uLCD.printf("Tilt to the left!");
ihansw 0:6d4adda58ec2 269
ihansw 0:6d4adda58ec2 270 myled1 = 0;
ihansw 0:6d4adda58ec2 271 myled2 = 1;
ihansw 0:6d4adda58ec2 272 myled3 = 0;
ihansw 0:6d4adda58ec2 273 myled4 = 0;
ihansw 0:6d4adda58ec2 274
ihansw 0:6d4adda58ec2 275 m_L.speed(sA-0.04);
ihansw 0:6d4adda58ec2 276 m_R.speed(sB+0.02);
ihansw 0:6d4adda58ec2 277
ihansw 0:6d4adda58ec2 278 count_L = 0;
ihansw 0:6d4adda58ec2 279 count_R = 0;
ihansw 0:6d4adda58ec2 280
ihansw 0:6d4adda58ec2 281
ihansw 0:6d4adda58ec2 282 }
ihansw 0:6d4adda58ec2 283
ihansw 0:6d4adda58ec2 284 ///////////////////////////Tilt to the right///////////////////////////
ihansw 0:6d4adda58ec2 285
ihansw 0:6d4adda58ec2 286 if(i == 'z')
ihansw 0:6d4adda58ec2 287 {
ihansw 0:6d4adda58ec2 288
ihansw 0:6d4adda58ec2 289 //speedB = speedB - 0.1;
ihansw 0:6d4adda58ec2 290
ihansw 0:6d4adda58ec2 291 //uLCD.cls();
ihansw 0:6d4adda58ec2 292 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 293 uLCD.printf("Tilt to the right!");
ihansw 0:6d4adda58ec2 294 m_L.speed(sA+0.00);
ihansw 0:6d4adda58ec2 295 m_R.speed(sB-0.05);
ihansw 0:6d4adda58ec2 296
ihansw 0:6d4adda58ec2 297 myled1 = 0;
ihansw 0:6d4adda58ec2 298 myled2 = 0;
ihansw 0:6d4adda58ec2 299 myled3 = 0;
ihansw 0:6d4adda58ec2 300 myled4 = 1;
ihansw 0:6d4adda58ec2 301
ihansw 0:6d4adda58ec2 302 count_L = 0;
ihansw 0:6d4adda58ec2 303 count_R = 0;
ihansw 0:6d4adda58ec2 304
ihansw 0:6d4adda58ec2 305 }
ihansw 0:6d4adda58ec2 306
ihansw 0:6d4adda58ec2 307
ihansw 0:6d4adda58ec2 308 ////////////////Tilt angle to the left(CCW)///////////////
ihansw 0:6d4adda58ec2 309
ihansw 0:6d4adda58ec2 310 if(i == 'g')
ihansw 0:6d4adda58ec2 311 {
ihansw 0:6d4adda58ec2 312
ihansw 0:6d4adda58ec2 313 //uLCD.cls();
ihansw 0:6d4adda58ec2 314 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 315 uLCD.printf("Turn CCW a bit more!");
ihansw 0:6d4adda58ec2 316
ihansw 0:6d4adda58ec2 317 m_L.speed(0);
ihansw 0:6d4adda58ec2 318 m_R.speed(0.8);
ihansw 0:6d4adda58ec2 319 wait(0.1);
ihansw 0:6d4adda58ec2 320 m_L.speed(0);
ihansw 0:6d4adda58ec2 321 m_R.speed(0);
ihansw 0:6d4adda58ec2 322
ihansw 0:6d4adda58ec2 323
ihansw 0:6d4adda58ec2 324 }
ihansw 0:6d4adda58ec2 325
ihansw 0:6d4adda58ec2 326
ihansw 0:6d4adda58ec2 327 //////////////////Tilt angle to the right (CW)////////////////////////
ihansw 0:6d4adda58ec2 328
ihansw 0:6d4adda58ec2 329 if(i == 'h')
ihansw 0:6d4adda58ec2 330 {
ihansw 0:6d4adda58ec2 331 //uLCD.cls();
ihansw 0:6d4adda58ec2 332 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 333 uLCD.printf("Turn CW a bit more!");
ihansw 0:6d4adda58ec2 334
ihansw 0:6d4adda58ec2 335 m_L.speed(0.8);
ihansw 0:6d4adda58ec2 336 m_R.speed(0);
ihansw 0:6d4adda58ec2 337 wait(0.1);
ihansw 0:6d4adda58ec2 338 m_L.speed(0);
ihansw 0:6d4adda58ec2 339 m_R.speed(0);
ihansw 0:6d4adda58ec2 340
ihansw 0:6d4adda58ec2 341
ihansw 0:6d4adda58ec2 342
ihansw 0:6d4adda58ec2 343 }
ihansw 0:6d4adda58ec2 344
ihansw 0:6d4adda58ec2 345 ///////////////////////////Parked ///////////////////////////
ihansw 0:6d4adda58ec2 346 if(i == 'p')
ihansw 0:6d4adda58ec2 347 {
ihansw 0:6d4adda58ec2 348 uLCD.cls();
ihansw 0:6d4adda58ec2 349 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 350 uLCD.printf("stoped");
ihansw 0:6d4adda58ec2 351 wait(0.5);
ihansw 0:6d4adda58ec2 352 m_L.speed(0);
ihansw 0:6d4adda58ec2 353 m_R.speed(0);
ihansw 0:6d4adda58ec2 354
ihansw 0:6d4adda58ec2 355 count_L = 0;
ihansw 0:6d4adda58ec2 356 count_R = 0;
ihansw 0:6d4adda58ec2 357
ihansw 0:6d4adda58ec2 358 }
ihansw 0:6d4adda58ec2 359
ihansw 0:6d4adda58ec2 360 /////////////////////////////////////// Parking #1, On the left ////////////////////////////
ihansw 0:6d4adda58ec2 361 if(i == 'w')
ihansw 0:6d4adda58ec2 362 {
ihansw 0:6d4adda58ec2 363 uLCD.cls();
ihansw 0:6d4adda58ec2 364 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 365 uLCD.printf("Parking on left! Front Park!");
ihansw 0:6d4adda58ec2 366
ihansw 0:6d4adda58ec2 367 m_L.speed(sA);
ihansw 0:6d4adda58ec2 368 m_R.speed(sB);
ihansw 0:6d4adda58ec2 369 wait(0);
ihansw 0:6d4adda58ec2 370
ihansw 0:6d4adda58ec2 371 m_L.speed(0);
ihansw 0:6d4adda58ec2 372 m_R.speed(0);
ihansw 0:6d4adda58ec2 373 wait(1);
ihansw 0:6d4adda58ec2 374 count_L = 0;
ihansw 0:6d4adda58ec2 375 count_R = 0;
ihansw 0:6d4adda58ec2 376
ihansw 0:6d4adda58ec2 377 m_L.speed(-0.6);
ihansw 0:6d4adda58ec2 378 m_R.speed(0.65);
ihansw 0:6d4adda58ec2 379
ihansw 0:6d4adda58ec2 380 // Step 1: Turn left 90 degrees
ihansw 0:6d4adda58ec2 381 while((count_L < 5)||(count_R < 5))
ihansw 0:6d4adda58ec2 382 {
ihansw 0:6d4adda58ec2 383
ihansw 0:6d4adda58ec2 384 }
ihansw 0:6d4adda58ec2 385 m_L.speed(0);
ihansw 0:6d4adda58ec2 386 m_R.speed(0);
ihansw 0:6d4adda58ec2 387 wait(.5);
ihansw 0:6d4adda58ec2 388 count_L = 0;
ihansw 0:6d4adda58ec2 389 count_R = 0;
ihansw 0:6d4adda58ec2 390
ihansw 0:6d4adda58ec2 391
ihansw 0:6d4adda58ec2 392 m_L.speed(0);
ihansw 0:6d4adda58ec2 393 m_R.speed(0);
ihansw 0:6d4adda58ec2 394 count_L = 0;
ihansw 0:6d4adda58ec2 395 count_R = 0;
ihansw 0:6d4adda58ec2 396
ihansw 0:6d4adda58ec2 397
ihansw 0:6d4adda58ec2 398 }
ihansw 0:6d4adda58ec2 399 /////////////////////////////////////// Parking #2, On the right ////////////////////////////
ihansw 0:6d4adda58ec2 400 if(i == 'x')
ihansw 0:6d4adda58ec2 401 {
ihansw 0:6d4adda58ec2 402 uLCD.cls();
ihansw 0:6d4adda58ec2 403 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 404 uLCD.printf("Parking on Right! Front Park!");
ihansw 0:6d4adda58ec2 405
ihansw 0:6d4adda58ec2 406 m_L.speed(sA);
ihansw 0:6d4adda58ec2 407 m_R.speed(sB);
ihansw 0:6d4adda58ec2 408 wait(0);
ihansw 0:6d4adda58ec2 409
ihansw 0:6d4adda58ec2 410 m_L.speed(0);
ihansw 0:6d4adda58ec2 411 m_R.speed(0);
ihansw 0:6d4adda58ec2 412 wait(1);
ihansw 0:6d4adda58ec2 413 count_L = 0;
ihansw 0:6d4adda58ec2 414 count_R = 0;
ihansw 0:6d4adda58ec2 415
ihansw 0:6d4adda58ec2 416 m_L.speed(0.6);
ihansw 0:6d4adda58ec2 417 m_R.speed(-0.65);
ihansw 0:6d4adda58ec2 418
ihansw 0:6d4adda58ec2 419 // Step 1: Turn left 90 degrees
ihansw 0:6d4adda58ec2 420 while((count_L < 5)||(count_R < 5))
ihansw 0:6d4adda58ec2 421 {
ihansw 0:6d4adda58ec2 422
ihansw 0:6d4adda58ec2 423 }
ihansw 0:6d4adda58ec2 424 m_L.speed(0);
ihansw 0:6d4adda58ec2 425 m_R.speed(0);
ihansw 0:6d4adda58ec2 426 wait(.5);
ihansw 0:6d4adda58ec2 427 count_L = 0;
ihansw 0:6d4adda58ec2 428 count_R = 0;
ihansw 0:6d4adda58ec2 429
ihansw 0:6d4adda58ec2 430
ihansw 0:6d4adda58ec2 431 m_L.speed(0);
ihansw 0:6d4adda58ec2 432 m_R.speed(0);
ihansw 0:6d4adda58ec2 433 count_L = 0;
ihansw 0:6d4adda58ec2 434 count_R = 0;
ihansw 0:6d4adda58ec2 435
ihansw 0:6d4adda58ec2 436
ihansw 0:6d4adda58ec2 437 }
ihansw 0:6d4adda58ec2 438
ihansw 0:6d4adda58ec2 439
ihansw 0:6d4adda58ec2 440 }
ihansw 0:6d4adda58ec2 441
ihansw 0:6d4adda58ec2 442 else
ihansw 0:6d4adda58ec2 443 {
ihansw 0:6d4adda58ec2 444 myled1 = 0;
ihansw 0:6d4adda58ec2 445 }
ihansw 0:6d4adda58ec2 446
ihansw 0:6d4adda58ec2 447
ihansw 0:6d4adda58ec2 448 /*
ihansw 0:6d4adda58ec2 449
ihansw 0:6d4adda58ec2 450 t.reset();
ihansw 0:6d4adda58ec2 451 t.start();
ihansw 0:6d4adda58ec2 452 count_L = 0;
ihansw 0:6d4adda58ec2 453 count_R = 0;
ihansw 0:6d4adda58ec2 454
ihansw 0:6d4adda58ec2 455 while(t.read_ms() < 251) { ; }
ihansw 0:6d4adda58ec2 456
ihansw 0:6d4adda58ec2 457 t.stop();
ihansw 0:6d4adda58ec2 458
ihansw 0:6d4adda58ec2 459 long int temp_L = count_L;
ihansw 0:6d4adda58ec2 460 long int temp_R = count_R;
ihansw 0:6d4adda58ec2 461
ihansw 0:6d4adda58ec2 462 uLCD.locate(0,1);
ihansw 0:6d4adda58ec2 463 uLCD.printf("Count_L: %d", temp_L);
ihansw 0:6d4adda58ec2 464
ihansw 0:6d4adda58ec2 465 uLCD.locate(0,8);
ihansw 0:6d4adda58ec2 466 uLCD.printf("Count_R: %d", temp_R);
ihansw 0:6d4adda58ec2 467
ihansw 0:6d4adda58ec2 468 double circumference = 0.065 * 3.1416; // 6 cm wheel diameter * pi
ihansw 0:6d4adda58ec2 469
ihansw 0:6d4adda58ec2 470 double rev_L = (double)(temp_L/4);
ihansw 0:6d4adda58ec2 471 double rpm_L = ((double)temp_L)*15;
ihansw 0:6d4adda58ec2 472 double speed_L = (circumference * ((double)temp_L))/4;
ihansw 0:6d4adda58ec2 473
ihansw 0:6d4adda58ec2 474 double rev_R = (double)(temp_R/4);
ihansw 0:6d4adda58ec2 475 double rpm_R = ((double)temp_R)*15;
ihansw 0:6d4adda58ec2 476 double speed_R = (circumference * ((double)temp_R))/4;
ihansw 0:6d4adda58ec2 477
ihansw 0:6d4adda58ec2 478
ihansw 0:6d4adda58ec2 479 uLCD.locate(0,3);
ihansw 0:6d4adda58ec2 480 uLCD.printf("L: %0.2f RPM", rpm_L);
ihansw 0:6d4adda58ec2 481
ihansw 0:6d4adda58ec2 482 uLCD.locate(0,4);
ihansw 0:6d4adda58ec2 483 uLCD.printf("L: speed: %f m/s", speed_L);
ihansw 0:6d4adda58ec2 484
ihansw 0:6d4adda58ec2 485 uLCD.locate(0,2);
ihansw 0:6d4adda58ec2 486 uLCD.printf("Ct_nr_L: %d", (count_nr_L/4));
ihansw 0:6d4adda58ec2 487
ihansw 0:6d4adda58ec2 488 uLCD.locate(0,10);
ihansw 0:6d4adda58ec2 489 uLCD.printf("R: %0.2f RPM", rpm_R);
ihansw 0:6d4adda58ec2 490
ihansw 0:6d4adda58ec2 491 uLCD.locate(0,11);
ihansw 0:6d4adda58ec2 492 uLCD.printf("R: speed: %f m/s", speed_R);
ihansw 0:6d4adda58ec2 493
ihansw 0:6d4adda58ec2 494 uLCD.locate(0,9);
ihansw 0:6d4adda58ec2 495 uLCD.printf("Ct_nr_R: %d", (count_nr_R/4));
ihansw 0:6d4adda58ec2 496
ihansw 0:6d4adda58ec2 497 */
ihansw 0:6d4adda58ec2 498
ihansw 0:6d4adda58ec2 499 //pc.putc(0xA);
ihansw 0:6d4adda58ec2 500 //pc.putc(0xD);
ihansw 0:6d4adda58ec2 501 }
ihansw 0:6d4adda58ec2 502 }