EE149 / Mbed 2 deprecated FinalProject

Final Project files for mBed development.

Dependencies:   m3pi mbed

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

Committer:
alecguertin
Date:
Sun Nov 30 21:52:58 2014 +0000
Revision:
16:f1beef7beeb9
fixed merge conflicts

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alecguertin 16:f1beef7beeb9 1 /**
alecguertin 16:f1beef7beeb9 2 * @file driver.c
alecguertin 16:f1beef7beeb9 3 * @brief Basic driver program for our robot's controller logic.
alecguertin 16:f1beef7beeb9 4 *
alecguertin 16:f1beef7beeb9 5 * Maybe add lots of stuff here or maybe split it off into
alecguertin 16:f1beef7beeb9 6 * multiple subfiles?
alecguertin 16:f1beef7beeb9 7 *
alecguertin 16:f1beef7beeb9 8 * @author John Wilkey
alecguertin 16:f1beef7beeb9 9 */
alecguertin 16:f1beef7beeb9 10 #include "main.h"
alecguertin 16:f1beef7beeb9 11 #include "control.h"
alecguertin 16:f1beef7beeb9 12
alecguertin 16:f1beef7beeb9 13 /**
alecguertin 16:f1beef7beeb9 14 * These are global data Used externally in all other files
alecguertin 16:f1beef7beeb9 15 */
alecguertin 16:f1beef7beeb9 16 m3pi pi;
alecguertin 16:f1beef7beeb9 17 Timer timer;
alecguertin 16:f1beef7beeb9 18
alecguertin 16:f1beef7beeb9 19 //
alecguertin 16:f1beef7beeb9 20 // Digital inputs to the mBed
alecguertin 16:f1beef7beeb9 21 DigitalIn start_button(p21);
alecguertin 16:f1beef7beeb9 22
alecguertin 16:f1beef7beeb9 23 //
alecguertin 16:f1beef7beeb9 24 // Digital outputs from the mBed. Note that by default these are
alecguertin 16:f1beef7beeb9 25 // used to drive the 8 LED's on the top board.
alecguertin 16:f1beef7beeb9 26 DigitalOut pin15(p15);
alecguertin 16:f1beef7beeb9 27 DigitalOut pin16(p16);
alecguertin 16:f1beef7beeb9 28 DigitalOut pin17(p17);
alecguertin 16:f1beef7beeb9 29 DigitalOut pin18(p18);
alecguertin 16:f1beef7beeb9 30 DigitalOut pin19(p19);
alecguertin 16:f1beef7beeb9 31 DigitalOut pin20(p20);
alecguertin 16:f1beef7beeb9 32
alecguertin 16:f1beef7beeb9 33 //
alecguertin 16:f1beef7beeb9 34 // mBed onboard LEDs
alecguertin 16:f1beef7beeb9 35 DigitalOut oled_1(LED1);
alecguertin 16:f1beef7beeb9 36 DigitalOut oled_2(LED2);
alecguertin 16:f1beef7beeb9 37 DigitalOut oled_3(LED3);
alecguertin 16:f1beef7beeb9 38 DigitalOut oled_4(LED4);
alecguertin 16:f1beef7beeb9 39
alecguertin 16:f1beef7beeb9 40
alecguertin 16:f1beef7beeb9 41 /**
alecguertin 16:f1beef7beeb9 42 * @brief Entry point. Main loop.
alecguertin 16:f1beef7beeb9 43 */
alecguertin 16:f1beef7beeb9 44 int main()
alecguertin 16:f1beef7beeb9 45 {
alecguertin 16:f1beef7beeb9 46 //
alecguertin 16:f1beef7beeb9 47 // Basic setup information
alecguertin 16:f1beef7beeb9 48 start_button.mode(PullUp);
alecguertin 16:f1beef7beeb9 49
alecguertin 16:f1beef7beeb9 50 //
alecguertin 16:f1beef7beeb9 51 // Drawing environment calibration.
alecguertin 16:f1beef7beeb9 52 pi.sensor_auto_calibrate();
alecguertin 16:f1beef7beeb9 53 pi.backward(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 54 float pos;
alecguertin 16:f1beef7beeb9 55 float over_thresh = 0.5;
alecguertin 16:f1beef7beeb9 56 float correction = 0.1;
alecguertin 16:f1beef7beeb9 57
alecguertin 16:f1beef7beeb9 58 do {
alecguertin 16:f1beef7beeb9 59 pos = pi.line_position();
alecguertin 16:f1beef7beeb9 60 if(pos > over_thresh) {
alecguertin 16:f1beef7beeb9 61 pi.right_motor(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 62 pi.left_motor(DRIVE_SPEED - correction);
alecguertin 16:f1beef7beeb9 63 } else if(pos < -over_thresh) {
alecguertin 16:f1beef7beeb9 64 pi.left_motor(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 65 pi.right_motor(DRIVE_SPEED - correction);
alecguertin 16:f1beef7beeb9 66 } else {
alecguertin 16:f1beef7beeb9 67 pi.forward(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 68 }
alecguertin 16:f1beef7beeb9 69 pi.cls();
alecguertin 16:f1beef7beeb9 70 pi.locate(0,0);
alecguertin 16:f1beef7beeb9 71 pi.printf("P: %f", pos);
alecguertin 16:f1beef7beeb9 72 } while(pos != -1 && pos != 1);
alecguertin 16:f1beef7beeb9 73 if(pos == 1) {
alecguertin 16:f1beef7beeb9 74 timer.start();
alecguertin 16:f1beef7beeb9 75 pi.forward(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 76 } else {
alecguertin 16:f1beef7beeb9 77 pi.stop();
alecguertin 16:f1beef7beeb9 78 pi.cls();
alecguertin 16:f1beef7beeb9 79 pi.locate(0,0);
alecguertin 16:f1beef7beeb9 80 pi.printf("LP: %f",pos);
alecguertin 16:f1beef7beeb9 81 while(1);
alecguertin 16:f1beef7beeb9 82 }
alecguertin 16:f1beef7beeb9 83
alecguertin 16:f1beef7beeb9 84 while(pi.line_position() == 1);
alecguertin 16:f1beef7beeb9 85 do {
alecguertin 16:f1beef7beeb9 86 pos = pi.line_position();
alecguertin 16:f1beef7beeb9 87 if(pos > over_thresh) {
alecguertin 16:f1beef7beeb9 88 pi.right_motor(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 89 pi.left_motor(DRIVE_SPEED - correction);
alecguertin 16:f1beef7beeb9 90 } else if(pos < -over_thresh) {
alecguertin 16:f1beef7beeb9 91 pi.left_motor(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 92 pi.right_motor(DRIVE_SPEED - correction);
alecguertin 16:f1beef7beeb9 93 } else {
alecguertin 16:f1beef7beeb9 94 pi.forward(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 95 }
alecguertin 16:f1beef7beeb9 96 pi.cls();
alecguertin 16:f1beef7beeb9 97 pi.locate(0,0);
alecguertin 16:f1beef7beeb9 98 pi.printf("Pos: %f", pos);
alecguertin 16:f1beef7beeb9 99 } while(pos != -1 && pos != 1);
alecguertin 16:f1beef7beeb9 100 if(pos == 1) {
alecguertin 16:f1beef7beeb9 101 oled_1 = 1;
alecguertin 16:f1beef7beeb9 102 timer.stop();
alecguertin 16:f1beef7beeb9 103 pi.stop();
alecguertin 16:f1beef7beeb9 104 } else {
alecguertin 16:f1beef7beeb9 105 pi.stop();
alecguertin 16:f1beef7beeb9 106 pi.cls();
alecguertin 16:f1beef7beeb9 107 pi.locate(0,0);
alecguertin 16:f1beef7beeb9 108 pi.printf("LP:%f", pos);
alecguertin 16:f1beef7beeb9 109 while(1);
alecguertin 16:f1beef7beeb9 110 }
alecguertin 16:f1beef7beeb9 111 // If we got here, calibration is complete.
alecguertin 16:f1beef7beeb9 112
alecguertin 16:f1beef7beeb9 113 //
alecguertin 16:f1beef7beeb9 114 // Main program loop.
alecguertin 16:f1beef7beeb9 115 // robot_loop();
alecguertin 16:f1beef7beeb9 116
alecguertin 16:f1beef7beeb9 117 //
alecguertin 16:f1beef7beeb9 118 // We should never reach this point!
alecguertin 16:f1beef7beeb9 119 while(1);
alecguertin 16:f1beef7beeb9 120 }
alecguertin 16:f1beef7beeb9 121
alecguertin 16:f1beef7beeb9 122 int forward(int amt)
alecguertin 16:f1beef7beeb9 123 {
alecguertin 16:f1beef7beeb9 124 Timer t;
alecguertin 16:f1beef7beeb9 125 t.start();
alecguertin 16:f1beef7beeb9 126 oled_2 = 1;
alecguertin 16:f1beef7beeb9 127 pi.locate(0,0);
alecguertin 16:f1beef7beeb9 128 pi.printf("Fwd %d", amt);
alecguertin 16:f1beef7beeb9 129 pi.forward(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 130 while(t.read_ms() < amt*DRIVE_RATE*1000);
alecguertin 16:f1beef7beeb9 131 t.stop();
alecguertin 16:f1beef7beeb9 132 oled_2 = 0;
alecguertin 16:f1beef7beeb9 133 pi.stop();
alecguertin 16:f1beef7beeb9 134 return EXIT_SUCCESS;
alecguertin 16:f1beef7beeb9 135 }
alecguertin 16:f1beef7beeb9 136
alecguertin 16:f1beef7beeb9 137 int backward(int amt)
alecguertin 16:f1beef7beeb9 138 {
alecguertin 16:f1beef7beeb9 139 Timer t;
alecguertin 16:f1beef7beeb9 140 oled_3 = 1;
alecguertin 16:f1beef7beeb9 141 pi.locate(0,0);
alecguertin 16:f1beef7beeb9 142 pi.printf("Back %d", amt);
alecguertin 16:f1beef7beeb9 143 pi.backward(DRIVE_SPEED);
alecguertin 16:f1beef7beeb9 144 t.start();
alecguertin 16:f1beef7beeb9 145 while(t.read_ms() < amt*DRIVE_RATE*1000);
alecguertin 16:f1beef7beeb9 146 t.stop();
alecguertin 16:f1beef7beeb9 147 oled_3 = 0;
alecguertin 16:f1beef7beeb9 148 pi.stop();
alecguertin 16:f1beef7beeb9 149 return EXIT_SUCCESS;
alecguertin 16:f1beef7beeb9 150 }
alecguertin 16:f1beef7beeb9 151
alecguertin 16:f1beef7beeb9 152 int right(float deg)
alecguertin 16:f1beef7beeb9 153 {
alecguertin 16:f1beef7beeb9 154 Timer t;
alecguertin 16:f1beef7beeb9 155 oled_4 = 1;
alecguertin 16:f1beef7beeb9 156 pi.locate(0,0);
alecguertin 16:f1beef7beeb9 157 pi.printf("Right %f", deg);
alecguertin 16:f1beef7beeb9 158 pi.right(TURN_SPEED);
alecguertin 16:f1beef7beeb9 159 t.start();
alecguertin 16:f1beef7beeb9 160 while(t.read_ms() < (deg/360)*1000);
alecguertin 16:f1beef7beeb9 161 t.stop();
alecguertin 16:f1beef7beeb9 162 oled_4 = 0;
alecguertin 16:f1beef7beeb9 163 pi.stop();
alecguertin 16:f1beef7beeb9 164 return EXIT_SUCCESS;
alecguertin 16:f1beef7beeb9 165 }
alecguertin 16:f1beef7beeb9 166
alecguertin 16:f1beef7beeb9 167 int left(float deg)
alecguertin 16:f1beef7beeb9 168 {
alecguertin 16:f1beef7beeb9 169 Timer t;
alecguertin 16:f1beef7beeb9 170 oled_4 = 1;
alecguertin 16:f1beef7beeb9 171 oled_2 = 1;
alecguertin 16:f1beef7beeb9 172 pi.locate(0,0);
alecguertin 16:f1beef7beeb9 173 pi.printf("Left %f", deg);
alecguertin 16:f1beef7beeb9 174 pi.left(TURN_SPEED);
alecguertin 16:f1beef7beeb9 175 t.start();
alecguertin 16:f1beef7beeb9 176 while(t.read_ms() < (deg/360)*1000);
alecguertin 16:f1beef7beeb9 177 t.stop();
alecguertin 16:f1beef7beeb9 178 oled_4 = 0;
alecguertin 16:f1beef7beeb9 179 oled_2 = 0;
alecguertin 16:f1beef7beeb9 180 pi.stop();
alecguertin 16:f1beef7beeb9 181 return EXIT_SUCCESS;
alecguertin 16:f1beef7beeb9 182 }
alecguertin 16:f1beef7beeb9 183
alecguertin 16:f1beef7beeb9 184 void pen_down()
alecguertin 16:f1beef7beeb9 185 {
alecguertin 16:f1beef7beeb9 186 oled_1 = 1;
alecguertin 16:f1beef7beeb9 187 }
alecguertin 16:f1beef7beeb9 188
alecguertin 16:f1beef7beeb9 189 void pen_up()
alecguertin 16:f1beef7beeb9 190 {
alecguertin 16:f1beef7beeb9 191 oled_1 = 0;
alecguertin 16:f1beef7beeb9 192 }