EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

main.c@15:14d4e7021125, 2014-11-30 (annotated)

Committer:
alecguertin
Date:
Sun Nov 30 21:49:46 2014 +0000
Revision:
15:14d4e7021125
Parent:
13:070846d87d4a
Child:
16:f1beef7beeb9
fixed line detection state

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 9:3a0433c391cb 152 // Main program loop.
lsaristo 12:1aa6b8a74136 153 // robot_loop();
lsaristo 9:3a0433c391cb 154
alecguertin 15:14d4e7021125 155 ps_file = fopen("/local/test.ps", "r");
alecguertin 15:14d4e7021125 156 while () {
alecguertin 15:14d4e7021125 157 memset(instbuf, 0, instbuflen);
alecguertin 15:14d4e7021125 158 fread(instbuf, sizeof(char), instbuflen, ps_file);
alecguertin 15:14d4e7021125 159 }
lsaristo 9:3a0433c391cb 160 //
lsaristo 9:3a0433c391cb 161 // We should never reach this point!
alecguertin 15:14d4e7021125 162 return 0;
John Wilkey 5:01882c3de2dc 163 }
lsaristo 8:12d780f7443e 164
lsaristo 9:3a0433c391cb 165 int forward(int amt)
lsaristo 8:12d780f7443e 166 {
lsaristo 12:1aa6b8a74136 167 Timer t;
lsaristo 12:1aa6b8a74136 168 t.start();
lsaristo 10:94b068b2ce1d 169 oled_2 = 1;
lsaristo 9:3a0433c391cb 170 pi.locate(0,0);
lsaristo 9:3a0433c391cb 171 pi.printf("Fwd %d", amt);
lsaristo 9:3a0433c391cb 172 pi.forward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 173 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 174 t.stop();
lsaristo 10:94b068b2ce1d 175 oled_2 = 0;
lsaristo 12:1aa6b8a74136 176 pi.stop();
lsaristo 8:12d780f7443e 177 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 178 }
lsaristo 8:12d780f7443e 179
lsaristo 9:3a0433c391cb 180 int backward(int amt)
lsaristo 8:12d780f7443e 181 {
lsaristo 12:1aa6b8a74136 182 Timer t;
lsaristo 10:94b068b2ce1d 183 oled_3 = 1;
lsaristo 9:3a0433c391cb 184 pi.locate(0,0);
lsaristo 9:3a0433c391cb 185 pi.printf("Back %d", amt);
lsaristo 9:3a0433c391cb 186 pi.backward(DRIVE_SPEED);
lsaristo 12:1aa6b8a74136 187 t.start();
lsaristo 12:1aa6b8a74136 188 while(t.read_ms() < amt*DRIVE_RATE*1000);
lsaristo 12:1aa6b8a74136 189 t.stop();
lsaristo 10:94b068b2ce1d 190 oled_3 = 0;
lsaristo 12:1aa6b8a74136 191 pi.stop();
lsaristo 8:12d780f7443e 192 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 193 }
lsaristo 8:12d780f7443e 194
lsaristo 8:12d780f7443e 195 int right(float deg)
lsaristo 8:12d780f7443e 196 {
lsaristo 12:1aa6b8a74136 197 Timer t;
lsaristo 10:94b068b2ce1d 198 oled_4 = 1;
lsaristo 9:3a0433c391cb 199 pi.locate(0,0);
lsaristo 9:3a0433c391cb 200 pi.printf("Right %f", deg);
lsaristo 8:12d780f7443e 201 pi.right(TURN_SPEED);
lsaristo 12:1aa6b8a74136 202 t.start();
lsaristo 12:1aa6b8a74136 203 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 204 t.stop();
lsaristo 10:94b068b2ce1d 205 oled_4 = 0;
lsaristo 12:1aa6b8a74136 206 pi.stop();
lsaristo 8:12d780f7443e 207 return EXIT_SUCCESS;
lsaristo 8:12d780f7443e 208 }
lsaristo 8:12d780f7443e 209
lsaristo 8:12d780f7443e 210 int left(float deg)
lsaristo 8:12d780f7443e 211 {
lsaristo 12:1aa6b8a74136 212 Timer t;
lsaristo 10:94b068b2ce1d 213 oled_4 = 1;
lsaristo 10:94b068b2ce1d 214 oled_2 = 1;
lsaristo 9:3a0433c391cb 215 pi.locate(0,0);
lsaristo 9:3a0433c391cb 216 pi.printf("Left %f", deg);
lsaristo 8:12d780f7443e 217 pi.left(TURN_SPEED);
lsaristo 12:1aa6b8a74136 218 t.start();
lsaristo 12:1aa6b8a74136 219 while(t.read_ms() < (deg/360)*1000);
lsaristo 12:1aa6b8a74136 220 t.stop();
lsaristo 10:94b068b2ce1d 221 oled_4 = 0;
lsaristo 10:94b068b2ce1d 222 oled_2 = 0;
lsaristo 12:1aa6b8a74136 223 pi.stop();
lsaristo 8:12d780f7443e 224 return EXIT_SUCCESS;
lsaristo 10:94b068b2ce1d 225 }
lsaristo 10:94b068b2ce1d 226
lsaristo 10:94b068b2ce1d 227 void pen_down()
lsaristo 10:94b068b2ce1d 228 {
lsaristo 10:94b068b2ce1d 229 oled_1 = 1;
lsaristo 10:94b068b2ce1d 230 }
lsaristo 10:94b068b2ce1d 231
lsaristo 10:94b068b2ce1d 232 void pen_up()
lsaristo 10:94b068b2ce1d 233 {
lsaristo 10:94b068b2ce1d 234 oled_1 = 0;
lsaristo 9:3a0433c391cb 235 }