Yasushi TAUCHI
LineMaze
main.cpp@0:85792b24326d, 2012-06-26 (annotated)
- Committer:
- yueee_yt
- Date:
- Tue Jun 26 03:02:06 2012 +0000
- Revision:
- 0:85792b24326d
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| yueee_yt | 0:85792b24326d | 1 | #include "mbed.h" |
| yueee_yt | 0:85792b24326d | 2 | #include "m3pi.h" |
| yueee_yt | 0:85792b24326d | 3 | |
| yueee_yt | 0:85792b24326d | 4 | m3pi m3pi; |
| yueee_yt | 0:85792b24326d | 5 | |
| yueee_yt | 0:85792b24326d | 6 | // Minimum and maximum motor speeds |
| yueee_yt | 0:85792b24326d | 7 | #define MAX 0.3 |
| yueee_yt | 0:85792b24326d | 8 | #define MAX2 0.6 |
| yueee_yt | 0:85792b24326d | 9 | #define MIN 0 |
| yueee_yt | 0:85792b24326d | 10 | |
| yueee_yt | 0:85792b24326d | 11 | // PID terms |
| yueee_yt | 0:85792b24326d | 12 | #define P_TERM 0.6 |
| yueee_yt | 0:85792b24326d | 13 | #define I_TERM 0 |
| yueee_yt | 0:85792b24326d | 14 | #define D_TERM 20 |
| yueee_yt | 0:85792b24326d | 15 | DigitalIn sw(p21); |
| yueee_yt | 0:85792b24326d | 16 | char path[100] = ""; |
| yueee_yt | 0:85792b24326d | 17 | unsigned char path_length = 0; |
| yueee_yt | 0:85792b24326d | 18 | |
| yueee_yt | 0:85792b24326d | 19 | void follow_segment(void); |
| yueee_yt | 0:85792b24326d | 20 | void follow_segment2(void); |
| yueee_yt | 0:85792b24326d | 21 | void m3pi_play( char x[]) { |
| yueee_yt | 0:85792b24326d | 22 | m3pi.putc(0xb3); |
| yueee_yt | 0:85792b24326d | 23 | int i; |
| yueee_yt | 0:85792b24326d | 24 | m3pi.putc(strlen(x)); |
| yueee_yt | 0:85792b24326d | 25 | for (i=0; i<strlen(x); i++) { |
| yueee_yt | 0:85792b24326d | 26 | m3pi.putc(x[i]); |
| yueee_yt | 0:85792b24326d | 27 | } |
| yueee_yt | 0:85792b24326d | 28 | } |
| yueee_yt | 0:85792b24326d | 29 | void read_line(unsigned int sensors[5]) { |
| yueee_yt | 0:85792b24326d | 30 | unsigned char x[10]; |
| yueee_yt | 0:85792b24326d | 31 | int i; |
| yueee_yt | 0:85792b24326d | 32 | m3pi.putc(0x87); |
| yueee_yt | 0:85792b24326d | 33 | for (i=0; i<10; i++) { |
| yueee_yt | 0:85792b24326d | 34 | x[i]=m3pi.getc(); |
| yueee_yt | 0:85792b24326d | 35 | } |
| yueee_yt | 0:85792b24326d | 36 | sensors[0]=(x[1]*0x100+x[0])*2; |
| yueee_yt | 0:85792b24326d | 37 | sensors[1]=(x[3]*0x100+x[2])*2; |
| yueee_yt | 0:85792b24326d | 38 | sensors[2]=(x[5]*0x100+x[4])*2; |
| yueee_yt | 0:85792b24326d | 39 | sensors[3]=(x[7]*0x100+x[6])*2; |
| yueee_yt | 0:85792b24326d | 40 | sensors[4]=(x[9]*0x100+x[8])*2; |
| yueee_yt | 0:85792b24326d | 41 | } |
| yueee_yt | 0:85792b24326d | 42 | |
| yueee_yt | 0:85792b24326d | 43 | char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right) { |
| yueee_yt | 0:85792b24326d | 44 | // Make a decision about how to turn. The following code |
| yueee_yt | 0:85792b24326d | 45 | // implements a left-hand-on-the-wall strategy, where we always |
| yueee_yt | 0:85792b24326d | 46 | // turn as far to the left as possible. |
| yueee_yt | 0:85792b24326d | 47 | if (found_left) |
| yueee_yt | 0:85792b24326d | 48 | return 'L'; |
| yueee_yt | 0:85792b24326d | 49 | else if (found_straight) |
| yueee_yt | 0:85792b24326d | 50 | return 'S'; |
| yueee_yt | 0:85792b24326d | 51 | else if (found_right) |
| yueee_yt | 0:85792b24326d | 52 | return 'R'; |
| yueee_yt | 0:85792b24326d | 53 | else |
| yueee_yt | 0:85792b24326d | 54 | return 'B'; |
| yueee_yt | 0:85792b24326d | 55 | } |
| yueee_yt | 0:85792b24326d | 56 | void simplify_path() { |
| yueee_yt | 0:85792b24326d | 57 | // only simplify the path if the second-to-last turn was a 'B' |
| yueee_yt | 0:85792b24326d | 58 | if (path_length < 3 || path[path_length-2] != 'B') |
| yueee_yt | 0:85792b24326d | 59 | return; |
| yueee_yt | 0:85792b24326d | 60 | |
| yueee_yt | 0:85792b24326d | 61 | int total_angle = 0; |
| yueee_yt | 0:85792b24326d | 62 | int i; |
| yueee_yt | 0:85792b24326d | 63 | for (i=1; i<=3; i++) { |
| yueee_yt | 0:85792b24326d | 64 | switch (path[path_length-i]) { |
| yueee_yt | 0:85792b24326d | 65 | case 'R': |
| yueee_yt | 0:85792b24326d | 66 | total_angle += 90; |
| yueee_yt | 0:85792b24326d | 67 | break; |
| yueee_yt | 0:85792b24326d | 68 | case 'L': |
| yueee_yt | 0:85792b24326d | 69 | total_angle += 270; |
| yueee_yt | 0:85792b24326d | 70 | break; |
| yueee_yt | 0:85792b24326d | 71 | case 'B': |
| yueee_yt | 0:85792b24326d | 72 | total_angle += 180; |
| yueee_yt | 0:85792b24326d | 73 | break; |
| yueee_yt | 0:85792b24326d | 74 | } |
| yueee_yt | 0:85792b24326d | 75 | } |
| yueee_yt | 0:85792b24326d | 76 | |
| yueee_yt | 0:85792b24326d | 77 | // Get the angle as a number between 0 and 360 degrees. |
| yueee_yt | 0:85792b24326d | 78 | total_angle = total_angle % 360; |
| yueee_yt | 0:85792b24326d | 79 | |
| yueee_yt | 0:85792b24326d | 80 | // Replace all of those turns with a single one. |
| yueee_yt | 0:85792b24326d | 81 | switch (total_angle) { |
| yueee_yt | 0:85792b24326d | 82 | case 0: |
| yueee_yt | 0:85792b24326d | 83 | path[path_length - 3] = 'S'; |
| yueee_yt | 0:85792b24326d | 84 | break; |
| yueee_yt | 0:85792b24326d | 85 | case 90: |
| yueee_yt | 0:85792b24326d | 86 | path[path_length - 3] = 'R'; |
| yueee_yt | 0:85792b24326d | 87 | break; |
| yueee_yt | 0:85792b24326d | 88 | case 180: |
| yueee_yt | 0:85792b24326d | 89 | path[path_length - 3] = 'B'; |
| yueee_yt | 0:85792b24326d | 90 | break; |
| yueee_yt | 0:85792b24326d | 91 | case 270: |
| yueee_yt | 0:85792b24326d | 92 | path[path_length - 3] = 'L'; |
| yueee_yt | 0:85792b24326d | 93 | break; |
| yueee_yt | 0:85792b24326d | 94 | } |
| yueee_yt | 0:85792b24326d | 95 | |
| yueee_yt | 0:85792b24326d | 96 | // The path is now two steps shorter. |
| yueee_yt | 0:85792b24326d | 97 | path_length -= 2; |
| yueee_yt | 0:85792b24326d | 98 | } |
| yueee_yt | 0:85792b24326d | 99 | |
| yueee_yt | 0:85792b24326d | 100 | void turn(char dir) { |
| yueee_yt | 0:85792b24326d | 101 | float a=0.0347; |
| yueee_yt | 0:85792b24326d | 102 | switch (dir) { |
| yueee_yt | 0:85792b24326d | 103 | case 'L': |
| yueee_yt | 0:85792b24326d | 104 | // Turn left. |
| yueee_yt | 0:85792b24326d | 105 | m3pi_play("T240ec"); |
| yueee_yt | 0:85792b24326d | 106 | m3pi.left(0.2); |
| yueee_yt | 0:85792b24326d | 107 | wait(0.3125+a); |
| yueee_yt | 0:85792b24326d | 108 | break; |
| yueee_yt | 0:85792b24326d | 109 | case 'R': |
| yueee_yt | 0:85792b24326d | 110 | // Turn right. |
| yueee_yt | 0:85792b24326d | 111 | m3pi_play("T240ce"); |
| yueee_yt | 0:85792b24326d | 112 | m3pi.right(0.2); |
| yueee_yt | 0:85792b24326d | 113 | wait(0.3125+a); |
| yueee_yt | 0:85792b24326d | 114 | break; |
| yueee_yt | 0:85792b24326d | 115 | case 'B': |
| yueee_yt | 0:85792b24326d | 116 | // Turn around. |
| yueee_yt | 0:85792b24326d | 117 | m3pi_play("T240cc"); |
| yueee_yt | 0:85792b24326d | 118 | |
| yueee_yt | 0:85792b24326d | 119 | m3pi.left(0.2); |
| yueee_yt | 0:85792b24326d | 120 | wait(0.625+a); |
| yueee_yt | 0:85792b24326d | 121 | break; |
| yueee_yt | 0:85792b24326d | 122 | case 'S': |
| yueee_yt | 0:85792b24326d | 123 | // Don't do anything! |
| yueee_yt | 0:85792b24326d | 124 | break; |
| yueee_yt | 0:85792b24326d | 125 | } |
| yueee_yt | 0:85792b24326d | 126 | } |
| yueee_yt | 0:85792b24326d | 127 | |
| yueee_yt | 0:85792b24326d | 128 | void turn2(char dir) { |
| yueee_yt | 0:85792b24326d | 129 | float a=0.0347; |
| yueee_yt | 0:85792b24326d | 130 | switch (dir) { |
| yueee_yt | 0:85792b24326d | 131 | case 'L': |
| yueee_yt | 0:85792b24326d | 132 | // Turn left. |
| yueee_yt | 0:85792b24326d | 133 | m3pi_play("T240ec"); |
| yueee_yt | 0:85792b24326d | 134 | m3pi.right_motor(0.0); |
| yueee_yt | 0:85792b24326d | 135 | m3pi.left_motor(0.6); |
| yueee_yt | 0:85792b24326d | 136 | wait(0.223); |
| yueee_yt | 0:85792b24326d | 137 | |
| yueee_yt | 0:85792b24326d | 138 | break; |
| yueee_yt | 0:85792b24326d | 139 | case 'R': |
| yueee_yt | 0:85792b24326d | 140 | // Turn right. |
| yueee_yt | 0:85792b24326d | 141 | m3pi_play("T240ce"); |
| yueee_yt | 0:85792b24326d | 142 | m3pi.right_motor(0.6); |
| yueee_yt | 0:85792b24326d | 143 | m3pi.left_motor(0); |
| yueee_yt | 0:85792b24326d | 144 | wait(0.223); |
| yueee_yt | 0:85792b24326d | 145 | break; |
| yueee_yt | 0:85792b24326d | 146 | case 'B': |
| yueee_yt | 0:85792b24326d | 147 | // Turn around. |
| yueee_yt | 0:85792b24326d | 148 | m3pi_play("T240cc"); |
| yueee_yt | 0:85792b24326d | 149 | |
| yueee_yt | 0:85792b24326d | 150 | m3pi.left(0.2); |
| yueee_yt | 0:85792b24326d | 151 | wait(0.625+a); |
| yueee_yt | 0:85792b24326d | 152 | break; |
| yueee_yt | 0:85792b24326d | 153 | case 'S': |
| yueee_yt | 0:85792b24326d | 154 | // Don't do anything! |
| yueee_yt | 0:85792b24326d | 155 | m3pi.forward(0.3); |
| yueee_yt | 0:85792b24326d | 156 | wait(0.2+a); |
| yueee_yt | 0:85792b24326d | 157 | break; |
| yueee_yt | 0:85792b24326d | 158 | } |
| yueee_yt | 0:85792b24326d | 159 | } |
| yueee_yt | 0:85792b24326d | 160 | |
| yueee_yt | 0:85792b24326d | 161 | |
| yueee_yt | 0:85792b24326d | 162 | |
| yueee_yt | 0:85792b24326d | 163 | int main() { |
| yueee_yt | 0:85792b24326d | 164 | unsigned int sensors[5]; |
| yueee_yt | 0:85792b24326d | 165 | |
| yueee_yt | 0:85792b24326d | 166 | sw.mode(PullUp); |
| yueee_yt | 0:85792b24326d | 167 | m3pi.locate(0,1); |
| yueee_yt | 0:85792b24326d | 168 | m3pi.printf("LineMaze"); |
| yueee_yt | 0:85792b24326d | 169 | m3pi.leds(0); |
| yueee_yt | 0:85792b24326d | 170 | wait(2.0); |
| yueee_yt | 0:85792b24326d | 171 | unsigned char ll=0; |
| yueee_yt | 0:85792b24326d | 172 | |
| yueee_yt | 0:85792b24326d | 173 | m3pi.sensor_auto_calibrate(); |
| yueee_yt | 0:85792b24326d | 174 | while (1) { |
| yueee_yt | 0:85792b24326d | 175 | follow_segment(); |
| yueee_yt | 0:85792b24326d | 176 | m3pi.left_motor(0.2); |
| yueee_yt | 0:85792b24326d | 177 | m3pi.right_motor(0.2); |
| yueee_yt | 0:85792b24326d | 178 | wait(0.02); |
| yueee_yt | 0:85792b24326d | 179 | //m3pi.left_motor(0);m3pi.right_motor(0); |
| yueee_yt | 0:85792b24326d | 180 | unsigned char found_left=0; |
| yueee_yt | 0:85792b24326d | 181 | unsigned char found_straight=0; |
| yueee_yt | 0:85792b24326d | 182 | unsigned char found_right=0; |
| yueee_yt | 0:85792b24326d | 183 | ll=0; |
| yueee_yt | 0:85792b24326d | 184 | read_line(sensors); |
| yueee_yt | 0:85792b24326d | 185 | if (sensors[0] > 1000) { |
| yueee_yt | 0:85792b24326d | 186 | found_left = 1; |
| yueee_yt | 0:85792b24326d | 187 | ll=ll|0x04; |
| yueee_yt | 0:85792b24326d | 188 | } |
| yueee_yt | 0:85792b24326d | 189 | if (sensors[4] > 1000) { |
| yueee_yt | 0:85792b24326d | 190 | found_right = 1; |
| yueee_yt | 0:85792b24326d | 191 | ll=ll|0x01; |
| yueee_yt | 0:85792b24326d | 192 | } |
| yueee_yt | 0:85792b24326d | 193 | wait(0.186); |
| yueee_yt | 0:85792b24326d | 194 | read_line(sensors); |
| yueee_yt | 0:85792b24326d | 195 | if (sensors[1] > 1000 || sensors[2] > 1000 || sensors[3] > 1000) { |
| yueee_yt | 0:85792b24326d | 196 | found_straight = 1; |
| yueee_yt | 0:85792b24326d | 197 | ll=ll|0x02; |
| yueee_yt | 0:85792b24326d | 198 | } |
| yueee_yt | 0:85792b24326d | 199 | if (sensors[1] > 1000 && sensors[2] > 1000 && sensors[3] > 1000) { |
| yueee_yt | 0:85792b24326d | 200 | ll=ll|0x08; |
| yueee_yt | 0:85792b24326d | 201 | break; |
| yueee_yt | 0:85792b24326d | 202 | // return; |
| yueee_yt | 0:85792b24326d | 203 | } |
| yueee_yt | 0:85792b24326d | 204 | unsigned char dir = select_turn(found_left, found_straight, found_right); |
| yueee_yt | 0:85792b24326d | 205 | turn(dir); |
| yueee_yt | 0:85792b24326d | 206 | path[path_length] = dir; |
| yueee_yt | 0:85792b24326d | 207 | path_length ++; |
| yueee_yt | 0:85792b24326d | 208 | |
| yueee_yt | 0:85792b24326d | 209 | // You should check to make sure that the path_length does not |
| yueee_yt | 0:85792b24326d | 210 | // exceed the bounds of the array. We'll ignore that in this |
| yueee_yt | 0:85792b24326d | 211 | // example. |
| yueee_yt | 0:85792b24326d | 212 | |
| yueee_yt | 0:85792b24326d | 213 | // Simplify the learned path. |
| yueee_yt | 0:85792b24326d | 214 | simplify_path(); |
| yueee_yt | 0:85792b24326d | 215 | m3pi.leds(ll); |
| yueee_yt | 0:85792b24326d | 216 | } |
| yueee_yt | 0:85792b24326d | 217 | m3pi.stop(); |
| yueee_yt | 0:85792b24326d | 218 | |
| yueee_yt | 0:85792b24326d | 219 | while (1) { |
| yueee_yt | 0:85792b24326d | 220 | if (sw==0)break; |
| yueee_yt | 0:85792b24326d | 221 | } |
| yueee_yt | 0:85792b24326d | 222 | wait(1); |
| yueee_yt | 0:85792b24326d | 223 | //2nd loop |
| yueee_yt | 0:85792b24326d | 224 | int i; |
| yueee_yt | 0:85792b24326d | 225 | for (i=0; i<path_length; i++) { |
| yueee_yt | 0:85792b24326d | 226 | // SECOND MAIN LOOP BODY |
| yueee_yt | 0:85792b24326d | 227 | follow_segment2(); |
| yueee_yt | 0:85792b24326d | 228 | // Drive straight while slowing down, as before. |
| yueee_yt | 0:85792b24326d | 229 | // m3pi.left_motor(0.2); |
| yueee_yt | 0:85792b24326d | 230 | // m3pi.right_motor(0.2); |
| yueee_yt | 0:85792b24326d | 231 | // wait(0.02); |
| yueee_yt | 0:85792b24326d | 232 | // read_line(sensors); |
| yueee_yt | 0:85792b24326d | 233 | // wait(0.186); |
| yueee_yt | 0:85792b24326d | 234 | |
| yueee_yt | 0:85792b24326d | 235 | |
| yueee_yt | 0:85792b24326d | 236 | // Make a turn according to the instruction stored in |
| yueee_yt | 0:85792b24326d | 237 | // path[i]. |
| yueee_yt | 0:85792b24326d | 238 | turn2(path[i]); |
| yueee_yt | 0:85792b24326d | 239 | |
| yueee_yt | 0:85792b24326d | 240 | } |
| yueee_yt | 0:85792b24326d | 241 | |
| yueee_yt | 0:85792b24326d | 242 | // Follow the last segment up to the finish. |
| yueee_yt | 0:85792b24326d | 243 | follow_segment2(); |
| yueee_yt | 0:85792b24326d | 244 | |
| yueee_yt | 0:85792b24326d | 245 | m3pi.stop(); |
| yueee_yt | 0:85792b24326d | 246 | |
| yueee_yt | 0:85792b24326d | 247 | |
| yueee_yt | 0:85792b24326d | 248 | } |
| yueee_yt | 0:85792b24326d | 249 | void follow_segment() { |
| yueee_yt | 0:85792b24326d | 250 | unsigned int sensors[5]; |
| yueee_yt | 0:85792b24326d | 251 | float right; |
| yueee_yt | 0:85792b24326d | 252 | float left; |
| yueee_yt | 0:85792b24326d | 253 | float current_pos_of_line = 0.0; |
| yueee_yt | 0:85792b24326d | 254 | float previous_pos_of_line = 0.0; |
| yueee_yt | 0:85792b24326d | 255 | float derivative,proportional,integral = 0; |
| yueee_yt | 0:85792b24326d | 256 | float power; |
| yueee_yt | 0:85792b24326d | 257 | float speed = MAX; |
| yueee_yt | 0:85792b24326d | 258 | int k=0; |
| yueee_yt | 0:85792b24326d | 259 | while (1) { |
| yueee_yt | 0:85792b24326d | 260 | |
| yueee_yt | 0:85792b24326d | 261 | // Get the position of the line. |
| yueee_yt | 0:85792b24326d | 262 | current_pos_of_line = m3pi.line_position(); |
| yueee_yt | 0:85792b24326d | 263 | proportional = current_pos_of_line; |
| yueee_yt | 0:85792b24326d | 264 | |
| yueee_yt | 0:85792b24326d | 265 | // Compute the derivative |
| yueee_yt | 0:85792b24326d | 266 | derivative = current_pos_of_line - previous_pos_of_line; |
| yueee_yt | 0:85792b24326d | 267 | |
| yueee_yt | 0:85792b24326d | 268 | // Compute the integral |
| yueee_yt | 0:85792b24326d | 269 | integral += proportional; |
| yueee_yt | 0:85792b24326d | 270 | |
| yueee_yt | 0:85792b24326d | 271 | // Remember the last position. |
| yueee_yt | 0:85792b24326d | 272 | previous_pos_of_line = current_pos_of_line; |
| yueee_yt | 0:85792b24326d | 273 | |
| yueee_yt | 0:85792b24326d | 274 | // Compute the power |
| yueee_yt | 0:85792b24326d | 275 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
| yueee_yt | 0:85792b24326d | 276 | |
| yueee_yt | 0:85792b24326d | 277 | // Compute new speeds |
| yueee_yt | 0:85792b24326d | 278 | right = speed+power; |
| yueee_yt | 0:85792b24326d | 279 | left = speed-power; |
| yueee_yt | 0:85792b24326d | 280 | |
| yueee_yt | 0:85792b24326d | 281 | // limit checks |
| yueee_yt | 0:85792b24326d | 282 | if (right < MIN) |
| yueee_yt | 0:85792b24326d | 283 | right = MIN; |
| yueee_yt | 0:85792b24326d | 284 | else if (right > MAX) |
| yueee_yt | 0:85792b24326d | 285 | right = MAX; |
| yueee_yt | 0:85792b24326d | 286 | |
| yueee_yt | 0:85792b24326d | 287 | if (left < MIN) |
| yueee_yt | 0:85792b24326d | 288 | left = MIN; |
| yueee_yt | 0:85792b24326d | 289 | else if (left > MAX) |
| yueee_yt | 0:85792b24326d | 290 | left = MAX; |
| yueee_yt | 0:85792b24326d | 291 | |
| yueee_yt | 0:85792b24326d | 292 | // set speed |
| yueee_yt | 0:85792b24326d | 293 | m3pi.left_motor(left); |
| yueee_yt | 0:85792b24326d | 294 | m3pi.right_motor(right); |
| yueee_yt | 0:85792b24326d | 295 | //Read Sensor |
| yueee_yt | 0:85792b24326d | 296 | if (k>200) { |
| yueee_yt | 0:85792b24326d | 297 | read_line(sensors); |
| yueee_yt | 0:85792b24326d | 298 | if (sensors[1] < 650 && sensors[2] < 650 && sensors[3] < 650) { |
| yueee_yt | 0:85792b24326d | 299 | // There is no line visible ahead, and we didn't see any |
| yueee_yt | 0:85792b24326d | 300 | // intersection. Must be a dead end. |
| yueee_yt | 0:85792b24326d | 301 | m3pi.leds(0x07); |
| yueee_yt | 0:85792b24326d | 302 | return; |
| yueee_yt | 0:85792b24326d | 303 | } else if (sensors[0] > 1000 || sensors[4] > 1000) { |
| yueee_yt | 0:85792b24326d | 304 | // Found an intersection. |
| yueee_yt | 0:85792b24326d | 305 | m3pi.leds(0x0f); |
| yueee_yt | 0:85792b24326d | 306 | return; |
| yueee_yt | 0:85792b24326d | 307 | } |
| yueee_yt | 0:85792b24326d | 308 | } |
| yueee_yt | 0:85792b24326d | 309 | k++; |
| yueee_yt | 0:85792b24326d | 310 | } |
| yueee_yt | 0:85792b24326d | 311 | } |
| yueee_yt | 0:85792b24326d | 312 | void follow_segment2() { |
| yueee_yt | 0:85792b24326d | 313 | unsigned int sensors[5]; |
| yueee_yt | 0:85792b24326d | 314 | float right; |
| yueee_yt | 0:85792b24326d | 315 | float left; |
| yueee_yt | 0:85792b24326d | 316 | float current_pos_of_line = 0.0; |
| yueee_yt | 0:85792b24326d | 317 | float previous_pos_of_line = 0.0; |
| yueee_yt | 0:85792b24326d | 318 | float derivative,proportional,integral = 0; |
| yueee_yt | 0:85792b24326d | 319 | float power; |
| yueee_yt | 0:85792b24326d | 320 | float speed = MAX2; |
| yueee_yt | 0:85792b24326d | 321 | int k=0; |
| yueee_yt | 0:85792b24326d | 322 | while (1) { |
| yueee_yt | 0:85792b24326d | 323 | |
| yueee_yt | 0:85792b24326d | 324 | // Get the position of the line. |
| yueee_yt | 0:85792b24326d | 325 | current_pos_of_line = m3pi.line_position(); |
| yueee_yt | 0:85792b24326d | 326 | proportional = current_pos_of_line; |
| yueee_yt | 0:85792b24326d | 327 | |
| yueee_yt | 0:85792b24326d | 328 | // Compute the derivative |
| yueee_yt | 0:85792b24326d | 329 | derivative = current_pos_of_line - previous_pos_of_line; |
| yueee_yt | 0:85792b24326d | 330 | |
| yueee_yt | 0:85792b24326d | 331 | // Compute the integral |
| yueee_yt | 0:85792b24326d | 332 | integral += proportional; |
| yueee_yt | 0:85792b24326d | 333 | |
| yueee_yt | 0:85792b24326d | 334 | // Remember the last position. |
| yueee_yt | 0:85792b24326d | 335 | previous_pos_of_line = current_pos_of_line; |
| yueee_yt | 0:85792b24326d | 336 | |
| yueee_yt | 0:85792b24326d | 337 | // Compute the power |
| yueee_yt | 0:85792b24326d | 338 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
| yueee_yt | 0:85792b24326d | 339 | |
| yueee_yt | 0:85792b24326d | 340 | // Compute new speeds |
| yueee_yt | 0:85792b24326d | 341 | right = speed+power; |
| yueee_yt | 0:85792b24326d | 342 | left = speed-power; |
| yueee_yt | 0:85792b24326d | 343 | |
| yueee_yt | 0:85792b24326d | 344 | // limit checks |
| yueee_yt | 0:85792b24326d | 345 | if (right < MIN) |
| yueee_yt | 0:85792b24326d | 346 | right = MIN; |
| yueee_yt | 0:85792b24326d | 347 | else if (right > MAX2) |
| yueee_yt | 0:85792b24326d | 348 | right = MAX2; |
| yueee_yt | 0:85792b24326d | 349 | |
| yueee_yt | 0:85792b24326d | 350 | if (left < MIN) |
| yueee_yt | 0:85792b24326d | 351 | left = MIN; |
| yueee_yt | 0:85792b24326d | 352 | else if (left > MAX2) |
| yueee_yt | 0:85792b24326d | 353 | left = MAX2; |
| yueee_yt | 0:85792b24326d | 354 | |
| yueee_yt | 0:85792b24326d | 355 | // set speed |
| yueee_yt | 0:85792b24326d | 356 | m3pi.left_motor(left); |
| yueee_yt | 0:85792b24326d | 357 | m3pi.right_motor(right); |
| yueee_yt | 0:85792b24326d | 358 | //Read Sensor |
| yueee_yt | 0:85792b24326d | 359 | if (k>100) { |
| yueee_yt | 0:85792b24326d | 360 | read_line(sensors); |
| yueee_yt | 0:85792b24326d | 361 | if (sensors[1] < 650 && sensors[2] < 650 && sensors[3] < 650) { |
| yueee_yt | 0:85792b24326d | 362 | // There is no line visible ahead, and we didn't see any |
| yueee_yt | 0:85792b24326d | 363 | // intersection. Must be a dead end. |
| yueee_yt | 0:85792b24326d | 364 | m3pi.leds(0x07); |
| yueee_yt | 0:85792b24326d | 365 | return; |
| yueee_yt | 0:85792b24326d | 366 | } else if (sensors[0] > 1000 || sensors[4] > 1000) { |
| yueee_yt | 0:85792b24326d | 367 | // Found an intersection. |
| yueee_yt | 0:85792b24326d | 368 | m3pi.leds(0x0f); |
| yueee_yt | 0:85792b24326d | 369 | return; |
| yueee_yt | 0:85792b24326d | 370 | } |
| yueee_yt | 0:85792b24326d | 371 | } |
| yueee_yt | 0:85792b24326d | 372 | k++; |
| yueee_yt | 0:85792b24326d | 373 | } |
| yueee_yt | 0:85792b24326d | 374 | } |