EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@18:eab7b0e89398, 2014-12-01 (annotated)

Committer:
alecguertin
Date:
Mon Dec 01 01:02:42 2014 +0000
Revision:
18:eab7b0e89398
Parent:
17:c72c092fcdf7
Child:
19:47759cf4f9b9
added logic to calculate angle to drive at -- untested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsaristo 1:7e0243c27ecb 1 /**
lsaristo 1:7e0243c27ecb 2 * @file driver.c
lsaristo 1:7e0243c27ecb 3 * @brief Basic driver program for our robot's controller logic.
lsaristo 1:7e0243c27ecb 4 *
lsaristo 1:7e0243c27ecb 5 * Maybe add lots of stuff here or maybe split it off into
lsaristo 1:7e0243c27ecb 6 * multiple subfiles?
lsaristo 1:7e0243c27ecb 7 *
lsaristo 1:7e0243c27ecb 8 * @author John Wilkey
lsaristo 1:7e0243c27ecb 9 */
lsaristo 9:3a0433c391cb 10 #include "main.h"
lsaristo 10:94b068b2ce1d 11 #include "control.h"
John Wilkey 5:01882c3de2dc 12
John Wilkey 5:01882c3de2dc 13 /**
John Wilkey 5:01882c3de2dc 14 * These are global data Used externally in all other files
John Wilkey 5:01882c3de2dc 15 */
lsaristo 7:6e5cc24e1ce7 16 m3pi pi;
lsaristo 12:1aa6b8a74136 17 Timer timer;
lsaristo 9:3a0433c391cb 18
lsaristo 9:3a0433c391cb 19 //
lsaristo 9:3a0433c391cb 20 // Digital inputs to the mBed
lsaristo 10:94b068b2ce1d 21 DigitalIn start_button(p21);
lsaristo 9:3a0433c391cb 22
lsaristo 9:3a0433c391cb 23 //
lsaristo 9:3a0433c391cb 24 // Digital outputs from the mBed. Note that by default these are
lsaristo 9:3a0433c391cb 25 // used to drive the 8 LED's on the top board.
lsaristo 7:6e5cc24e1ce7 26 DigitalOut pin15(p15);
lsaristo 7:6e5cc24e1ce7 27 DigitalOut pin16(p16);
lsaristo 7:6e5cc24e1ce7 28 DigitalOut pin17(p17);
lsaristo 7:6e5cc24e1ce7 29 DigitalOut pin18(p18);
lsaristo 7:6e5cc24e1ce7 30 DigitalOut pin19(p19);
lsaristo 7:6e5cc24e1ce7 31 DigitalOut pin20(p20);
lsaristo 7:6e5cc24e1ce7 32
lsaristo 9:3a0433c391cb 33 //
lsaristo 9:3a0433c391cb 34 // mBed onboard LEDs
lsaristo 9:3a0433c391cb 35 DigitalOut oled_1(LED1);
lsaristo 9:3a0433c391cb 36 DigitalOut oled_2(LED2);
lsaristo 9:3a0433c391cb 37 DigitalOut oled_3(LED3);
lsaristo 9:3a0433c391cb 38 DigitalOut oled_4(LED4);
lsaristo 9:3a0433c391cb 39
alecguertin 15:14d4e7021125 40 /* Local File System */
alecguertin 15:14d4e7021125 41 LocalFileSystem local("local");
lsaristo 9:3a0433c391cb 42
alecguertin 17:c72c092fcdf7 43 /* Boolean for drawing/moving. */
alecguertin 17:c72c092fcdf7 44 int draw;
alecguertin 17:c72c092fcdf7 45
John Wilkey 5:01882c3de2dc 46 /**
John Wilkey 5:01882c3de2dc 47 * @brief Entry point. Main loop.
John Wilkey 5:01882c3de2dc 48 */
John Wilkey 5:01882c3de2dc 49 int main()
John Wilkey 5:01882c3de2dc 50 {
alecguertin 17:c72c092fcdf7 51 FILE *ps_file;
alecguertin 17:c72c092fcdf7 52 int instbuflen = 250;
alecguertin 17:c72c092fcdf7 53 char instbuf[instbuflen];
alecguertin 17:c72c092fcdf7 54
lsaristo 9:3a0433c391cb 55 //
lsaristo 9:3a0433c391cb 56 // Basic setup information
lsaristo 10:94b068b2ce1d 57 start_button.mode(PullUp);
lsaristo 12:1aa6b8a74136 58
lsaristo 13:070846d87d4a 59 //
lsaristo 13:070846d87d4a 60 // Drawing environment calibration.
lsaristo 12:1aa6b8a74136 61 pi.sensor_auto_calibrate();
alecguertin 15:14d4e7021125 62 //pi.backward(DRIVE_SPEED);
alecguertin 15:14d4e7021125 63 float pos = 0;
alecguertin 15:14d4e7021125 64 float over_thresh = 0.05;
alecguertin 15:14d4e7021125 65 float correction = 0.2*DRIVE_SPEED;
alecguertin 15:14d4e7021125 66
alecguertin 15:14d4e7021125 67 wait(1);
alecguertin 15:14d4e7021125 68
alecguertin 15:14d4e7021125 69 do {
alecguertin 15:14d4e7021125 70 pos = pi.line_position();
alecguertin 15:14d4e7021125 71 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 72 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 73 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 74 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 75 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 76 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 77 } else {
alecguertin 15:14d4e7021125 78 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 79 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 80 }
alecguertin 15:14d4e7021125 81 /*
alecguertin 15:14d4e7021125 82 pi.cls();
alecguertin 15:14d4e7021125 83 pi.locate(0,0);
alecguertin 15:14d4e7021125 84 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 85 */
alecguertin 17:c72c092fcdf7 86 } while (pos != -1 && pos <= 0.3);
alecguertin 15:14d4e7021125 87 pi.stop();
alecguertin 15:14d4e7021125 88 wait(1);
alecguertin 15:14d4e7021125 89 if (pos != -1) {
alecguertin 15:14d4e7021125 90 timer.start();
alecguertin 15:14d4e7021125 91 pi.forward(DRIVE_SPEED);
alecguertin 15:14d4e7021125 92 } else {
alecguertin 15:14d4e7021125 93 pi.cls();
alecguertin 15:14d4e7021125 94 pi.locate(0,0);
alecguertin 15:14d4e7021125 95 pi.printf("LP: %f",pos);
alecguertin 15:14d4e7021125 96 return 1;
alecguertin 15:14d4e7021125 97 }
alecguertin 15:14d4e7021125 98
alecguertin 15:14d4e7021125 99 wait(1);
lsaristo 12:1aa6b8a74136 100
lsaristo 12:1aa6b8a74136 101 do {
lsaristo 12:1aa6b8a74136 102 pos = pi.line_position();
lsaristo 13:070846d87d4a 103 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 104 pi.right_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 105 pi.left_motor(DRIVE_SPEED-correction);
lsaristo 13:070846d87d4a 106 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 107 pi.left_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 108 pi.right_motor(DRIVE_SPEED-correction);
lsaristo 13:070846d87d4a 109 } else {
alecguertin 15:14d4e7021125 110 pi.right_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 111 pi.left_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 112 }
alecguertin 15:14d4e7021125 113 pi.cls();
alecguertin 15:14d4e7021125 114 pi.locate(0,0);
alecguertin 15:14d4e7021125 115 pi.printf("Pos: %f", pos);
alecguertin 15:14d4e7021125 116 } while(pos != -1 && pos <= 0.3);
alecguertin 15:14d4e7021125 117 pi.stop();
alecguertin 15:14d4e7021125 118 if(pos != -1) {
alecguertin 15:14d4e7021125 119 oled_1 = 1;
alecguertin 15:14d4e7021125 120 timer.stop();
alecguertin 15:14d4e7021125 121 pi.printf("T: %d", timer.read_ms());
alecguertin 15:14d4e7021125 122 } else {
alecguertin 15:14d4e7021125 123 pi.cls();
alecguertin 15:14d4e7021125 124 pi.locate(0,0);
alecguertin 15:14d4e7021125 125 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 126 return 1;
alecguertin 15:14d4e7021125 127 }
alecguertin 15:14d4e7021125 128 pi.right(TURN_SPEED);
alecguertin 15:14d4e7021125 129 wait(0.5);
alecguertin 15:14d4e7021125 130 pi.stop();
alecguertin 15:14d4e7021125 131 do {
alecguertin 15:14d4e7021125 132 pos = pi.line_position();
alecguertin 15:14d4e7021125 133 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 134 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 135 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 136 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 137 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 138 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 139 } else {
alecguertin 15:14d4e7021125 140 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 141 pi.left_motor(-DRIVE_SPEED);
lsaristo 13:070846d87d4a 142 }
lsaristo 12:1aa6b8a74136 143 pi.cls();
lsaristo 12:1aa6b8a74136 144 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 145 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 146 } while (pos != -1 && pos > -0.3);
alecguertin 15:14d4e7021125 147 pi.stop();
alecguertin 15:14d4e7021125 148 if(pos != -1) {
alecguertin 15:14d4e7021125 149 oled_1 = 1;
alecguertin 15:14d4e7021125 150 timer.stop();
alecguertin 15:14d4e7021125 151 pi.printf("T: %d", timer.read_ms());
lsaristo 12:1aa6b8a74136 152 } else {
lsaristo 12:1aa6b8a74136 153 pi.cls();
lsaristo 12:1aa6b8a74136 154 pi.locate(0,0);
alecguertin 15:14d4e7021125 155 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 156 return 1;
lsaristo 12:1aa6b8a74136 157 }
John Wilkey 6:00b7198f0b51 158
lsaristo 12:1aa6b8a74136 159 while(pi.line_position() == 1);
lsaristo 12:1aa6b8a74136 160 do {
lsaristo 12:1aa6b8a74136 161 pos = pi.line_position();
lsaristo 13:070846d87d4a 162 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 163 pi.right_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 164 pi.left_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 165 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 166 pi.left_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 167 pi.right_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 168 } else {
lsaristo 13:070846d87d4a 169 pi.forward(DRIVE_SPEED);
lsaristo 13:070846d87d4a 170 }
lsaristo 12:1aa6b8a74136 171 pi.cls();
lsaristo 12:1aa6b8a74136 172 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 173 pi.printf("Pos: %f", pos);
lsaristo 12:1aa6b8a74136 174 } while(pos != -1 && pos != 1);
lsaristo 12:1aa6b8a74136 175 if(pos == 1) {
lsaristo 12:1aa6b8a74136 176 oled_1 = 1;
lsaristo 12:1aa6b8a74136 177 timer.stop();
lsaristo 12:1aa6b8a74136 178 pi.stop();
lsaristo 12:1aa6b8a74136 179 } else {
lsaristo 12:1aa6b8a74136 180 pi.stop();
lsaristo 12:1aa6b8a74136 181 pi.cls();
lsaristo 12:1aa6b8a74136 182 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 183 pi.printf("LP:%f", pos);
lsaristo 12:1aa6b8a74136 184 while(1);
lsaristo 12:1aa6b8a74136 185 }
lsaristo 14:41fa8b95a9ab 186 // If we got here, calibration is complete.
lsaristo 12:1aa6b8a74136 187
John Wilkey 6:00b7198f0b51 188 //
lsaristo 9:3a0433c391cb 189 // Main program loop.
lsaristo 12:1aa6b8a74136 190 // robot_loop();
alecguertin 17:c72c092fcdf7 191 size_t bytes_read = 0;
alecguertin 18:eab7b0e89398 192 int err, x, y, last_x, last_y, delta_x, delta_y;
alecguertin 17:c72c092fcdf7 193 int offset = 0;
alecguertin 17:c72c092fcdf7 194 char *cur, *next;
alecguertin 18:eab7b0e89398 195 float angle, theta;
alecguertin 18:eab7b0e89398 196 angle = 0;
alecguertin 18:eab7b0e89398 197 theta = 0;
alecguertin 15:14d4e7021125 198 ps_file = fopen("/local/test.ps", "r");
alecguertin 17:c72c092fcdf7 199 /* PS parsing loop. */
alecguertin 17:c72c092fcdf7 200 while (1) {
alecguertin 17:c72c092fcdf7 201 memset(instbuf+offset, 0, instbuflen-offset);
alecguertin 17:c72c092fcdf7 202 bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
alecguertin 17:c72c092fcdf7 203 if (bytes_read == 0) {
alecguertin 17:c72c092fcdf7 204 break;
alecguertin 17:c72c092fcdf7 205 }
alecguertin 17:c72c092fcdf7 206 cur = instbuf;
alecguertin 17:c72c092fcdf7 207 while (cur[0] != '\0') {
alecguertin 17:c72c092fcdf7 208 err = retrieve_inst(instbuf, &x, &y, &draw);
alecguertin 18:eab7b0e89398 209 delta_x = x-last_x;
alecguertin 18:eab7b0e89398 210 delta_y = y-last_y;
alecguertin 18:eab7b0e89398 211 theta = tan(delta_x/delta_y);
alecguertin 18:eab7b0e89398 212 theta *= 57.2957795;
alecguertin 18:eab7b0e89398 213 delta_a = theta-angle;
alecguertin 17:c72c092fcdf7 214 last_x = x;
alecguertin 17:c72c092fcdf7 215 last_y = y;
alecguertin 17:c72c092fcdf7 216 next = strchr(cur, '\n');
alecguertin 17:c72c092fcdf7 217 if (next == NULL) {
alecguertin 17:c72c092fcdf7 218 break;
alecguertin 17:c72c092fcdf7 219 }
alecguertin 17:c72c092fcdf7 220 cur = next+1;
alecguertin 17:c72c092fcdf7 221 }
alecguertin 17:c72c092fcdf7 222 offset = instbuf+instbuflen-cur;
alecguertin 17:c72c092fcdf7 223 memcpy(instbuf, cur, offset);
alecguertin 15:14d4e7021125 224 }
lsaristo 9:3a0433c391cb 225 //
lsaristo 9:3a0433c391cb 226 // We should never reach this point!
alecguertin 18:eab7b0e89398 227 //
alecguertin 18:eab7b0e89398 228 // Yes, we should...
alecguertin 15:14d4e7021125 229 return 0;
John Wilkey 5:01882c3de2dc 230 }
lsaristo 8:12d780f7443e 231
alecguertin 17:c72c092fcdf7 232 int retrieve_inst(char *buf, int *x, int *y, int *draw)
alecguertin 17:c72c092fcdf7 233 {
alecguertin 17:c72c092fcdf7 234 int matches;
alecguertin 17:c72c092fcdf7 235 char *srch;
alecguertin 17:c72c092fcdf7 236 matches = sscanf(buf, "%d %d", x, y);
alecguertin 17:c72c092fcdf7 237 if (matches != 2) {
alecguertin 17:c72c092fcdf7 238 return 0;
alecguertin 17:c72c092fcdf7 239 }
alecguertin 17:c72c092fcdf7 240 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 241 srch++;
alecguertin 17:c72c092fcdf7 242 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 243 srch++;
alecguertin 17:c72c092fcdf7 244 if (srch[0] == 'm') {
alecguertin 17:c72c092fcdf7 245 *draw = 0;
alecguertin 17:c72c092fcdf7 246 } else if (srch[0] == 'd') {
alecguertin 17:c72c092fcdf7 247 *draw = 1;
alecguertin 17:c72c092fcdf7 248 } else {
alecguertin 17:c72c092fcdf7 249 return 0;
alecguertin 17:c72c092fcdf7 250 }
alecguertin 17:c72c092fcdf7 251 return 1;
alecguertin 17:c72c092fcdf7 252 }
alecguertin 17:c72c092fcdf7 253
lsaristo 9:3a0433c391cb 254 int forward(int amt)
lsaristo 8:12d780f7443e 255 {
lsaristo 12:1aa6b8a74136 256 Timer t;
lsaristo 12:1aa6b8a74136 257 t.start();
lsaristo 10:94b068b2ce1d 258 oled_2 = 1;
lsaristo 9:3a0433c391cb 259 pi.locate(0,0);
lsaristo 9:3a0433c391cb 260 pi.printf("Fwd %d", amt);
lsaristo 9:3a0433c391cb 261 pi.forward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 262 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 263 t.stop();
lsaristo 10:94b068b2ce1d 264 oled_2 = 0;
lsaristo 12:1aa6b8a74136 265 pi.stop();
lsaristo 8:12d780f7443e 266 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 267 }
lsaristo 8:12d780f7443e 268
lsaristo 9:3a0433c391cb 269 int backward(int amt)
lsaristo 8:12d780f7443e 270 {
lsaristo 12:1aa6b8a74136 271 Timer t;
lsaristo 10:94b068b2ce1d 272 oled_3 = 1;
lsaristo 9:3a0433c391cb 273 pi.locate(0,0);
lsaristo 9:3a0433c391cb 274 pi.printf("Back %d", amt);
lsaristo 9:3a0433c391cb 275 pi.backward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 276 t.start();
lsaristo 12:1aa6b8a74136 277 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 278 t.stop();
lsaristo 10:94b068b2ce1d 279 oled_3 = 0;
lsaristo 12:1aa6b8a74136 280 pi.stop();
lsaristo 8:12d780f7443e 281 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 282 }
lsaristo 8:12d780f7443e 283
lsaristo 8:12d780f7443e 284 int right(float deg)
lsaristo 8:12d780f7443e 285 {
lsaristo 12:1aa6b8a74136 286 Timer t;
lsaristo 10:94b068b2ce1d 287 oled_4 = 1;
lsaristo 9:3a0433c391cb 288 pi.locate(0,0);
lsaristo 9:3a0433c391cb 289 pi.printf("Right %f", deg);
lsaristo 8:12d780f7443e 290 pi.right(TURN_SPEED);
lsaristo 12:1aa6b8a74136 291 t.start();
lsaristo 12:1aa6b8a74136 292 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 293 t.stop();
lsaristo 10:94b068b2ce1d 294 oled_4 = 0;
lsaristo 12:1aa6b8a74136 295 pi.stop();
lsaristo 8:12d780f7443e 296 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 297 }
lsaristo 8:12d780f7443e 298
lsaristo 8:12d780f7443e 299 int left(float deg)
lsaristo 8:12d780f7443e 300 {
lsaristo 12:1aa6b8a74136 301 Timer t;
lsaristo 10:94b068b2ce1d 302 oled_4 = 1;
lsaristo 10:94b068b2ce1d 303 oled_2 = 1;
lsaristo 9:3a0433c391cb 304 pi.locate(0,0);
lsaristo 9:3a0433c391cb 305 pi.printf("Left %f", deg);
lsaristo 8:12d780f7443e 306 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 307 t.start();
lsaristo 12:1aa6b8a74136 308 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 309 t.stop();
lsaristo 10:94b068b2ce1d 310 oled_4 = 0;
lsaristo 10:94b068b2ce1d 311 oled_2 = 0;
lsaristo 12:1aa6b8a74136 312 pi.stop();
lsaristo 8:12d780f7443e 313 return EXIT_SUCCESS;
lsaristo 10:94b068b2ce1d 314 }
lsaristo 10:94b068b2ce1d 315
lsaristo 10:94b068b2ce1d 316 void pen_down()
lsaristo 10:94b068b2ce1d 317 {
lsaristo 10:94b068b2ce1d 318 oled_1 = 1;
lsaristo 10:94b068b2ce1d 319 }
lsaristo 10:94b068b2ce1d 320
lsaristo 10:94b068b2ce1d 321 void pen_up()
lsaristo 10:94b068b2ce1d 322 {
lsaristo 10:94b068b2ce1d 323 oled_1 = 0;
lsaristo 9:3a0433c391cb 324 }