EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@16:f1beef7beeb9, 2014-11-30 (annotated)

Committer:
alecguertin
Date:
Sun Nov 30 21:52:58 2014 +0000
Revision:
16:f1beef7beeb9
Parent:
14:41fa8b95a9ab
Parent:
15:14d4e7021125
Child:
17:c72c092fcdf7
fixed merge conflicts

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
John Wilkey 5:01882c3de2dc 43 /**
John Wilkey 5:01882c3de2dc 44 * @brief Entry point. Main loop.
John Wilkey 5:01882c3de2dc 45 */
John Wilkey 5:01882c3de2dc 46 int main()
John Wilkey 5:01882c3de2dc 47 {
lsaristo 9:3a0433c391cb 48 //
lsaristo 9:3a0433c391cb 49 // Basic setup information
lsaristo 10:94b068b2ce1d 50 start_button.mode(PullUp);
lsaristo 12:1aa6b8a74136 51
lsaristo 13:070846d87d4a 52 //
lsaristo 13:070846d87d4a 53 // Drawing environment calibration.
lsaristo 12:1aa6b8a74136 54 pi.sensor_auto_calibrate();
alecguertin 15:14d4e7021125 55 //pi.backward(DRIVE_SPEED);
alecguertin 15:14d4e7021125 56 float pos = 0;
alecguertin 15:14d4e7021125 57 float over_thresh = 0.05;
alecguertin 15:14d4e7021125 58 float correction = 0.2*DRIVE_SPEED;
alecguertin 15:14d4e7021125 59
alecguertin 15:14d4e7021125 60 wait(1);
alecguertin 15:14d4e7021125 61
alecguertin 15:14d4e7021125 62 do {
alecguertin 15:14d4e7021125 63 pos = pi.line_position();
alecguertin 15:14d4e7021125 64 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 65 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 66 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 67 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 68 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 69 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 70 } else {
alecguertin 15:14d4e7021125 71 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 72 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 73 }
alecguertin 15:14d4e7021125 74 /*
alecguertin 15:14d4e7021125 75 pi.cls();
alecguertin 15:14d4e7021125 76 pi.locate(0,0);
alecguertin 15:14d4e7021125 77 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 78 */
alecguertin 15:14d4e7021125 79 } while (pos != -1 && pos < 0.2);
alecguertin 15:14d4e7021125 80 pi.stop();
alecguertin 15:14d4e7021125 81 wait(1);
alecguertin 15:14d4e7021125 82 if (pos != -1) {
alecguertin 15:14d4e7021125 83 timer.start();
alecguertin 15:14d4e7021125 84 pi.forward(DRIVE_SPEED);
alecguertin 15:14d4e7021125 85 } else {
alecguertin 15:14d4e7021125 86 pi.cls();
alecguertin 15:14d4e7021125 87 pi.locate(0,0);
alecguertin 15:14d4e7021125 88 pi.printf("LP: %f",pos);
alecguertin 15:14d4e7021125 89 return 1;
alecguertin 15:14d4e7021125 90 }
alecguertin 15:14d4e7021125 91
alecguertin 15:14d4e7021125 92 wait(1);
lsaristo 12:1aa6b8a74136 93
lsaristo 12:1aa6b8a74136 94 do {
lsaristo 12:1aa6b8a74136 95 pos = pi.line_position();
lsaristo 13:070846d87d4a 96 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 97 pi.right_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 98 pi.left_motor(DRIVE_SPEED-correction);
lsaristo 13:070846d87d4a 99 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 100 pi.left_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 101 pi.right_motor(DRIVE_SPEED-correction);
lsaristo 13:070846d87d4a 102 } else {
alecguertin 15:14d4e7021125 103 pi.right_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 104 pi.left_motor(DRIVE_SPEED);
alecguertin 15:14d4e7021125 105 }
alecguertin 15:14d4e7021125 106 pi.cls();
alecguertin 15:14d4e7021125 107 pi.locate(0,0);
alecguertin 15:14d4e7021125 108 pi.printf("Pos: %f", pos);
alecguertin 15:14d4e7021125 109 } while(pos != -1 && pos <= 0.3);
alecguertin 15:14d4e7021125 110 pi.stop();
alecguertin 15:14d4e7021125 111 if(pos != -1) {
alecguertin 15:14d4e7021125 112 oled_1 = 1;
alecguertin 15:14d4e7021125 113 timer.stop();
alecguertin 15:14d4e7021125 114 pi.printf("T: %d", timer.read_ms());
alecguertin 15:14d4e7021125 115 } else {
alecguertin 15:14d4e7021125 116 pi.cls();
alecguertin 15:14d4e7021125 117 pi.locate(0,0);
alecguertin 15:14d4e7021125 118 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 119 return 1;
alecguertin 15:14d4e7021125 120 }
alecguertin 15:14d4e7021125 121 pi.right(TURN_SPEED);
alecguertin 15:14d4e7021125 122 wait(0.5);
alecguertin 15:14d4e7021125 123 pi.stop();
alecguertin 15:14d4e7021125 124 do {
alecguertin 15:14d4e7021125 125 pos = pi.line_position();
alecguertin 15:14d4e7021125 126 if(pos > over_thresh) {
alecguertin 15:14d4e7021125 127 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 128 pi.left_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 129 } else if(pos < -over_thresh) {
alecguertin 15:14d4e7021125 130 pi.left_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 131 pi.right_motor(-DRIVE_SPEED+correction);
alecguertin 15:14d4e7021125 132 } else {
alecguertin 15:14d4e7021125 133 pi.right_motor(-DRIVE_SPEED);
alecguertin 15:14d4e7021125 134 pi.left_motor(-DRIVE_SPEED);
lsaristo 13:070846d87d4a 135 }
lsaristo 12:1aa6b8a74136 136 pi.cls();
lsaristo 12:1aa6b8a74136 137 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 138 pi.printf("P: %f", pos);
alecguertin 15:14d4e7021125 139 } while (pos != -1 && pos > -0.3);
alecguertin 15:14d4e7021125 140 pi.stop();
alecguertin 15:14d4e7021125 141 if(pos != -1) {
alecguertin 15:14d4e7021125 142 oled_1 = 1;
alecguertin 15:14d4e7021125 143 timer.stop();
alecguertin 15:14d4e7021125 144 pi.printf("T: %d", timer.read_ms());
lsaristo 12:1aa6b8a74136 145 } else {
lsaristo 12:1aa6b8a74136 146 pi.cls();
lsaristo 12:1aa6b8a74136 147 pi.locate(0,0);
alecguertin 15:14d4e7021125 148 pi.printf("lP:%f", pos);
alecguertin 15:14d4e7021125 149 return 1;
lsaristo 12:1aa6b8a74136 150 }
John Wilkey 6:00b7198f0b51 151
lsaristo 12:1aa6b8a74136 152 while(pi.line_position() == 1);
lsaristo 12:1aa6b8a74136 153 do {
lsaristo 12:1aa6b8a74136 154 pos = pi.line_position();
lsaristo 13:070846d87d4a 155 if(pos > over_thresh) {
lsaristo 13:070846d87d4a 156 pi.right_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 157 pi.left_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 158 } else if(pos < -over_thresh) {
lsaristo 13:070846d87d4a 159 pi.left_motor(DRIVE_SPEED);
lsaristo 13:070846d87d4a 160 pi.right_motor(DRIVE_SPEED - correction);
lsaristo 13:070846d87d4a 161 } else {
lsaristo 13:070846d87d4a 162 pi.forward(DRIVE_SPEED);
lsaristo 13:070846d87d4a 163 }
lsaristo 12:1aa6b8a74136 164 pi.cls();
lsaristo 12:1aa6b8a74136 165 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 166 pi.printf("Pos: %f", pos);
lsaristo 12:1aa6b8a74136 167 } while(pos != -1 && pos != 1);
lsaristo 12:1aa6b8a74136 168 if(pos == 1) {
lsaristo 12:1aa6b8a74136 169 oled_1 = 1;
lsaristo 12:1aa6b8a74136 170 timer.stop();
lsaristo 12:1aa6b8a74136 171 pi.stop();
lsaristo 12:1aa6b8a74136 172 } else {
lsaristo 12:1aa6b8a74136 173 pi.stop();
lsaristo 12:1aa6b8a74136 174 pi.cls();
lsaristo 12:1aa6b8a74136 175 pi.locate(0,0);
lsaristo 12:1aa6b8a74136 176 pi.printf("LP:%f", pos);
lsaristo 12:1aa6b8a74136 177 while(1);
lsaristo 12:1aa6b8a74136 178 }
lsaristo 14:41fa8b95a9ab 179 // If we got here, calibration is complete.
lsaristo 12:1aa6b8a74136 180
John Wilkey 6:00b7198f0b51 181 //
lsaristo 9:3a0433c391cb 182 // Main program loop.
lsaristo 12:1aa6b8a74136 183 // robot_loop();
lsaristo 9:3a0433c391cb 184
alecguertin 15:14d4e7021125 185 ps_file = fopen("/local/test.ps", "r");
alecguertin 15:14d4e7021125 186 while () {
alecguertin 15:14d4e7021125 187 memset(instbuf, 0, instbuflen);
alecguertin 15:14d4e7021125 188 fread(instbuf, sizeof(char), instbuflen, ps_file);
alecguertin 15:14d4e7021125 189 }
lsaristo 9:3a0433c391cb 190 //
lsaristo 9:3a0433c391cb 191 // We should never reach this point!
alecguertin 15:14d4e7021125 192 return 0;
John Wilkey 5:01882c3de2dc 193 }
lsaristo 8:12d780f7443e 194
lsaristo 9:3a0433c391cb 195 int forward(int amt)
lsaristo 8:12d780f7443e 196 {
lsaristo 12:1aa6b8a74136 197 Timer t;
lsaristo 12:1aa6b8a74136 198 t.start();
lsaristo 10:94b068b2ce1d 199 oled_2 = 1;
lsaristo 9:3a0433c391cb 200 pi.locate(0,0);
lsaristo 9:3a0433c391cb 201 pi.printf("Fwd %d", amt);
lsaristo 9:3a0433c391cb 202 pi.forward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 203 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 204 t.stop();
lsaristo 10:94b068b2ce1d 205 oled_2 = 0;
lsaristo 12:1aa6b8a74136 206 pi.stop();
lsaristo 8:12d780f7443e 207 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 208 }
lsaristo 8:12d780f7443e 209
lsaristo 9:3a0433c391cb 210 int backward(int amt)
lsaristo 8:12d780f7443e 211 {
lsaristo 12:1aa6b8a74136 212 Timer t;
lsaristo 10:94b068b2ce1d 213 oled_3 = 1;
lsaristo 9:3a0433c391cb 214 pi.locate(0,0);
lsaristo 9:3a0433c391cb 215 pi.printf("Back %d", amt);
lsaristo 9:3a0433c391cb 216 pi.backward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 217 t.start();
lsaristo 12:1aa6b8a74136 218 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 219 t.stop();
lsaristo 10:94b068b2ce1d 220 oled_3 = 0;
lsaristo 12:1aa6b8a74136 221 pi.stop();
lsaristo 8:12d780f7443e 222 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 223 }
lsaristo 8:12d780f7443e 224
lsaristo 8:12d780f7443e 225 int right(float deg)
lsaristo 8:12d780f7443e 226 {
lsaristo 12:1aa6b8a74136 227 Timer t;
lsaristo 10:94b068b2ce1d 228 oled_4 = 1;
lsaristo 9:3a0433c391cb 229 pi.locate(0,0);
lsaristo 9:3a0433c391cb 230 pi.printf("Right %f", deg);
lsaristo 8:12d780f7443e 231 pi.right(TURN_SPEED);
lsaristo 12:1aa6b8a74136 232 t.start();
lsaristo 12:1aa6b8a74136 233 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 234 t.stop();
lsaristo 10:94b068b2ce1d 235 oled_4 = 0;
lsaristo 12:1aa6b8a74136 236 pi.stop();
lsaristo 8:12d780f7443e 237 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 238 }
lsaristo 8:12d780f7443e 239
lsaristo 8:12d780f7443e 240 int left(float deg)
lsaristo 8:12d780f7443e 241 {
lsaristo 12:1aa6b8a74136 242 Timer t;
lsaristo 10:94b068b2ce1d 243 oled_4 = 1;
lsaristo 10:94b068b2ce1d 244 oled_2 = 1;
lsaristo 9:3a0433c391cb 245 pi.locate(0,0);
lsaristo 9:3a0433c391cb 246 pi.printf("Left %f", deg);
lsaristo 8:12d780f7443e 247 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 248 t.start();
lsaristo 12:1aa6b8a74136 249 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 250 t.stop();
lsaristo 10:94b068b2ce1d 251 oled_4 = 0;
lsaristo 10:94b068b2ce1d 252 oled_2 = 0;
lsaristo 12:1aa6b8a74136 253 pi.stop();
lsaristo 8:12d780f7443e 254 return EXIT_SUCCESS;
lsaristo 10:94b068b2ce1d 255 }
lsaristo 10:94b068b2ce1d 256
lsaristo 10:94b068b2ce1d 257 void pen_down()
lsaristo 10:94b068b2ce1d 258 {
lsaristo 10:94b068b2ce1d 259 oled_1 = 1;
lsaristo 10:94b068b2ce1d 260 }
lsaristo 10:94b068b2ce1d 261
lsaristo 10:94b068b2ce1d 262 void pen_up()
lsaristo 10:94b068b2ce1d 263 {
lsaristo 10:94b068b2ce1d 264 oled_1 = 0;
lsaristo 9:3a0433c391cb 265 }