Yasushi TAUCHI
LineMaze
main.cpp@0:0280b02789e8, 2012-06-12 (annotated)
- Committer:
- yueee_yt
- Date:
- Tue Jun 12 02:08:48 2012 +0000
- Revision:
- 0:0280b02789e8
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| yueee_yt | 0:0280b02789e8 | 1 | #include "mbed.h" |
| yueee_yt | 0:0280b02789e8 | 2 | #include "m3pi.h" |
| yueee_yt | 0:0280b02789e8 | 3 | |
| yueee_yt | 0:0280b02789e8 | 4 | m3pi m3pi; |
| yueee_yt | 0:0280b02789e8 | 5 | |
| yueee_yt | 0:0280b02789e8 | 6 | // Minimum and maximum motor speeds |
| yueee_yt | 0:0280b02789e8 | 7 | #define MAX 0.3 |
| yueee_yt | 0:0280b02789e8 | 8 | #define MIN 0 |
| yueee_yt | 0:0280b02789e8 | 9 | |
| yueee_yt | 0:0280b02789e8 | 10 | // PID terms |
| yueee_yt | 0:0280b02789e8 | 11 | #define P_TERM 0.6 |
| yueee_yt | 0:0280b02789e8 | 12 | #define I_TERM 0 |
| yueee_yt | 0:0280b02789e8 | 13 | #define D_TERM 20 |
| yueee_yt | 0:0280b02789e8 | 14 | DigitalIn sw(p21); |
| yueee_yt | 0:0280b02789e8 | 15 | char path[100] = ""; |
| yueee_yt | 0:0280b02789e8 | 16 | unsigned char path_length = 0; |
| yueee_yt | 0:0280b02789e8 | 17 | |
| yueee_yt | 0:0280b02789e8 | 18 | void follow_segment(void); |
| yueee_yt | 0:0280b02789e8 | 19 | void read_line(unsigned int sensors[5]) { |
| yueee_yt | 0:0280b02789e8 | 20 | unsigned char x[10]; |
| yueee_yt | 0:0280b02789e8 | 21 | int i; |
| yueee_yt | 0:0280b02789e8 | 22 | m3pi.putc(0x86); |
| yueee_yt | 0:0280b02789e8 | 23 | for (i=0; i<10; i++) { |
| yueee_yt | 0:0280b02789e8 | 24 | x[i]=m3pi.getc(); |
| yueee_yt | 0:0280b02789e8 | 25 | } |
| yueee_yt | 0:0280b02789e8 | 26 | sensors[0]=x[1]*0x100+x[0]; |
| yueee_yt | 0:0280b02789e8 | 27 | sensors[1]=x[3]*0x100+x[2]; |
| yueee_yt | 0:0280b02789e8 | 28 | sensors[2]=x[5]*0x100+x[4]; |
| yueee_yt | 0:0280b02789e8 | 29 | sensors[3]=x[7]*0x100+x[6]; |
| yueee_yt | 0:0280b02789e8 | 30 | sensors[4]=x[9]*0x100+x[8]; |
| yueee_yt | 0:0280b02789e8 | 31 | } |
| yueee_yt | 0:0280b02789e8 | 32 | |
| yueee_yt | 0:0280b02789e8 | 33 | char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right) { |
| yueee_yt | 0:0280b02789e8 | 34 | // Make a decision about how to turn. The following code |
| yueee_yt | 0:0280b02789e8 | 35 | // implements a left-hand-on-the-wall strategy, where we always |
| yueee_yt | 0:0280b02789e8 | 36 | // turn as far to the left as possible. |
| yueee_yt | 0:0280b02789e8 | 37 | if (found_left) |
| yueee_yt | 0:0280b02789e8 | 38 | return 'L'; |
| yueee_yt | 0:0280b02789e8 | 39 | else if (found_straight) |
| yueee_yt | 0:0280b02789e8 | 40 | return 'S'; |
| yueee_yt | 0:0280b02789e8 | 41 | else if (found_right) |
| yueee_yt | 0:0280b02789e8 | 42 | return 'R'; |
| yueee_yt | 0:0280b02789e8 | 43 | else |
| yueee_yt | 0:0280b02789e8 | 44 | return 'B'; |
| yueee_yt | 0:0280b02789e8 | 45 | } |
| yueee_yt | 0:0280b02789e8 | 46 | void simplify_path() { |
| yueee_yt | 0:0280b02789e8 | 47 | // only simplify the path if the second-to-last turn was a 'B' |
| yueee_yt | 0:0280b02789e8 | 48 | if (path_length < 3 || path[path_length-2] != 'B') |
| yueee_yt | 0:0280b02789e8 | 49 | return; |
| yueee_yt | 0:0280b02789e8 | 50 | |
| yueee_yt | 0:0280b02789e8 | 51 | int total_angle = 0; |
| yueee_yt | 0:0280b02789e8 | 52 | int i; |
| yueee_yt | 0:0280b02789e8 | 53 | for (i=1; i<=3; i++) { |
| yueee_yt | 0:0280b02789e8 | 54 | switch (path[path_length-i]) { |
| yueee_yt | 0:0280b02789e8 | 55 | case 'R': |
| yueee_yt | 0:0280b02789e8 | 56 | total_angle += 90; |
| yueee_yt | 0:0280b02789e8 | 57 | break; |
| yueee_yt | 0:0280b02789e8 | 58 | case 'L': |
| yueee_yt | 0:0280b02789e8 | 59 | total_angle += 270; |
| yueee_yt | 0:0280b02789e8 | 60 | break; |
| yueee_yt | 0:0280b02789e8 | 61 | case 'B': |
| yueee_yt | 0:0280b02789e8 | 62 | total_angle += 180; |
| yueee_yt | 0:0280b02789e8 | 63 | break; |
| yueee_yt | 0:0280b02789e8 | 64 | } |
| yueee_yt | 0:0280b02789e8 | 65 | } |
| yueee_yt | 0:0280b02789e8 | 66 | |
| yueee_yt | 0:0280b02789e8 | 67 | // Get the angle as a number between 0 and 360 degrees. |
| yueee_yt | 0:0280b02789e8 | 68 | total_angle = total_angle % 360; |
| yueee_yt | 0:0280b02789e8 | 69 | |
| yueee_yt | 0:0280b02789e8 | 70 | // Replace all of those turns with a single one. |
| yueee_yt | 0:0280b02789e8 | 71 | switch (total_angle) { |
| yueee_yt | 0:0280b02789e8 | 72 | case 0: |
| yueee_yt | 0:0280b02789e8 | 73 | path[path_length - 3] = 'S'; |
| yueee_yt | 0:0280b02789e8 | 74 | break; |
| yueee_yt | 0:0280b02789e8 | 75 | case 90: |
| yueee_yt | 0:0280b02789e8 | 76 | path[path_length - 3] = 'R'; |
| yueee_yt | 0:0280b02789e8 | 77 | break; |
| yueee_yt | 0:0280b02789e8 | 78 | case 180: |
| yueee_yt | 0:0280b02789e8 | 79 | path[path_length - 3] = 'B'; |
| yueee_yt | 0:0280b02789e8 | 80 | break; |
| yueee_yt | 0:0280b02789e8 | 81 | case 270: |
| yueee_yt | 0:0280b02789e8 | 82 | path[path_length - 3] = 'L'; |
| yueee_yt | 0:0280b02789e8 | 83 | break; |
| yueee_yt | 0:0280b02789e8 | 84 | } |
| yueee_yt | 0:0280b02789e8 | 85 | |
| yueee_yt | 0:0280b02789e8 | 86 | // The path is now two steps shorter. |
| yueee_yt | 0:0280b02789e8 | 87 | path_length -= 2; |
| yueee_yt | 0:0280b02789e8 | 88 | } |
| yueee_yt | 0:0280b02789e8 | 89 | |
| yueee_yt | 0:0280b02789e8 | 90 | void turn(char dir) { |
| yueee_yt | 0:0280b02789e8 | 91 | switch (dir) { |
| yueee_yt | 0:0280b02789e8 | 92 | case 'L': |
| yueee_yt | 0:0280b02789e8 | 93 | // Turn left. |
| yueee_yt | 0:0280b02789e8 | 94 | m3pi.left(0.2); |
| yueee_yt | 0:0280b02789e8 | 95 | wait(0.3125); |
| yueee_yt | 0:0280b02789e8 | 96 | break; |
| yueee_yt | 0:0280b02789e8 | 97 | case 'R': |
| yueee_yt | 0:0280b02789e8 | 98 | // Turn right. |
| yueee_yt | 0:0280b02789e8 | 99 | m3pi.right(0.2); |
| yueee_yt | 0:0280b02789e8 | 100 | wait(0.3125); |
| yueee_yt | 0:0280b02789e8 | 101 | break; |
| yueee_yt | 0:0280b02789e8 | 102 | case 'B': |
| yueee_yt | 0:0280b02789e8 | 103 | // Turn around. |
| yueee_yt | 0:0280b02789e8 | 104 | m3pi.left(0.2); |
| yueee_yt | 0:0280b02789e8 | 105 | wait(0.625); |
| yueee_yt | 0:0280b02789e8 | 106 | break; |
| yueee_yt | 0:0280b02789e8 | 107 | case 'S': |
| yueee_yt | 0:0280b02789e8 | 108 | // Don't do anything! |
| yueee_yt | 0:0280b02789e8 | 109 | break; |
| yueee_yt | 0:0280b02789e8 | 110 | } |
| yueee_yt | 0:0280b02789e8 | 111 | } |
| yueee_yt | 0:0280b02789e8 | 112 | |
| yueee_yt | 0:0280b02789e8 | 113 | |
| yueee_yt | 0:0280b02789e8 | 114 | int main() { |
| yueee_yt | 0:0280b02789e8 | 115 | unsigned int sensors[5]; |
| yueee_yt | 0:0280b02789e8 | 116 | |
| yueee_yt | 0:0280b02789e8 | 117 | sw.mode(PullUp); |
| yueee_yt | 0:0280b02789e8 | 118 | m3pi.locate(0,1); |
| yueee_yt | 0:0280b02789e8 | 119 | m3pi.printf("LineMaze"); |
| yueee_yt | 0:0280b02789e8 | 120 | m3pi.leds(0); |
| yueee_yt | 0:0280b02789e8 | 121 | wait(2.0); |
| yueee_yt | 0:0280b02789e8 | 122 | unsigned char ll=0; |
| yueee_yt | 0:0280b02789e8 | 123 | |
| yueee_yt | 0:0280b02789e8 | 124 | m3pi.sensor_auto_calibrate(); |
| yueee_yt | 0:0280b02789e8 | 125 | while (1) { |
| yueee_yt | 0:0280b02789e8 | 126 | follow_segment(); |
| yueee_yt | 0:0280b02789e8 | 127 | m3pi.left_motor(0.2); |
| yueee_yt | 0:0280b02789e8 | 128 | m3pi.right_motor(0.2); |
| yueee_yt | 0:0280b02789e8 | 129 | wait(0.02); |
| yueee_yt | 0:0280b02789e8 | 130 | //m3pi.left_motor(0);m3pi.right_motor(0); |
| yueee_yt | 0:0280b02789e8 | 131 | unsigned char found_left=0; |
| yueee_yt | 0:0280b02789e8 | 132 | unsigned char found_straight=0; |
| yueee_yt | 0:0280b02789e8 | 133 | unsigned char found_right=0; |
| yueee_yt | 0:0280b02789e8 | 134 | ll=0; |
| yueee_yt | 0:0280b02789e8 | 135 | read_line(sensors); |
| yueee_yt | 0:0280b02789e8 | 136 | if (sensors[0] > 1000) { |
| yueee_yt | 0:0280b02789e8 | 137 | found_left = 1; |
| yueee_yt | 0:0280b02789e8 | 138 | ll=ll|0x04; |
| yueee_yt | 0:0280b02789e8 | 139 | } |
| yueee_yt | 0:0280b02789e8 | 140 | if (sensors[4] > 1000) { |
| yueee_yt | 0:0280b02789e8 | 141 | found_right = 1; |
| yueee_yt | 0:0280b02789e8 | 142 | ll=ll|0x01; |
| yueee_yt | 0:0280b02789e8 | 143 | } |
| yueee_yt | 0:0280b02789e8 | 144 | wait(0.186); |
| yueee_yt | 0:0280b02789e8 | 145 | read_line(sensors); |
| yueee_yt | 0:0280b02789e8 | 146 | if (sensors[1] > 1000 || sensors[2] > 1000 || sensors[3] > 1000) { |
| yueee_yt | 0:0280b02789e8 | 147 | found_straight = 1; |
| yueee_yt | 0:0280b02789e8 | 148 | ll=ll|0x02; |
| yueee_yt | 0:0280b02789e8 | 149 | } |
| yueee_yt | 0:0280b02789e8 | 150 | if (sensors[1] > 1000 && sensors[2] > 1000 && sensors[3] > 1000) { |
| yueee_yt | 0:0280b02789e8 | 151 | ll=ll|0x08; |
| yueee_yt | 0:0280b02789e8 | 152 | break; |
| yueee_yt | 0:0280b02789e8 | 153 | // return; |
| yueee_yt | 0:0280b02789e8 | 154 | } |
| yueee_yt | 0:0280b02789e8 | 155 | unsigned char dir = select_turn(found_left, found_straight, found_right); |
| yueee_yt | 0:0280b02789e8 | 156 | turn(dir); |
| yueee_yt | 0:0280b02789e8 | 157 | path[path_length] = dir; |
| yueee_yt | 0:0280b02789e8 | 158 | path_length ++; |
| yueee_yt | 0:0280b02789e8 | 159 | |
| yueee_yt | 0:0280b02789e8 | 160 | // You should check to make sure that the path_length does not |
| yueee_yt | 0:0280b02789e8 | 161 | // exceed the bounds of the array. We'll ignore that in this |
| yueee_yt | 0:0280b02789e8 | 162 | // example. |
| yueee_yt | 0:0280b02789e8 | 163 | |
| yueee_yt | 0:0280b02789e8 | 164 | // Simplify the learned path. |
| yueee_yt | 0:0280b02789e8 | 165 | simplify_path(); |
| yueee_yt | 0:0280b02789e8 | 166 | m3pi.leds(ll); |
| yueee_yt | 0:0280b02789e8 | 167 | } |
| yueee_yt | 0:0280b02789e8 | 168 | m3pi.stop(); |
| yueee_yt | 0:0280b02789e8 | 169 | |
| yueee_yt | 0:0280b02789e8 | 170 | while (1) { |
| yueee_yt | 0:0280b02789e8 | 171 | if (sw==0)break; |
| yueee_yt | 0:0280b02789e8 | 172 | } |
| yueee_yt | 0:0280b02789e8 | 173 | wait(1); |
| yueee_yt | 0:0280b02789e8 | 174 | //2nd loop |
| yueee_yt | 0:0280b02789e8 | 175 | int i; |
| yueee_yt | 0:0280b02789e8 | 176 | for (i=0; i<path_length; i++) { |
| yueee_yt | 0:0280b02789e8 | 177 | // SECOND MAIN LOOP BODY |
| yueee_yt | 0:0280b02789e8 | 178 | follow_segment(); |
| yueee_yt | 0:0280b02789e8 | 179 | // Drive straight while slowing down, as before. |
| yueee_yt | 0:0280b02789e8 | 180 | m3pi.left_motor(0.2); |
| yueee_yt | 0:0280b02789e8 | 181 | m3pi.right_motor(0.2); |
| yueee_yt | 0:0280b02789e8 | 182 | wait(0.02); |
| yueee_yt | 0:0280b02789e8 | 183 | read_line(sensors); |
| yueee_yt | 0:0280b02789e8 | 184 | wait(0.186); |
| yueee_yt | 0:0280b02789e8 | 185 | |
| yueee_yt | 0:0280b02789e8 | 186 | |
| yueee_yt | 0:0280b02789e8 | 187 | // Make a turn according to the instruction stored in |
| yueee_yt | 0:0280b02789e8 | 188 | // path[i]. |
| yueee_yt | 0:0280b02789e8 | 189 | turn(path[i]); |
| yueee_yt | 0:0280b02789e8 | 190 | |
| yueee_yt | 0:0280b02789e8 | 191 | } |
| yueee_yt | 0:0280b02789e8 | 192 | |
| yueee_yt | 0:0280b02789e8 | 193 | // Follow the last segment up to the finish. |
| yueee_yt | 0:0280b02789e8 | 194 | follow_segment(); |
| yueee_yt | 0:0280b02789e8 | 195 | |
| yueee_yt | 0:0280b02789e8 | 196 | m3pi.stop(); |
| yueee_yt | 0:0280b02789e8 | 197 | |
| yueee_yt | 0:0280b02789e8 | 198 | |
| yueee_yt | 0:0280b02789e8 | 199 | } |
| yueee_yt | 0:0280b02789e8 | 200 | void follow_segment() { |
| yueee_yt | 0:0280b02789e8 | 201 | unsigned int sensors[5]; |
| yueee_yt | 0:0280b02789e8 | 202 | float right; |
| yueee_yt | 0:0280b02789e8 | 203 | float left; |
| yueee_yt | 0:0280b02789e8 | 204 | float current_pos_of_line = 0.0; |
| yueee_yt | 0:0280b02789e8 | 205 | float previous_pos_of_line = 0.0; |
| yueee_yt | 0:0280b02789e8 | 206 | float derivative,proportional,integral = 0; |
| yueee_yt | 0:0280b02789e8 | 207 | float power; |
| yueee_yt | 0:0280b02789e8 | 208 | float speed = MAX; |
| yueee_yt | 0:0280b02789e8 | 209 | |
| yueee_yt | 0:0280b02789e8 | 210 | while (1) { |
| yueee_yt | 0:0280b02789e8 | 211 | |
| yueee_yt | 0:0280b02789e8 | 212 | // Get the position of the line. |
| yueee_yt | 0:0280b02789e8 | 213 | current_pos_of_line = m3pi.line_position(); |
| yueee_yt | 0:0280b02789e8 | 214 | proportional = current_pos_of_line; |
| yueee_yt | 0:0280b02789e8 | 215 | |
| yueee_yt | 0:0280b02789e8 | 216 | // Compute the derivative |
| yueee_yt | 0:0280b02789e8 | 217 | derivative = current_pos_of_line - previous_pos_of_line; |
| yueee_yt | 0:0280b02789e8 | 218 | |
| yueee_yt | 0:0280b02789e8 | 219 | // Compute the integral |
| yueee_yt | 0:0280b02789e8 | 220 | integral += proportional; |
| yueee_yt | 0:0280b02789e8 | 221 | |
| yueee_yt | 0:0280b02789e8 | 222 | // Remember the last position. |
| yueee_yt | 0:0280b02789e8 | 223 | previous_pos_of_line = current_pos_of_line; |
| yueee_yt | 0:0280b02789e8 | 224 | |
| yueee_yt | 0:0280b02789e8 | 225 | // Compute the power |
| yueee_yt | 0:0280b02789e8 | 226 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
| yueee_yt | 0:0280b02789e8 | 227 | |
| yueee_yt | 0:0280b02789e8 | 228 | // Compute new speeds |
| yueee_yt | 0:0280b02789e8 | 229 | right = speed+power; |
| yueee_yt | 0:0280b02789e8 | 230 | left = speed-power; |
| yueee_yt | 0:0280b02789e8 | 231 | |
| yueee_yt | 0:0280b02789e8 | 232 | // limit checks |
| yueee_yt | 0:0280b02789e8 | 233 | if (right < MIN) |
| yueee_yt | 0:0280b02789e8 | 234 | right = MIN; |
| yueee_yt | 0:0280b02789e8 | 235 | else if (right > MAX) |
| yueee_yt | 0:0280b02789e8 | 236 | right = MAX; |
| yueee_yt | 0:0280b02789e8 | 237 | |
| yueee_yt | 0:0280b02789e8 | 238 | if (left < MIN) |
| yueee_yt | 0:0280b02789e8 | 239 | left = MIN; |
| yueee_yt | 0:0280b02789e8 | 240 | else if (left > MAX) |
| yueee_yt | 0:0280b02789e8 | 241 | left = MAX; |
| yueee_yt | 0:0280b02789e8 | 242 | |
| yueee_yt | 0:0280b02789e8 | 243 | // set speed |
| yueee_yt | 0:0280b02789e8 | 244 | m3pi.left_motor(left); |
| yueee_yt | 0:0280b02789e8 | 245 | m3pi.right_motor(right); |
| yueee_yt | 0:0280b02789e8 | 246 | //Read Sensor |
| yueee_yt | 0:0280b02789e8 | 247 | read_line(sensors); |
| yueee_yt | 0:0280b02789e8 | 248 | if (sensors[1] < 650 && sensors[2] < 650 && sensors[3] < 650) { |
| yueee_yt | 0:0280b02789e8 | 249 | // There is no line visible ahead, and we didn't see any |
| yueee_yt | 0:0280b02789e8 | 250 | // intersection. Must be a dead end. |
| yueee_yt | 0:0280b02789e8 | 251 | m3pi.leds(0x07); |
| yueee_yt | 0:0280b02789e8 | 252 | return; |
| yueee_yt | 0:0280b02789e8 | 253 | } else if (sensors[0] > 1000 || sensors[4] > 1000) { |
| yueee_yt | 0:0280b02789e8 | 254 | // Found an intersection. |
| yueee_yt | 0:0280b02789e8 | 255 | m3pi.leds(0x0f); |
| yueee_yt | 0:0280b02789e8 | 256 | return; |
| yueee_yt | 0:0280b02789e8 | 257 | } |
| yueee_yt | 0:0280b02789e8 | 258 | } |
| yueee_yt | 0:0280b02789e8 | 259 | } |