Jon Marsh
/
m3pi_MazeSolverLVC
Code to be modified by LVC students
main.cpp@0:64bbd41c50cb, 2011-03-10 (annotated)
- Committer:
- jonmarsh
- Date:
- Thu Mar 10 12:17:35 2011 +0000
- Revision:
- 0:64bbd41c50cb
- Child:
- 1:092dad68a533
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jonmarsh | 0:64bbd41c50cb | 1 | #include "mbed.h" |
jonmarsh | 0:64bbd41c50cb | 2 | #include "m3pimaze.h" |
jonmarsh | 0:64bbd41c50cb | 3 | |
jonmarsh | 0:64bbd41c50cb | 4 | BusOut leds(LED1,LED2,LED3,LED4); |
jonmarsh | 0:64bbd41c50cb | 5 | m3pi m3pi(p23,p9,p10); |
jonmarsh | 0:64bbd41c50cb | 6 | |
jonmarsh | 0:64bbd41c50cb | 7 | #define MAX 0.5 |
jonmarsh | 0:64bbd41c50cb | 8 | #define MIN 0 |
jonmarsh | 0:64bbd41c50cb | 9 | |
jonmarsh | 0:64bbd41c50cb | 10 | |
jonmarsh | 0:64bbd41c50cb | 11 | #define P_TERM 1 |
jonmarsh | 0:64bbd41c50cb | 12 | #define I_TERM 0 |
jonmarsh | 0:64bbd41c50cb | 13 | #define D_TERM 20 |
jonmarsh | 0:64bbd41c50cb | 14 | |
jonmarsh | 0:64bbd41c50cb | 15 | // Global variables |
jonmarsh | 0:64bbd41c50cb | 16 | // The path array stores the route info. Each element shows what we did at an intersection |
jonmarsh | 0:64bbd41c50cb | 17 | |
jonmarsh | 0:64bbd41c50cb | 18 | // 'L' for left |
jonmarsh | 0:64bbd41c50cb | 19 | // 'R' for right |
jonmarsh | 0:64bbd41c50cb | 20 | // 'F' for forward |
jonmarsh | 0:64bbd41c50cb | 21 | // 'B' for back |
jonmarsh | 0:64bbd41c50cb | 22 | // |
jonmarsh | 0:64bbd41c50cb | 23 | char path[1000] = ""; |
jonmarsh | 0:64bbd41c50cb | 24 | unsigned char path_length = 0; // the length of the path so far |
jonmarsh | 0:64bbd41c50cb | 25 | |
jonmarsh | 0:64bbd41c50cb | 26 | |
jonmarsh | 0:64bbd41c50cb | 27 | void follow_line() |
jonmarsh | 0:64bbd41c50cb | 28 | { |
jonmarsh | 0:64bbd41c50cb | 29 | float right; |
jonmarsh | 0:64bbd41c50cb | 30 | float left; |
jonmarsh | 0:64bbd41c50cb | 31 | float position_of_line = 0.0; |
jonmarsh | 0:64bbd41c50cb | 32 | float prev_pos_of_line = 0.0; |
jonmarsh | 0:64bbd41c50cb | 33 | float derivative,proportional; |
jonmarsh | 0:64bbd41c50cb | 34 | float integral = 0; |
jonmarsh | 0:64bbd41c50cb | 35 | float power; |
jonmarsh | 0:64bbd41c50cb | 36 | float speed = MAX; |
jonmarsh | 0:64bbd41c50cb | 37 | int foundjunction=0; |
jonmarsh | 0:64bbd41c50cb | 38 | |
jonmarsh | 0:64bbd41c50cb | 39 | |
jonmarsh | 0:64bbd41c50cb | 40 | int sensors[5]; |
jonmarsh | 0:64bbd41c50cb | 41 | while (foundjunction==0) { |
jonmarsh | 0:64bbd41c50cb | 42 | |
jonmarsh | 0:64bbd41c50cb | 43 | // Get the position of the line. |
jonmarsh | 0:64bbd41c50cb | 44 | position_of_line = m3pi.line_position(); |
jonmarsh | 0:64bbd41c50cb | 45 | proportional = position_of_line; |
jonmarsh | 0:64bbd41c50cb | 46 | // Compute the derivative |
jonmarsh | 0:64bbd41c50cb | 47 | derivative = position_of_line - prev_pos_of_line; |
jonmarsh | 0:64bbd41c50cb | 48 | // Compute the integral |
jonmarsh | 0:64bbd41c50cb | 49 | integral += proportional; |
jonmarsh | 0:64bbd41c50cb | 50 | // Remember the last position. |
jonmarsh | 0:64bbd41c50cb | 51 | prev_pos_of_line = position_of_line; |
jonmarsh | 0:64bbd41c50cb | 52 | // Compute |
jonmarsh | 0:64bbd41c50cb | 53 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
jonmarsh | 0:64bbd41c50cb | 54 | |
jonmarsh | 0:64bbd41c50cb | 55 | // Compute new speeds |
jonmarsh | 0:64bbd41c50cb | 56 | right = speed+power; |
jonmarsh | 0:64bbd41c50cb | 57 | left = speed-power; |
jonmarsh | 0:64bbd41c50cb | 58 | // limit checks |
jonmarsh | 0:64bbd41c50cb | 59 | if (right < MIN) |
jonmarsh | 0:64bbd41c50cb | 60 | right = MIN; |
jonmarsh | 0:64bbd41c50cb | 61 | else if (right > MAX) |
jonmarsh | 0:64bbd41c50cb | 62 | right = MAX; |
jonmarsh | 0:64bbd41c50cb | 63 | |
jonmarsh | 0:64bbd41c50cb | 64 | if (left < MIN) |
jonmarsh | 0:64bbd41c50cb | 65 | left = MIN; |
jonmarsh | 0:64bbd41c50cb | 66 | else if (left > MAX) |
jonmarsh | 0:64bbd41c50cb | 67 | left = MAX; |
jonmarsh | 0:64bbd41c50cb | 68 | |
jonmarsh | 0:64bbd41c50cb | 69 | // set speed |
jonmarsh | 0:64bbd41c50cb | 70 | m3pi.left_motor(left); |
jonmarsh | 0:64bbd41c50cb | 71 | m3pi.right_motor(right); |
jonmarsh | 0:64bbd41c50cb | 72 | |
jonmarsh | 0:64bbd41c50cb | 73 | m3pi.readsensor(sensors); |
jonmarsh | 0:64bbd41c50cb | 74 | |
jonmarsh | 0:64bbd41c50cb | 75 | if(sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100) |
jonmarsh | 0:64bbd41c50cb | 76 | { |
jonmarsh | 0:64bbd41c50cb | 77 | // There is no line visible ahead, and we didn't see any |
jonmarsh | 0:64bbd41c50cb | 78 | // intersection. Must be a dead end. |
jonmarsh | 0:64bbd41c50cb | 79 | foundjunction=1; |
jonmarsh | 0:64bbd41c50cb | 80 | } |
jonmarsh | 0:64bbd41c50cb | 81 | |
jonmarsh | 0:64bbd41c50cb | 82 | // you need to put some code here to look for a left or right junction |
jonmarsh | 0:64bbd41c50cb | 83 | |
jonmarsh | 0:64bbd41c50cb | 84 | |
jonmarsh | 0:64bbd41c50cb | 85 | } |
jonmarsh | 0:64bbd41c50cb | 86 | |
jonmarsh | 0:64bbd41c50cb | 87 | |
jonmarsh | 0:64bbd41c50cb | 88 | |
jonmarsh | 0:64bbd41c50cb | 89 | } |
jonmarsh | 0:64bbd41c50cb | 90 | |
jonmarsh | 0:64bbd41c50cb | 91 | // This function is called once, from main.c. |
jonmarsh | 0:64bbd41c50cb | 92 | void mazesolve() |
jonmarsh | 0:64bbd41c50cb | 93 | { |
jonmarsh | 0:64bbd41c50cb | 94 | // These variables record whether the robot has seen a line to the |
jonmarsh | 0:64bbd41c50cb | 95 | // left, straight ahead, and right, while examining the current |
jonmarsh | 0:64bbd41c50cb | 96 | // intersection. |
jonmarsh | 0:64bbd41c50cb | 97 | unsigned char found_left=0; |
jonmarsh | 0:64bbd41c50cb | 98 | unsigned char found_forward=0; |
jonmarsh | 0:64bbd41c50cb | 99 | unsigned char found_right=0; |
jonmarsh | 0:64bbd41c50cb | 100 | int sensors[5]; |
jonmarsh | 0:64bbd41c50cb | 101 | |
jonmarsh | 0:64bbd41c50cb | 102 | // Follow the line until an intersection is detected |
jonmarsh | 0:64bbd41c50cb | 103 | follow_line(); |
jonmarsh | 0:64bbd41c50cb | 104 | |
jonmarsh | 0:64bbd41c50cb | 105 | |
jonmarsh | 0:64bbd41c50cb | 106 | m3pi.forward(0.0); |
jonmarsh | 0:64bbd41c50cb | 107 | |
jonmarsh | 0:64bbd41c50cb | 108 | // Check for a forward exit. |
jonmarsh | 0:64bbd41c50cb | 109 | m3pi.readsensor(sensors); |
jonmarsh | 0:64bbd41c50cb | 110 | if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200) |
jonmarsh | 0:64bbd41c50cb | 111 | found_forward = 1; |
jonmarsh | 0:64bbd41c50cb | 112 | |
jonmarsh | 0:64bbd41c50cb | 113 | // Check for left and right exits. |
jonmarsh | 0:64bbd41c50cb | 114 | |
jonmarsh | 0:64bbd41c50cb | 115 | |
jonmarsh | 0:64bbd41c50cb | 116 | //debug code |
jonmarsh | 0:64bbd41c50cb | 117 | m3pi.cls(); |
jonmarsh | 0:64bbd41c50cb | 118 | if (found_left==1) |
jonmarsh | 0:64bbd41c50cb | 119 | m3pi.printf("L"); |
jonmarsh | 0:64bbd41c50cb | 120 | if (found_right==1) |
jonmarsh | 0:64bbd41c50cb | 121 | m3pi.printf("R"); |
jonmarsh | 0:64bbd41c50cb | 122 | if (found_forward==1) |
jonmarsh | 0:64bbd41c50cb | 123 | m3pi.printf("F"); |
jonmarsh | 0:64bbd41c50cb | 124 | wait (3); |
jonmarsh | 0:64bbd41c50cb | 125 | // Check for the ending spot. |
jonmarsh | 0:64bbd41c50cb | 126 | // If all five sensors are on dark black, we have |
jonmarsh | 0:64bbd41c50cb | 127 | // solved the maze. |
jonmarsh | 0:64bbd41c50cb | 128 | if(sensors[0]>600 && sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600 && sensors[4]>600) |
jonmarsh | 0:64bbd41c50cb | 129 | m3pi.printf("Done"); |
jonmarsh | 0:64bbd41c50cb | 130 | } |
jonmarsh | 0:64bbd41c50cb | 131 | |
jonmarsh | 0:64bbd41c50cb | 132 | void checksensors() |
jonmarsh | 0:64bbd41c50cb | 133 | { |
jonmarsh | 0:64bbd41c50cb | 134 | int sensors[5]; |
jonmarsh | 0:64bbd41c50cb | 135 | while (1) { |
jonmarsh | 0:64bbd41c50cb | 136 | m3pi.readsensor(sensors); |
jonmarsh | 0:64bbd41c50cb | 137 | m3pi.cls(); |
jonmarsh | 0:64bbd41c50cb | 138 | if (sensors[0]>200) |
jonmarsh | 0:64bbd41c50cb | 139 | m3pi.printf("D"); |
jonmarsh | 0:64bbd41c50cb | 140 | else m3pi.printf("L"); |
jonmarsh | 0:64bbd41c50cb | 141 | if (sensors[1]>200) |
jonmarsh | 0:64bbd41c50cb | 142 | m3pi.printf("D"); |
jonmarsh | 0:64bbd41c50cb | 143 | else m3pi.printf("L"); |
jonmarsh | 0:64bbd41c50cb | 144 | if (sensors[2]>200) |
jonmarsh | 0:64bbd41c50cb | 145 | m3pi.printf("D"); |
jonmarsh | 0:64bbd41c50cb | 146 | else m3pi.printf("L"); |
jonmarsh | 0:64bbd41c50cb | 147 | if (sensors[3]>200) |
jonmarsh | 0:64bbd41c50cb | 148 | m3pi.printf("D"); |
jonmarsh | 0:64bbd41c50cb | 149 | else m3pi.printf("L"); |
jonmarsh | 0:64bbd41c50cb | 150 | if (sensors[4]>200) |
jonmarsh | 0:64bbd41c50cb | 151 | m3pi.printf("D"); |
jonmarsh | 0:64bbd41c50cb | 152 | else m3pi.printf("L"); |
jonmarsh | 0:64bbd41c50cb | 153 | } |
jonmarsh | 0:64bbd41c50cb | 154 | } |
jonmarsh | 0:64bbd41c50cb | 155 | int main() { |
jonmarsh | 0:64bbd41c50cb | 156 | // int sensors[5]; |
jonmarsh | 0:64bbd41c50cb | 157 | m3pi.locate(0,1); |
jonmarsh | 0:64bbd41c50cb | 158 | m3pi.sensor_auto_calibrate(); |
jonmarsh | 0:64bbd41c50cb | 159 | m3pi.printf("MazeSolve"); |
jonmarsh | 0:64bbd41c50cb | 160 | |
jonmarsh | 0:64bbd41c50cb | 161 | wait(2.0); |
jonmarsh | 0:64bbd41c50cb | 162 | |
jonmarsh | 0:64bbd41c50cb | 163 | mazesolve(); |
jonmarsh | 0:64bbd41c50cb | 164 | |
jonmarsh | 0:64bbd41c50cb | 165 | m3pi.forward(0.0); |
jonmarsh | 0:64bbd41c50cb | 166 | |
jonmarsh | 0:64bbd41c50cb | 167 | } |