EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@30:3211e2962441, 2014-12-09 (annotated)

Committer:
lsaristo
Date:
Tue Dec 09 01:56:03 2014 +0000
Revision:
30:3211e2962441
Parent:
29:459ff10d2a07
Child:
31:1e950ee04481
Removed extra centering 'wiggle' at calibration and shortened interval between instructions.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsaristo 1:7e0243c27ecb 1 /**
lsaristo 1:7e0243c27ecb 2 * @file driver.c
lsaristo 29:459ff10d2a07 3 * @brief Primary control logic for robot.
lsaristo 1:7e0243c27ecb 4 *
lsaristo 29:459ff10d2a07 5 * @author John Wilkey
lsaristo 29:459ff10d2a07 6 * @author Alec Guertin
lsaristo 29:459ff10d2a07 7 * @author Chester Chu
lsaristo 1:7e0243c27ecb 8 */
lsaristo 9:3a0433c391cb 9 #include "main.h"
John Wilkey 5:01882c3de2dc 10
lsaristo 29:459ff10d2a07 11 m3pi pi; /**< m3pi Object for all control operations. */
lsaristo 29:459ff10d2a07 12 Timer timer; /**< Timer used to wait in timerWait. */
lsaristo 29:459ff10d2a07 13 DigitalIn start_button(p21); /**< Pi start button input on mBed pin 32 */
lsaristo 29:459ff10d2a07 14 int draw; /**< Boolean for drawing/moving. */
lsaristo 9:3a0433c391cb 15
lsaristo 29:459ff10d2a07 16 /* 4 mBed onboard LEDs */
lsaristo 29:459ff10d2a07 17 DigitalOut oled_1(LED1), oled_2(LED2), oled_3(LED3), oled_4(LED4);
lsaristo 7:6e5cc24e1ce7 18
lsaristo 29:459ff10d2a07 19 /* 8 LEDs driven from mBed digital outs. */
lsaristo 29:459ff10d2a07 20 DigitalOut pin13(p13), pin14(p14), pin15(p15), pin16(p16),
lsaristo 29:459ff10d2a07 21 pin17(p17), pin18(p18), pin19(p19), pin20(p20);
lsaristo 9:3a0433c391cb 22
alecguertin 15:14d4e7021125 23 /* Local File System */
alecguertin 15:14d4e7021125 24 LocalFileSystem local("local");
lsaristo 9:3a0433c391cb 25
lsaristo 29:459ff10d2a07 26 /** @brief Entry point. Main loop. */
John Wilkey 5:01882c3de2dc 27 int main()
John Wilkey 5:01882c3de2dc 28 {
lsaristo 29:459ff10d2a07 29 FILE *ps_file;
lsaristo 29:459ff10d2a07 30 float cal_time;
lsaristo 29:459ff10d2a07 31 float pos = 0;
lsaristo 29:459ff10d2a07 32 float over_thresh = 0.05;
lsaristo 29:459ff10d2a07 33 float correction = 0.2*DRIVE_SPEED;
lsaristo 29:459ff10d2a07 34 int instbuflen = 250;
lsaristo 29:459ff10d2a07 35 char instbuf[instbuflen];
lsaristo 29:459ff10d2a07 36
lsaristo 10:94b068b2ce1d 37 start_button.mode(PullUp);
lsaristo 21:0c80a5d89ea3 38 pi.cls();
lsaristo 21:0c80a5d89ea3 39 pi.locate(0,0);
lsaristo 29:459ff10d2a07 40 pi.printf("Ready");
lsaristo 21:0c80a5d89ea3 41 pi.locate(0,1);
lsaristo 21:0c80a5d89ea3 42 pi.printf("%f mV", pi.battery());
lsaristo 21:0c80a5d89ea3 43 wait(.5);
lsaristo 12:1aa6b8a74136 44 pi.sensor_auto_calibrate();
lsaristo 29:459ff10d2a07 45
alecguertin 15:14d4e7021125 46 wait(1);
alecguertin 26:a74edc4c6acb 47
lsaristo 21:0c80a5d89ea3 48 do {
lsaristo 21:0c80a5d89ea3 49 pos = pi.line_position();
lsaristo 21:0c80a5d89ea3 50 if(pos > over_thresh) {
lsaristo 21:0c80a5d89ea3 51 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 52 pi.left_motor(CAL_SPEED-correction);
lsaristo 21:0c80a5d89ea3 53 } else if(pos < -over_thresh) {
lsaristo 21:0c80a5d89ea3 54 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 55 pi.right_motor(CAL_SPEED-correction);
lsaristo 21:0c80a5d89ea3 56 } else {
lsaristo 21:0c80a5d89ea3 57 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 58 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 59 }
lsaristo 21:0c80a5d89ea3 60 pi.cls();
lsaristo 21:0c80a5d89ea3 61 pi.locate(0,0);
lsaristo 21:0c80a5d89ea3 62 pi.printf("Pos: %f", pos);
lsaristo 21:0c80a5d89ea3 63 } while(pos != -1 && pos > -0.3);
lsaristo 21:0c80a5d89ea3 64 pi.stop();
lsaristo 21:0c80a5d89ea3 65 if(pos != -1) {
lsaristo 21:0c80a5d89ea3 66 timer.stop();
lsaristo 21:0c80a5d89ea3 67 } else {
lsaristo 21:0c80a5d89ea3 68 return 1;
lsaristo 21:0c80a5d89ea3 69 }
lsaristo 21:0c80a5d89ea3 70 right(180);
alecguertin 22:46b9d9b2e35c 71 Timer caltimer;
alecguertin 22:46b9d9b2e35c 72 caltimer.start();
lsaristo 12:1aa6b8a74136 73 do {
lsaristo 12:1aa6b8a74136 74 pos = pi.line_position();
lsaristo 13:070846d87d4a 75 if(pos > over_thresh) {
lsaristo 21:0c80a5d89ea3 76 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 77 pi.left_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 78 } else if(pos < -over_thresh) {
lsaristo 21:0c80a5d89ea3 79 pi.left_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 80 pi.right_motor(CAL_SPEED-correction);
lsaristo 13:070846d87d4a 81 } else {
lsaristo 21:0c80a5d89ea3 82 pi.right_motor(CAL_SPEED);
lsaristo 21:0c80a5d89ea3 83 pi.left_motor(CAL_SPEED);
alecguertin 15:14d4e7021125 84 }
alecguertin 15:14d4e7021125 85 pi.cls();
alecguertin 15:14d4e7021125 86 pi.locate(0,0);
alecguertin 15:14d4e7021125 87 pi.printf("Pos: %f", pos);
alecguertin 15:14d4e7021125 88 } while(pos != -1 && pos <= 0.3);
alecguertin 22:46b9d9b2e35c 89 caltimer.stop();
alecguertin 22:46b9d9b2e35c 90 cal_time = caltimer.read_ms();
alecguertin 15:14d4e7021125 91 pi.stop();
alecguertin 15:14d4e7021125 92 if(pos != -1) {
alecguertin 15:14d4e7021125 93 timer.stop();
alecguertin 15:14d4e7021125 94 } else {
alecguertin 15:14d4e7021125 95 return 1;
alecguertin 15:14d4e7021125 96 }
lsaristo 21:0c80a5d89ea3 97 right(180);
alecguertin 22:46b9d9b2e35c 98 timerWait(.5);
alecguertin 22:46b9d9b2e35c 99 while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
alecguertin 22:46b9d9b2e35c 100 pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
alecguertin 22:46b9d9b2e35c 101 pi.cls();
alecguertin 22:46b9d9b2e35c 102 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 103 pi.printf("O: %f", pos);
alecguertin 22:46b9d9b2e35c 104 }
lsaristo 30:3211e2962441 105 pi.stop();
lsaristo 30:3211e2962441 106 timerWait(0.5);
alecguertin 22:46b9d9b2e35c 107 backward(500);
lsaristo 30:3211e2962441 108
lsaristo 30:3211e2962441 109 /* Maybe uncomment this depending on the surface.
alecguertin 22:46b9d9b2e35c 110 timerWait(.5);
alecguertin 22:46b9d9b2e35c 111 while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
alecguertin 22:46b9d9b2e35c 112 pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
alecguertin 22:46b9d9b2e35c 113 pi.cls();
alecguertin 22:46b9d9b2e35c 114 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 115 pi.printf("O: %f", pos);
alecguertin 22:46b9d9b2e35c 116 }
alecguertin 22:46b9d9b2e35c 117 pi.stop();
lsaristo 30:3211e2962441 118 */
lsaristo 30:3211e2962441 119
lsaristo 30:3211e2962441 120 timerWait(.6);
alecguertin 26:a74edc4c6acb 121
lsaristo 9:3a0433c391cb 122 // Main program loop.
alecguertin 17:c72c092fcdf7 123 size_t bytes_read = 0;
alecguertin 22:46b9d9b2e35c 124 int err, x, y, last_x, last_y, delta_x, delta_y;
alecguertin 22:46b9d9b2e35c 125 float delta_a;
alecguertin 19:47759cf4f9b9 126 int dim_x, dim_y;
alecguertin 17:c72c092fcdf7 127 int offset = 0;
alecguertin 17:c72c092fcdf7 128 char *cur, *next;
alecguertin 19:47759cf4f9b9 129 float angle;
alecguertin 19:47759cf4f9b9 130 double dist, theta;
alecguertin 18:eab7b0e89398 131 angle = 0;
alecguertin 18:eab7b0e89398 132 theta = 0;
alecguertin 19:47759cf4f9b9 133 last_x = 0;
alecguertin 19:47759cf4f9b9 134 last_y = 0;
alecguertin 15:14d4e7021125 135 ps_file = fopen("/local/test.ps", "r");
chstrchu 20:76718145b403 136 if (ps_file == NULL) {
chstrchu 20:76718145b403 137 return 1;
chstrchu 20:76718145b403 138 }
alecguertin 25:2c7717684d09 139
alecguertin 17:c72c092fcdf7 140 /* PS parsing loop. */
alecguertin 19:47759cf4f9b9 141 memset(instbuf, 0, instbuflen);
chstrchu 20:76718145b403 142 bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
alecguertin 22:46b9d9b2e35c 143 if (bytes_read == 0) {
alecguertin 22:46b9d9b2e35c 144 return 1;
alecguertin 22:46b9d9b2e35c 145 }
alecguertin 19:47759cf4f9b9 146 err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
alecguertin 19:47759cf4f9b9 147 if (err != 2) {
chstrchu 20:76718145b403 148 pi.cls();
chstrchu 20:76718145b403 149 pi.locate(0,0);
chstrchu 20:76718145b403 150 pi.printf("sscanf1");
alecguertin 19:47759cf4f9b9 151 return 1;
alecguertin 19:47759cf4f9b9 152 }
alecguertin 19:47759cf4f9b9 153 cur = strchr(instbuf, '\n');
chstrchu 20:76718145b403 154 cur++;
alecguertin 22:46b9d9b2e35c 155 offset = instbuf+instbuflen-cur-1;
chstrchu 20:76718145b403 156 memcpy(instbuf, cur, offset);
alecguertin 17:c72c092fcdf7 157 while (1) {
lsaristo 30:3211e2962441 158 timerWait(.18);
alecguertin 17:c72c092fcdf7 159 memset(instbuf+offset, 0, instbuflen-offset);
alecguertin 17:c72c092fcdf7 160 bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
alecguertin 28:9976a94efa83 161
alecguertin 27:44a4a0abc8ee 162 if (instbuf[0] == '\0') {
alecguertin 28:9976a94efa83 163 pi.cls();
alecguertin 28:9976a94efa83 164 pi.locate(0,0);
alecguertin 28:9976a94efa83 165 pi.printf("%s", instbuf+1);
alecguertin 28:9976a94efa83 166 while(1);
alecguertin 27:44a4a0abc8ee 167 }
alecguertin 28:9976a94efa83 168
alecguertin 17:c72c092fcdf7 169 cur = instbuf;
alecguertin 22:46b9d9b2e35c 170 err = retrieve_inst(instbuf, &x, &y, &draw);
alecguertin 22:46b9d9b2e35c 171 if (err == 0) {
chstrchu 20:76718145b403 172 pi.cls();
chstrchu 20:76718145b403 173 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 174 pi.printf("noinst");
alecguertin 22:46b9d9b2e35c 175 return 1;
alecguertin 22:46b9d9b2e35c 176 }
alecguertin 22:46b9d9b2e35c 177 delta_x = x-last_x;
alecguertin 22:46b9d9b2e35c 178 delta_y = y-last_y;
alecguertin 22:46b9d9b2e35c 179
alecguertin 22:46b9d9b2e35c 180 /* Compute turn angle and turn. */
alecguertin 25:2c7717684d09 181 theta = atan(((double) delta_y)/((double) delta_x));
alecguertin 22:46b9d9b2e35c 182 theta *= 57.29;
alecguertin 27:44a4a0abc8ee 183 if (delta_x < 0 && delta_y > 0) {
alecguertin 27:44a4a0abc8ee 184 theta += 180;
alecguertin 27:44a4a0abc8ee 185 }
alecguertin 22:46b9d9b2e35c 186 delta_a = theta-angle;
alecguertin 23:e4616259a7f0 187 if (delta_x < 0 && delta_y < 0) {
alecguertin 23:e4616259a7f0 188 delta_a += 180;
alecguertin 23:e4616259a7f0 189 }
alecguertin 28:9976a94efa83 190 if (delta_a > 180) {
alecguertin 28:9976a94efa83 191 delta_a -= 360;
alecguertin 28:9976a94efa83 192 }
lsaristo 30:3211e2962441 193 if (delta_a < -180) {
lsaristo 30:3211e2962441 194 delta_a = 360 + delta_a;
lsaristo 30:3211e2962441 195 }
alecguertin 25:2c7717684d09 196 angle += delta_a;
alecguertin 28:9976a94efa83 197 if (angle >= 360) {
alecguertin 28:9976a94efa83 198 angle -= 360;
alecguertin 28:9976a94efa83 199 }
alecguertin 25:2c7717684d09 200 //pi.cls();
alecguertin 25:2c7717684d09 201 //pi.locate(0,0);
alecguertin 25:2c7717684d09 202 //pi.printf("a:%f", delta_a);
alecguertin 25:2c7717684d09 203 //timerWait(1.5);
alecguertin 25:2c7717684d09 204 left(delta_a);
alecguertin 19:47759cf4f9b9 205
alecguertin 22:46b9d9b2e35c 206 /* Put pen into position. */
alecguertin 22:46b9d9b2e35c 207 if (draw) {
alecguertin 28:9976a94efa83 208 oled_3 = 1;
alecguertin 22:46b9d9b2e35c 209 } else {
alecguertin 28:9976a94efa83 210 oled_3 = 0;
alecguertin 22:46b9d9b2e35c 211 }
alecguertin 22:46b9d9b2e35c 212
alecguertin 22:46b9d9b2e35c 213 /* Compute drive time and move forward. */
alecguertin 22:46b9d9b2e35c 214 dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
alecguertin 22:46b9d9b2e35c 215 if (dist < 0) {
alecguertin 22:46b9d9b2e35c 216 dist *= -1;
alecguertin 22:46b9d9b2e35c 217 }
alecguertin 25:2c7717684d09 218 //pi.cls();
alecguertin 25:2c7717684d09 219 //pi.locate(0,0);
alecguertin 25:2c7717684d09 220 //pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x));
alecguertin 25:2c7717684d09 221 //timerWait(1.5);
alecguertin 22:46b9d9b2e35c 222 forward(0.4*cal_time*(dist/(double)dim_x));
alecguertin 22:46b9d9b2e35c 223
alecguertin 22:46b9d9b2e35c 224 last_x = x;
alecguertin 22:46b9d9b2e35c 225 last_y = y;
alecguertin 22:46b9d9b2e35c 226 next = strchr(cur, '\n');
alecguertin 22:46b9d9b2e35c 227 if (next == NULL) {
chstrchu 20:76718145b403 228 pi.cls();
chstrchu 20:76718145b403 229 pi.locate(0,0);
alecguertin 22:46b9d9b2e35c 230 pi.printf("nonext");
alecguertin 22:46b9d9b2e35c 231 break;
alecguertin 17:c72c092fcdf7 232 }
alecguertin 22:46b9d9b2e35c 233 cur = next+1;
alecguertin 22:46b9d9b2e35c 234 offset = instbuf+instbuflen-cur-1;
alecguertin 17:c72c092fcdf7 235 memcpy(instbuf, cur, offset);
alecguertin 15:14d4e7021125 236 }
chstrchu 20:76718145b403 237 pi.cls();
chstrchu 20:76718145b403 238 pi.locate(0,0);
lsaristo 29:459ff10d2a07 239 pi.printf("Done");
alecguertin 27:44a4a0abc8ee 240 while (1);
John Wilkey 5:01882c3de2dc 241 }
lsaristo 8:12d780f7443e 242
alecguertin 17:c72c092fcdf7 243 int retrieve_inst(char *buf, int *x, int *y, int *draw)
alecguertin 17:c72c092fcdf7 244 {
alecguertin 17:c72c092fcdf7 245 int matches;
alecguertin 17:c72c092fcdf7 246 char *srch;
alecguertin 17:c72c092fcdf7 247 matches = sscanf(buf, "%d %d", x, y);
alecguertin 17:c72c092fcdf7 248 if (matches != 2) {
chstrchu 20:76718145b403 249 pi.cls();
chstrchu 20:76718145b403 250 pi.locate(0,0);
chstrchu 20:76718145b403 251 pi.printf("nomatch");
alecguertin 17:c72c092fcdf7 252 return 0;
alecguertin 17:c72c092fcdf7 253 }
alecguertin 17:c72c092fcdf7 254 srch = strchr(buf, ' ');
alecguertin 17:c72c092fcdf7 255 srch++;
chstrchu 20:76718145b403 256 srch = strchr(srch, ' ');
alecguertin 17:c72c092fcdf7 257 srch++;
alecguertin 17:c72c092fcdf7 258 if (srch[0] == 'm') {
alecguertin 17:c72c092fcdf7 259 *draw = 0;
chstrchu 20:76718145b403 260 } else if (srch[0] == 'l') {
alecguertin 17:c72c092fcdf7 261 *draw = 1;
alecguertin 17:c72c092fcdf7 262 } else {
chstrchu 20:76718145b403 263 pi.cls();
chstrchu 20:76718145b403 264 pi.locate(0,0);
chstrchu 20:76718145b403 265 pi.printf("%.8s", srch);
alecguertin 17:c72c092fcdf7 266 return 0;
alecguertin 17:c72c092fcdf7 267 }
alecguertin 17:c72c092fcdf7 268 return 1;
alecguertin 17:c72c092fcdf7 269 }
alecguertin 17:c72c092fcdf7 270
lsaristo 29:459ff10d2a07 271 void forward(int amt)
lsaristo 8:12d780f7443e 272 {
lsaristo 12:1aa6b8a74136 273 Timer t;
lsaristo 9:3a0433c391cb 274 pi.locate(0,0);
lsaristo 9:3a0433c391cb 275 pi.printf("Fwd %d", amt);
lsaristo 30:3211e2962441 276 t.start();
lsaristo 21:0c80a5d89ea3 277 pi.left_motor(DRIVE_SPEED+.0023);
lsaristo 21:0c80a5d89ea3 278 pi.right_motor(DRIVE_SPEED);
alecguertin 25:2c7717684d09 279 while(t.read_ms() < amt);
lsaristo 12:1aa6b8a74136 280 t.stop();
lsaristo 12:1aa6b8a74136 281 pi.stop();
lsaristo 8:12d780f7443e 282 }
lsaristo 8:12d780f7443e 283
lsaristo 29:459ff10d2a07 284 void backward(int amt)
lsaristo 8:12d780f7443e 285 {
lsaristo 12:1aa6b8a74136 286 Timer t;
lsaristo 12:1aa6b8a74136 287 t.start();
alecguertin 22:46b9d9b2e35c 288 pi.backward(.5*DRIVE_SPEED);
alecguertin 22:46b9d9b2e35c 289 while(t.read_ms() < (amt-500));
alecguertin 22:46b9d9b2e35c 290 pi.backward(.5*DRIVE_SPEED);
chstrchu 20:76718145b403 291 while(t.read_ms() < amt);
lsaristo 12:1aa6b8a74136 292 t.stop();
lsaristo 12:1aa6b8a74136 293 pi.stop();
lsaristo 8:12d780f7443e 294 }
lsaristo 8:12d780f7443e 295
lsaristo 29:459ff10d2a07 296 void right(float deg)
lsaristo 8:12d780f7443e 297 {
lsaristo 24:b797563776fc 298 if(deg < 0) {
lsaristo 24:b797563776fc 299 return left(-1*deg);
lsaristo 24:b797563776fc 300 }
lsaristo 12:1aa6b8a74136 301 Timer t;
lsaristo 30:3211e2962441 302 float amt = ((float)(deg/360)*TIME_FACT);
lsaristo 12:1aa6b8a74136 303 t.start();
lsaristo 30:3211e2962441 304 pi.right(TURN_SPEED);
lsaristo 30:3211e2962441 305 while(t.read_ms() < amt);
lsaristo 12:1aa6b8a74136 306 t.stop();
lsaristo 12:1aa6b8a74136 307 pi.stop();
lsaristo 8:12d780f7443e 308 }
lsaristo 8:12d780f7443e 309
lsaristo 21:0c80a5d89ea3 310 void timerWait(float seconds)
lsaristo 21:0c80a5d89ea3 311 {
lsaristo 21:0c80a5d89ea3 312 Timer t;
lsaristo 21:0c80a5d89ea3 313 t.start();
lsaristo 21:0c80a5d89ea3 314 while(t.read_us() < 1000000*seconds);
lsaristo 21:0c80a5d89ea3 315 t.stop();
lsaristo 21:0c80a5d89ea3 316 }
lsaristo 21:0c80a5d89ea3 317
lsaristo 29:459ff10d2a07 318 void left(float deg)
lsaristo 8:12d780f7443e 319 {
lsaristo 24:b797563776fc 320 if(deg < 0) {
lsaristo 24:b797563776fc 321 return right(-1*deg);
lsaristo 24:b797563776fc 322 }
lsaristo 12:1aa6b8a74136 323 Timer t;
lsaristo 8:12d780f7443e 324 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 325 t.start();
alecguertin 22:46b9d9b2e35c 326 while(t.read_ms() < ((float)(deg/360)*TIME_FACT));
lsaristo 12:1aa6b8a74136 327 t.stop();
lsaristo 12:1aa6b8a74136 328 pi.stop();
lsaristo 10:94b068b2ce1d 329 }
lsaristo 10:94b068b2ce1d 330
lsaristo 10:94b068b2ce1d 331 void pen_down()
lsaristo 10:94b068b2ce1d 332 {
lsaristo 10:94b068b2ce1d 333 oled_1 = 1;
lsaristo 10:94b068b2ce1d 334 }
lsaristo 10:94b068b2ce1d 335
lsaristo 10:94b068b2ce1d 336 void pen_up()
lsaristo 10:94b068b2ce1d 337 {
lsaristo 10:94b068b2ce1d 338 oled_1 = 0;
lsaristo 9:3a0433c391cb 339 }