EE149
/
FinalProject
Final Project files for mBed development.
main.c.orig@28:9976a94efa83, 2014-12-08 (annotated)
- Committer:
- alecguertin
- Date:
- Mon Dec 08 10:52:42 2014 +0000
- Revision:
- 28:9976a94efa83
- Parent:
- 16:f1beef7beeb9
end of sunday
Who changed what in which revision?
User | Revision | Line number | New 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 | } |