Jon Marsh
Code to be modified by LVC students
main.cpp@1:092dad68a533, 2011-03-16 (annotated)
- Committer:
- jonmarsh
- Date:
- Wed Mar 16 11:56:59 2011 +0000
- Revision:
- 1:092dad68a533
- Parent:
- 0:64bbd41c50cb
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 | } |
| jonmarsh | 0:64bbd41c50cb | 126 | |
| jonmarsh | 0:64bbd41c50cb | 127 | void checksensors() |
| jonmarsh | 0:64bbd41c50cb | 128 | { |
| jonmarsh | 0:64bbd41c50cb | 129 | int sensors[5]; |
| jonmarsh | 0:64bbd41c50cb | 130 | while (1) { |
| jonmarsh | 0:64bbd41c50cb | 131 | m3pi.readsensor(sensors); |
| jonmarsh | 0:64bbd41c50cb | 132 | m3pi.cls(); |
| jonmarsh | 0:64bbd41c50cb | 133 | if (sensors[0]>200) |
| jonmarsh | 0:64bbd41c50cb | 134 | m3pi.printf("D"); |
| jonmarsh | 0:64bbd41c50cb | 135 | else m3pi.printf("L"); |
| jonmarsh | 0:64bbd41c50cb | 136 | if (sensors[1]>200) |
| jonmarsh | 0:64bbd41c50cb | 137 | m3pi.printf("D"); |
| jonmarsh | 0:64bbd41c50cb | 138 | else m3pi.printf("L"); |
| jonmarsh | 0:64bbd41c50cb | 139 | if (sensors[2]>200) |
| jonmarsh | 0:64bbd41c50cb | 140 | m3pi.printf("D"); |
| jonmarsh | 0:64bbd41c50cb | 141 | else m3pi.printf("L"); |
| jonmarsh | 0:64bbd41c50cb | 142 | if (sensors[3]>200) |
| jonmarsh | 0:64bbd41c50cb | 143 | m3pi.printf("D"); |
| jonmarsh | 0:64bbd41c50cb | 144 | else m3pi.printf("L"); |
| jonmarsh | 0:64bbd41c50cb | 145 | if (sensors[4]>200) |
| jonmarsh | 0:64bbd41c50cb | 146 | m3pi.printf("D"); |
| jonmarsh | 0:64bbd41c50cb | 147 | else m3pi.printf("L"); |
| jonmarsh | 0:64bbd41c50cb | 148 | } |
| jonmarsh | 0:64bbd41c50cb | 149 | } |
| jonmarsh | 0:64bbd41c50cb | 150 | int main() { |
| jonmarsh | 0:64bbd41c50cb | 151 | // int sensors[5]; |
| jonmarsh | 0:64bbd41c50cb | 152 | m3pi.locate(0,1); |
| jonmarsh | 0:64bbd41c50cb | 153 | m3pi.sensor_auto_calibrate(); |
| jonmarsh | 0:64bbd41c50cb | 154 | m3pi.printf("MazeSolve"); |
| jonmarsh | 0:64bbd41c50cb | 155 | |
| jonmarsh | 0:64bbd41c50cb | 156 | wait(2.0); |
| jonmarsh | 0:64bbd41c50cb | 157 | |
| jonmarsh | 0:64bbd41c50cb | 158 | mazesolve(); |
| jonmarsh | 0:64bbd41c50cb | 159 | |
| jonmarsh | 0:64bbd41c50cb | 160 | m3pi.forward(0.0); |
| jonmarsh | 0:64bbd41c50cb | 161 | |
| jonmarsh | 0:64bbd41c50cb | 162 | } |