Jon Marsh
Testcode for LVC Robot club
main.cpp@0:f4922a2a0292, 2012-04-16 (annotated)
- Committer:
- jonmarsh
- Date:
- Mon Apr 16 12:25:11 2012 +0000
- Revision:
- 0:f4922a2a0292
- Child:
- 1:fe52aa73cd6a
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jonmarsh | 0:f4922a2a0292 | 1 | /** |
| jonmarsh | 0:f4922a2a0292 | 2 | Author: Giles Barton-Owen |
| jonmarsh | 0:f4922a2a0292 | 3 | Name: mbed ranger |
| jonmarsh | 0:f4922a2a0292 | 4 | Description: |
| jonmarsh | 0:f4922a2a0292 | 5 | |
| jonmarsh | 0:f4922a2a0292 | 6 | This program keeps a m3pi inside a ring, and has 6 (4 Active) different game modes. |
| jonmarsh | 0:f4922a2a0292 | 7 | It relies on a Sharp Digital Distance sensor (GP2Y0D810) attached to pin 30 and 11 |
| jonmarsh | 0:f4922a2a0292 | 8 | (the two headers on the right and left of the board). The system refreshes at 10Hz |
| jonmarsh | 0:f4922a2a0292 | 9 | when it reads the line sensors on the 3pi and the distance states. The states are |
| jonmarsh | 0:f4922a2a0292 | 10 | then handled in a big FSM to reach the conclusions. |
| jonmarsh | 0:f4922a2a0292 | 11 | |
| jonmarsh | 0:f4922a2a0292 | 12 | To keep the robot saving itself for longer a timeout was programmed, such that the |
| jonmarsh | 0:f4922a2a0292 | 13 | robot belives itself on a line (as was reported when the timeout was set) until |
| jonmarsh | 0:f4922a2a0292 | 14 | 0.3 seconds has elapsed. This could be adjusted. |
| jonmarsh | 0:f4922a2a0292 | 15 | |
| jonmarsh | 0:f4922a2a0292 | 16 | The fore-ground loop just switches the LEDs on the board as instructed. The current |
| jonmarsh | 0:f4922a2a0292 | 17 | state of the robot is reported on line 2 of the LCD screen on the 3pi, displaying |
| jonmarsh | 0:f4922a2a0292 | 18 | first mode and the last character being line detection state (none, front, back). |
| jonmarsh | 0:f4922a2a0292 | 19 | |
| jonmarsh | 0:f4922a2a0292 | 20 | |
| jonmarsh | 0:f4922a2a0292 | 21 | **/ |
| jonmarsh | 0:f4922a2a0292 | 22 | |
| jonmarsh | 0:f4922a2a0292 | 23 | #include "mbed.h" |
| jonmarsh | 0:f4922a2a0292 | 24 | #include "SharpDigiDist100.h" |
| jonmarsh | 0:f4922a2a0292 | 25 | #include "m3pi.h" |
| jonmarsh | 0:f4922a2a0292 | 26 | |
| jonmarsh | 0:f4922a2a0292 | 27 | DigitalOut Left[2] = {LED1,LED2}; //Some indicator LEDs for the range finders |
| jonmarsh | 0:f4922a2a0292 | 28 | DigitalOut Right[2] = {LED4,LED3}; |
| jonmarsh | 0:f4922a2a0292 | 29 | |
| jonmarsh | 0:f4922a2a0292 | 30 | SharpDigiDist100 right(p30); // The range finder class initialisations |
| jonmarsh | 0:f4922a2a0292 | 31 | SharpDigiDist100 left(p11); |
| jonmarsh | 0:f4922a2a0292 | 32 | |
| jonmarsh | 0:f4922a2a0292 | 33 | m3pi m3pi; // Initialise the m3pi |
| jonmarsh | 0:f4922a2a0292 | 34 | |
| jonmarsh | 0:f4922a2a0292 | 35 | InterruptIn button(p21); // SW1 on the shield, for stratergy switching |
| jonmarsh | 0:f4922a2a0292 | 36 | |
| jonmarsh | 0:f4922a2a0292 | 37 | Ticker guidance; // The main guidance caller |
| jonmarsh | 0:f4922a2a0292 | 38 | |
| jonmarsh | 0:f4922a2a0292 | 39 | Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging |
| jonmarsh | 0:f4922a2a0292 | 40 | |
| jonmarsh | 0:f4922a2a0292 | 41 | Timer debounce; // To debounce the switch |
| jonmarsh | 0:f4922a2a0292 | 42 | |
| jonmarsh | 0:f4922a2a0292 | 43 | int previousLine; // A set of variables to sort out the line detection, previousLine is pretty much not used |
| jonmarsh | 0:f4922a2a0292 | 44 | int isLine; |
| jonmarsh | 0:f4922a2a0292 | 45 | int lineCatch; |
| jonmarsh | 0:f4922a2a0292 | 46 | |
| jonmarsh | 0:f4922a2a0292 | 47 | Timeout liner; // A timeout to stop it ignoring the line |
| jonmarsh | 0:f4922a2a0292 | 48 | |
| jonmarsh | 0:f4922a2a0292 | 49 | void CycleMode(); // Function defs |
| jonmarsh | 0:f4922a2a0292 | 50 | void guideCall(); |
| jonmarsh | 0:f4922a2a0292 | 51 | void clearLiner(); |
| jonmarsh | 0:f4922a2a0292 | 52 | |
| jonmarsh | 0:f4922a2a0292 | 53 | enum Stratergies { // The enum for stratergies |
| jonmarsh | 0:f4922a2a0292 | 54 | Off, |
| jonmarsh | 0:f4922a2a0292 | 55 | Dormant, |
| jonmarsh | 0:f4922a2a0292 | 56 | RunMid, |
| jonmarsh | 0:f4922a2a0292 | 57 | RunNear, |
| jonmarsh | 0:f4922a2a0292 | 58 | AttackMid, |
| jonmarsh | 0:f4922a2a0292 | 59 | AttackFar, |
| jonmarsh | 0:f4922a2a0292 | 60 | Hunt |
| jonmarsh | 0:f4922a2a0292 | 61 | }; |
| jonmarsh | 0:f4922a2a0292 | 62 | |
| jonmarsh | 0:f4922a2a0292 | 63 | void updateScreen(int line, enum Stratergies stratergy); |
| jonmarsh | 0:f4922a2a0292 | 64 | |
| jonmarsh | 0:f4922a2a0292 | 65 | enum Stratergies stratMode = Off; // The current stratergy |
| jonmarsh | 0:f4922a2a0292 | 66 | |
| jonmarsh | 0:f4922a2a0292 | 67 | int main() { |
| jonmarsh | 0:f4922a2a0292 | 68 | // guidance.attach(&guideCall,0.1); // Sets up the control loop |
| jonmarsh | 0:f4922a2a0292 | 69 | |
| jonmarsh | 0:f4922a2a0292 | 70 | // button.mode(PullUp); // Sets the internal pull up on SW1 |
| jonmarsh | 0:f4922a2a0292 | 71 | // button.fall(&CycleMode); // Attaches an interupt for when it is pressed |
| jonmarsh | 0:f4922a2a0292 | 72 | |
| jonmarsh | 0:f4922a2a0292 | 73 | m3pi.locate(0,0); // Write the name to the screen |
| jonmarsh | 0:f4922a2a0292 | 74 | m3pi.printf("This is us"); |
| jonmarsh | 0:f4922a2a0292 | 75 | |
| jonmarsh | 0:f4922a2a0292 | 76 | m3pi.get_white_levels(); // Saves the current levels of the sensors to know what is white |
| jonmarsh | 0:f4922a2a0292 | 77 | |
| jonmarsh | 0:f4922a2a0292 | 78 | // debounce.start(); // Starts the debounce timer so that the switch thinks it isnt a bounce first time |
| jonmarsh | 0:f4922a2a0292 | 79 | |
| jonmarsh | 0:f4922a2a0292 | 80 | //pc.printf("\n\n\rMbedRanger\n\r"); // Prints a hello to the computer |
| jonmarsh | 0:f4922a2a0292 | 81 | |
| jonmarsh | 0:f4922a2a0292 | 82 | while (1) { |
| jonmarsh | 0:f4922a2a0292 | 83 | //pc.printf("Loop\n\r"); |
| jonmarsh | 0:f4922a2a0292 | 84 | switch (right.getDistance()) { // Sets up the distance indicator LEDs for the right side |
| jonmarsh | 0:f4922a2a0292 | 85 | case SharpDigiDist100::Far : |
| jonmarsh | 0:f4922a2a0292 | 86 | Right[0] = true; |
| jonmarsh | 0:f4922a2a0292 | 87 | Right[1] = false; |
| jonmarsh | 0:f4922a2a0292 | 88 | break; |
| jonmarsh | 0:f4922a2a0292 | 89 | case SharpDigiDist100::Near : |
| jonmarsh | 0:f4922a2a0292 | 90 | Right[1] = true; |
| jonmarsh | 0:f4922a2a0292 | 91 | Right[0] = false; |
| jonmarsh | 0:f4922a2a0292 | 92 | break; |
| jonmarsh | 0:f4922a2a0292 | 93 | case SharpDigiDist100::Mid : |
| jonmarsh | 0:f4922a2a0292 | 94 | Right[0] = true; |
| jonmarsh | 0:f4922a2a0292 | 95 | Right[1] = true; |
| jonmarsh | 0:f4922a2a0292 | 96 | break; |
| jonmarsh | 0:f4922a2a0292 | 97 | default: |
| jonmarsh | 0:f4922a2a0292 | 98 | break; |
| jonmarsh | 0:f4922a2a0292 | 99 | } |
| jonmarsh | 0:f4922a2a0292 | 100 | switch (left.getDistance()) { // Sets up the distance indicator LEDs for the left side |
| jonmarsh | 0:f4922a2a0292 | 101 | case SharpDigiDist100::Far : |
| jonmarsh | 0:f4922a2a0292 | 102 | Left[0] = true; |
| jonmarsh | 0:f4922a2a0292 | 103 | Left[1] = false; |
| jonmarsh | 0:f4922a2a0292 | 104 | break; |
| jonmarsh | 0:f4922a2a0292 | 105 | case SharpDigiDist100::Near : |
| jonmarsh | 0:f4922a2a0292 | 106 | Left[1] = true; |
| jonmarsh | 0:f4922a2a0292 | 107 | Left[0] = false; |
| jonmarsh | 0:f4922a2a0292 | 108 | break; |
| jonmarsh | 0:f4922a2a0292 | 109 | case SharpDigiDist100::Mid : |
| jonmarsh | 0:f4922a2a0292 | 110 | Left[0] = true; |
| jonmarsh | 0:f4922a2a0292 | 111 | Left[1] = true; |
| jonmarsh | 0:f4922a2a0292 | 112 | break; |
| jonmarsh | 0:f4922a2a0292 | 113 | default: |
| jonmarsh | 0:f4922a2a0292 | 114 | break; |
| jonmarsh | 0:f4922a2a0292 | 115 | } |
| jonmarsh | 0:f4922a2a0292 | 116 | |
| jonmarsh | 0:f4922a2a0292 | 117 | } |
| jonmarsh | 0:f4922a2a0292 | 118 | } |
| jonmarsh | 0:f4922a2a0292 | 119 | |
| jonmarsh | 0:f4922a2a0292 | 120 | |
| jonmarsh | 0:f4922a2a0292 | 121 | |
| jonmarsh | 0:f4922a2a0292 | 122 | void CycleMode() { // Cycles through the modes, probably could have written this with a simple ++ statement... |
| jonmarsh | 0:f4922a2a0292 | 123 | debounce.stop(); // Stops and reads the timer since the last press of the button |
| jonmarsh | 0:f4922a2a0292 | 124 | if (debounce.read_ms() > 100) { // If it was more than 100ms ago it treats it as a button press, otherwise it just ignores it |
| jonmarsh | 0:f4922a2a0292 | 125 | switch (stratMode) { |
| jonmarsh | 0:f4922a2a0292 | 126 | case Off: |
| jonmarsh | 0:f4922a2a0292 | 127 | stratMode = Dormant; |
| jonmarsh | 0:f4922a2a0292 | 128 | m3pi.locate(0,1); |
| jonmarsh | 0:f4922a2a0292 | 129 | m3pi.printf("\nDormant "); |
| jonmarsh | 0:f4922a2a0292 | 130 | break; |
| jonmarsh | 0:f4922a2a0292 | 131 | case Dormant: |
| jonmarsh | 0:f4922a2a0292 | 132 | stratMode = RunMid; |
| jonmarsh | 0:f4922a2a0292 | 133 | m3pi.locate(0,1); |
| jonmarsh | 0:f4922a2a0292 | 134 | m3pi.printf("\nRunMid "); |
| jonmarsh | 0:f4922a2a0292 | 135 | break; |
| jonmarsh | 0:f4922a2a0292 | 136 | case RunMid: |
| jonmarsh | 0:f4922a2a0292 | 137 | stratMode = RunNear; |
| jonmarsh | 0:f4922a2a0292 | 138 | m3pi.locate(0,1); |
| jonmarsh | 0:f4922a2a0292 | 139 | m3pi.printf("\nRunNear "); |
| jonmarsh | 0:f4922a2a0292 | 140 | break; |
| jonmarsh | 0:f4922a2a0292 | 141 | case RunNear: |
| jonmarsh | 0:f4922a2a0292 | 142 | stratMode = AttackMid; |
| jonmarsh | 0:f4922a2a0292 | 143 | m3pi.locate(0,1); |
| jonmarsh | 0:f4922a2a0292 | 144 | m3pi.printf("\nTakNear "); |
| jonmarsh | 0:f4922a2a0292 | 145 | break; |
| jonmarsh | 0:f4922a2a0292 | 146 | case AttackMid: |
| jonmarsh | 0:f4922a2a0292 | 147 | stratMode = AttackFar; |
| jonmarsh | 0:f4922a2a0292 | 148 | m3pi.locate(0,1); |
| jonmarsh | 0:f4922a2a0292 | 149 | m3pi.printf("\nTakFar "); |
| jonmarsh | 0:f4922a2a0292 | 150 | break; |
| jonmarsh | 0:f4922a2a0292 | 151 | case AttackFar: |
| jonmarsh | 0:f4922a2a0292 | 152 | stratMode = Hunt; |
| jonmarsh | 0:f4922a2a0292 | 153 | m3pi.locate(0,1); |
| jonmarsh | 0:f4922a2a0292 | 154 | m3pi.printf("\nHUNTING! "); |
| jonmarsh | 0:f4922a2a0292 | 155 | break; |
| jonmarsh | 0:f4922a2a0292 | 156 | case Hunt: |
| jonmarsh | 0:f4922a2a0292 | 157 | stratMode = Off; |
| jonmarsh | 0:f4922a2a0292 | 158 | m3pi.printf("\nOFF "); |
| jonmarsh | 0:f4922a2a0292 | 159 | break; |
| jonmarsh | 0:f4922a2a0292 | 160 | } |
| jonmarsh | 0:f4922a2a0292 | 161 | } |
| jonmarsh | 0:f4922a2a0292 | 162 | debounce.reset(); |
| jonmarsh | 0:f4922a2a0292 | 163 | debounce.start(); |
| jonmarsh | 0:f4922a2a0292 | 164 | } |
| jonmarsh | 0:f4922a2a0292 | 165 | |
| jonmarsh | 0:f4922a2a0292 | 166 | void guideCall() { |
| jonmarsh | 0:f4922a2a0292 | 167 | |
| jonmarsh | 0:f4922a2a0292 | 168 | isLine = m3pi.is_line(); // Gets whether the m3pi is on a line, and if so front/back |
| jonmarsh | 0:f4922a2a0292 | 169 | |
| jonmarsh | 0:f4922a2a0292 | 170 | if (lineCatch == 0) { // Has it been off a line for long enough? |
| jonmarsh | 0:f4922a2a0292 | 171 | isLine = isLine; // Yes - then go ahead |
| jonmarsh | 0:f4922a2a0292 | 172 | } else { |
| jonmarsh | 0:f4922a2a0292 | 173 | isLine = lineCatch; // No - pretend to still be on that line |
| jonmarsh | 0:f4922a2a0292 | 174 | |
| jonmarsh | 0:f4922a2a0292 | 175 | } |
| jonmarsh | 0:f4922a2a0292 | 176 | float position; |
| jonmarsh | 0:f4922a2a0292 | 177 | |
| jonmarsh | 0:f4922a2a0292 | 178 | switch (isLine) { |
| jonmarsh | 0:f4922a2a0292 | 179 | case 0: // No line, not even recently so go ahead with the stratergies |
| jonmarsh | 0:f4922a2a0292 | 180 | |
| jonmarsh | 0:f4922a2a0292 | 181 | updateScreen(isLine, stratMode); |
| jonmarsh | 0:f4922a2a0292 | 182 | bool atRight = false; |
| jonmarsh | 0:f4922a2a0292 | 183 | bool atLeft = false; |
| jonmarsh | 0:f4922a2a0292 | 184 | |
| jonmarsh | 0:f4922a2a0292 | 185 | switch (stratMode) { |
| jonmarsh | 0:f4922a2a0292 | 186 | case Off: // No motors |
| jonmarsh | 0:f4922a2a0292 | 187 | case Dormant: // Will take action against lines though |
| jonmarsh | 0:f4922a2a0292 | 188 | m3pi.stop(); |
| jonmarsh | 0:f4922a2a0292 | 189 | break; |
| jonmarsh | 0:f4922a2a0292 | 190 | |
| jonmarsh | 0:f4922a2a0292 | 191 | case RunNear: // Runs if something is near |
| jonmarsh | 0:f4922a2a0292 | 192 | if (right.getDistance() == SharpDigiDist100::Near) { |
| jonmarsh | 0:f4922a2a0292 | 193 | atRight = true; |
| jonmarsh | 0:f4922a2a0292 | 194 | } else atRight = false; |
| jonmarsh | 0:f4922a2a0292 | 195 | if (left.getDistance() == SharpDigiDist100::Near) { |
| jonmarsh | 0:f4922a2a0292 | 196 | atLeft = true; |
| jonmarsh | 0:f4922a2a0292 | 197 | } else atLeft = false; |
| jonmarsh | 0:f4922a2a0292 | 198 | |
| jonmarsh | 0:f4922a2a0292 | 199 | case RunMid: // Runs if something is in the middle distance |
| jonmarsh | 0:f4922a2a0292 | 200 | |
| jonmarsh | 0:f4922a2a0292 | 201 | if (right.getDistance() == SharpDigiDist100::Mid) { |
| jonmarsh | 0:f4922a2a0292 | 202 | atRight = true; |
| jonmarsh | 0:f4922a2a0292 | 203 | } |
| jonmarsh | 0:f4922a2a0292 | 204 | if (left.getDistance() == SharpDigiDist100::Mid) { |
| jonmarsh | 0:f4922a2a0292 | 205 | atLeft = true; |
| jonmarsh | 0:f4922a2a0292 | 206 | } |
| jonmarsh | 0:f4922a2a0292 | 207 | |
| jonmarsh | 0:f4922a2a0292 | 208 | |
| jonmarsh | 0:f4922a2a0292 | 209 | if (atRight && atLeft) { |
| jonmarsh | 0:f4922a2a0292 | 210 | m3pi.backward(0.5); |
| jonmarsh | 0:f4922a2a0292 | 211 | } else { |
| jonmarsh | 0:f4922a2a0292 | 212 | if (atRight == true) { |
| jonmarsh | 0:f4922a2a0292 | 213 | m3pi.left_motor(-0.3); |
| jonmarsh | 0:f4922a2a0292 | 214 | m3pi.right_motor(-0.5); |
| jonmarsh | 0:f4922a2a0292 | 215 | } else { |
| jonmarsh | 0:f4922a2a0292 | 216 | if (atLeft == true) { |
| jonmarsh | 0:f4922a2a0292 | 217 | m3pi.left_motor(-0.5); |
| jonmarsh | 0:f4922a2a0292 | 218 | m3pi.right_motor(-0.3); |
| jonmarsh | 0:f4922a2a0292 | 219 | } else { |
| jonmarsh | 0:f4922a2a0292 | 220 | m3pi.stop(); |
| jonmarsh | 0:f4922a2a0292 | 221 | } |
| jonmarsh | 0:f4922a2a0292 | 222 | } |
| jonmarsh | 0:f4922a2a0292 | 223 | } |
| jonmarsh | 0:f4922a2a0292 | 224 | break; |
| jonmarsh | 0:f4922a2a0292 | 225 | |
| jonmarsh | 0:f4922a2a0292 | 226 | case AttackMid: // Attacks something in the middle distance |
| jonmarsh | 0:f4922a2a0292 | 227 | if (right.getDistance() == SharpDigiDist100::Mid) { |
| jonmarsh | 0:f4922a2a0292 | 228 | atRight = true; |
| jonmarsh | 0:f4922a2a0292 | 229 | } |
| jonmarsh | 0:f4922a2a0292 | 230 | if (left.getDistance() == SharpDigiDist100::Mid) { |
| jonmarsh | 0:f4922a2a0292 | 231 | atLeft = true; |
| jonmarsh | 0:f4922a2a0292 | 232 | } |
| jonmarsh | 0:f4922a2a0292 | 233 | |
| jonmarsh | 0:f4922a2a0292 | 234 | |
| jonmarsh | 0:f4922a2a0292 | 235 | if (atRight && atLeft) { |
| jonmarsh | 0:f4922a2a0292 | 236 | m3pi.forward(0.6); |
| jonmarsh | 0:f4922a2a0292 | 237 | } else { |
| jonmarsh | 0:f4922a2a0292 | 238 | if (atRight == true) { |
| jonmarsh | 0:f4922a2a0292 | 239 | m3pi.left_motor(0.5); |
| jonmarsh | 0:f4922a2a0292 | 240 | m3pi.right_motor(0.7); |
| jonmarsh | 0:f4922a2a0292 | 241 | } else { |
| jonmarsh | 0:f4922a2a0292 | 242 | if (atLeft == true) { |
| jonmarsh | 0:f4922a2a0292 | 243 | m3pi.left_motor(0.7); |
| jonmarsh | 0:f4922a2a0292 | 244 | m3pi.right_motor(0.5); |
| jonmarsh | 0:f4922a2a0292 | 245 | } else { |
| jonmarsh | 0:f4922a2a0292 | 246 | m3pi.stop(); |
| jonmarsh | 0:f4922a2a0292 | 247 | } |
| jonmarsh | 0:f4922a2a0292 | 248 | } |
| jonmarsh | 0:f4922a2a0292 | 249 | } |
| jonmarsh | 0:f4922a2a0292 | 250 | |
| jonmarsh | 0:f4922a2a0292 | 251 | if (right.getDistance() == SharpDigiDist100::Near) { |
| jonmarsh | 0:f4922a2a0292 | 252 | atRight = true; |
| jonmarsh | 0:f4922a2a0292 | 253 | } else atRight = false; |
| jonmarsh | 0:f4922a2a0292 | 254 | if (left.getDistance() == SharpDigiDist100::Near) { |
| jonmarsh | 0:f4922a2a0292 | 255 | atLeft = true; |
| jonmarsh | 0:f4922a2a0292 | 256 | } else atLeft = false; |
| jonmarsh | 0:f4922a2a0292 | 257 | |
| jonmarsh | 0:f4922a2a0292 | 258 | |
| jonmarsh | 0:f4922a2a0292 | 259 | if (atRight && atLeft) { |
| jonmarsh | 0:f4922a2a0292 | 260 | m3pi.forward(0.5); |
| jonmarsh | 0:f4922a2a0292 | 261 | } else { |
| jonmarsh | 0:f4922a2a0292 | 262 | if (atRight == true) { |
| jonmarsh | 0:f4922a2a0292 | 263 | m3pi.left_motor(0.1); |
| jonmarsh | 0:f4922a2a0292 | 264 | m3pi.right_motor(0.2); |
| jonmarsh | 0:f4922a2a0292 | 265 | } else { |
| jonmarsh | 0:f4922a2a0292 | 266 | if (atLeft == true) { |
| jonmarsh | 0:f4922a2a0292 | 267 | m3pi.left_motor(0.2); |
| jonmarsh | 0:f4922a2a0292 | 268 | m3pi.right_motor(0.1); |
| jonmarsh | 0:f4922a2a0292 | 269 | } |
| jonmarsh | 0:f4922a2a0292 | 270 | } |
| jonmarsh | 0:f4922a2a0292 | 271 | } |
| jonmarsh | 0:f4922a2a0292 | 272 | |
| jonmarsh | 0:f4922a2a0292 | 273 | break; |
| jonmarsh | 0:f4922a2a0292 | 274 | |
| jonmarsh | 0:f4922a2a0292 | 275 | case AttackFar: |
| jonmarsh | 0:f4922a2a0292 | 276 | |
| jonmarsh | 0:f4922a2a0292 | 277 | break; |
| jonmarsh | 0:f4922a2a0292 | 278 | case Hunt: // Runs forward until something is really close |
| jonmarsh | 0:f4922a2a0292 | 279 | |
| jonmarsh | 0:f4922a2a0292 | 280 | if (right.getDistance() == SharpDigiDist100::Mid || right.getDistance() == SharpDigiDist100::Near) { |
| jonmarsh | 0:f4922a2a0292 | 281 | atRight = true; |
| jonmarsh | 0:f4922a2a0292 | 282 | } else atRight = false; |
| jonmarsh | 0:f4922a2a0292 | 283 | if (left.getDistance() == SharpDigiDist100::Mid || left.getDistance() == SharpDigiDist100::Near) { |
| jonmarsh | 0:f4922a2a0292 | 284 | atLeft = true; |
| jonmarsh | 0:f4922a2a0292 | 285 | } else atLeft = false; |
| jonmarsh | 0:f4922a2a0292 | 286 | |
| jonmarsh | 0:f4922a2a0292 | 287 | if (atRight && atLeft) { |
| jonmarsh | 0:f4922a2a0292 | 288 | m3pi.stop(); |
| jonmarsh | 0:f4922a2a0292 | 289 | } else { |
| jonmarsh | 0:f4922a2a0292 | 290 | if (atRight == true) { |
| jonmarsh | 0:f4922a2a0292 | 291 | m3pi.left_motor(0.1); |
| jonmarsh | 0:f4922a2a0292 | 292 | m3pi.right_motor(0.2); |
| jonmarsh | 0:f4922a2a0292 | 293 | } else { |
| jonmarsh | 0:f4922a2a0292 | 294 | if (atLeft == true) { |
| jonmarsh | 0:f4922a2a0292 | 295 | m3pi.left_motor(0.2); |
| jonmarsh | 0:f4922a2a0292 | 296 | m3pi.right_motor(0.1); |
| jonmarsh | 0:f4922a2a0292 | 297 | } else { |
| jonmarsh | 0:f4922a2a0292 | 298 | m3pi.forward(0.3); |
| jonmarsh | 0:f4922a2a0292 | 299 | } |
| jonmarsh | 0:f4922a2a0292 | 300 | } |
| jonmarsh | 0:f4922a2a0292 | 301 | } |
| jonmarsh | 0:f4922a2a0292 | 302 | break; |
| jonmarsh | 0:f4922a2a0292 | 303 | default: |
| jonmarsh | 0:f4922a2a0292 | 304 | break; |
| jonmarsh | 0:f4922a2a0292 | 305 | } |
| jonmarsh | 0:f4922a2a0292 | 306 | break; |
| jonmarsh | 0:f4922a2a0292 | 307 | |
| jonmarsh | 0:f4922a2a0292 | 308 | case 1: // Line in front, reverse |
| jonmarsh | 0:f4922a2a0292 | 309 | if(stratMode != Off) |
| jonmarsh | 0:f4922a2a0292 | 310 | { |
| jonmarsh | 0:f4922a2a0292 | 311 | if (lineCatch == 0) { |
| jonmarsh | 0:f4922a2a0292 | 312 | lineCatch = 1; |
| jonmarsh | 0:f4922a2a0292 | 313 | |
| jonmarsh | 0:f4922a2a0292 | 314 | liner.attach(&clearLiner, 0.3); |
| jonmarsh | 0:f4922a2a0292 | 315 | } |
| jonmarsh | 0:f4922a2a0292 | 316 | |
| jonmarsh | 0:f4922a2a0292 | 317 | position = m3pi.line_position(); |
| jonmarsh | 0:f4922a2a0292 | 318 | if (position < 0) { |
| jonmarsh | 0:f4922a2a0292 | 319 | m3pi.left_motor(-1); |
| jonmarsh | 0:f4922a2a0292 | 320 | m3pi.right_motor(-0.8); |
| jonmarsh | 0:f4922a2a0292 | 321 | } else if (position == 0) { |
| jonmarsh | 0:f4922a2a0292 | 322 | m3pi.backward(1); |
| jonmarsh | 0:f4922a2a0292 | 323 | } else if (position > 0) { |
| jonmarsh | 0:f4922a2a0292 | 324 | m3pi.left_motor(-0.8); |
| jonmarsh | 0:f4922a2a0292 | 325 | m3pi.right_motor(-1); |
| jonmarsh | 0:f4922a2a0292 | 326 | } |
| jonmarsh | 0:f4922a2a0292 | 327 | |
| jonmarsh | 0:f4922a2a0292 | 328 | //m3pi.locate(0,1); |
| jonmarsh | 0:f4922a2a0292 | 329 | //m3pi.printf("LINE_FWD"); |
| jonmarsh | 0:f4922a2a0292 | 330 | } |
| jonmarsh | 0:f4922a2a0292 | 331 | else |
| jonmarsh | 0:f4922a2a0292 | 332 | { |
| jonmarsh | 0:f4922a2a0292 | 333 | m3pi.stop(); |
| jonmarsh | 0:f4922a2a0292 | 334 | } |
| jonmarsh | 0:f4922a2a0292 | 335 | updateScreen(isLine, stratMode); |
| jonmarsh | 0:f4922a2a0292 | 336 | break; |
| jonmarsh | 0:f4922a2a0292 | 337 | |
| jonmarsh | 0:f4922a2a0292 | 338 | case -1: // Line behind, forward |
| jonmarsh | 0:f4922a2a0292 | 339 | if(stratMode != Off) |
| jonmarsh | 0:f4922a2a0292 | 340 | { |
| jonmarsh | 0:f4922a2a0292 | 341 | if (lineCatch == 0) { |
| jonmarsh | 0:f4922a2a0292 | 342 | lineCatch = -1; |
| jonmarsh | 0:f4922a2a0292 | 343 | liner.attach(&clearLiner, 0.3); |
| jonmarsh | 0:f4922a2a0292 | 344 | } |
| jonmarsh | 0:f4922a2a0292 | 345 | |
| jonmarsh | 0:f4922a2a0292 | 346 | |
| jonmarsh | 0:f4922a2a0292 | 347 | position = m3pi.line_position(); |
| jonmarsh | 0:f4922a2a0292 | 348 | if (position < 0) { |
| jonmarsh | 0:f4922a2a0292 | 349 | m3pi.left_motor(1); |
| jonmarsh | 0:f4922a2a0292 | 350 | m3pi.right_motor(0.8); |
| jonmarsh | 0:f4922a2a0292 | 351 | } else if (position == 0) { |
| jonmarsh | 0:f4922a2a0292 | 352 | m3pi.forward(1); |
| jonmarsh | 0:f4922a2a0292 | 353 | } else if (position > 0) { |
| jonmarsh | 0:f4922a2a0292 | 354 | m3pi.left_motor(0.8); |
| jonmarsh | 0:f4922a2a0292 | 355 | m3pi.right_motor(1); |
| jonmarsh | 0:f4922a2a0292 | 356 | } |
| jonmarsh | 0:f4922a2a0292 | 357 | } |
| jonmarsh | 0:f4922a2a0292 | 358 | else |
| jonmarsh | 0:f4922a2a0292 | 359 | { |
| jonmarsh | 0:f4922a2a0292 | 360 | m3pi.stop(); |
| jonmarsh | 0:f4922a2a0292 | 361 | } |
| jonmarsh | 0:f4922a2a0292 | 362 | //m3pi.locate(0,1); |
| jonmarsh | 0:f4922a2a0292 | 363 | //m3pi.printf("LINE_BKD"); |
| jonmarsh | 0:f4922a2a0292 | 364 | updateScreen(isLine, stratMode); |
| jonmarsh | 0:f4922a2a0292 | 365 | break; |
| jonmarsh | 0:f4922a2a0292 | 366 | } |
| jonmarsh | 0:f4922a2a0292 | 367 | |
| jonmarsh | 0:f4922a2a0292 | 368 | //previousLine = isLine; |
| jonmarsh | 0:f4922a2a0292 | 369 | } |
| jonmarsh | 0:f4922a2a0292 | 370 | |
| jonmarsh | 0:f4922a2a0292 | 371 | void clearLiner() { // Gets called a bit after a line is detected |
| jonmarsh | 0:f4922a2a0292 | 372 | lineCatch = 0; |
| jonmarsh | 0:f4922a2a0292 | 373 | //pc.printf("Cleared liner\n\r"); |
| jonmarsh | 0:f4922a2a0292 | 374 | } |
| jonmarsh | 0:f4922a2a0292 | 375 | |
| jonmarsh | 0:f4922a2a0292 | 376 | void updateScreen(int line, enum Stratergies stratergy) { // Update the bottom line with the running info |
| jonmarsh | 0:f4922a2a0292 | 377 | m3pi.locate(0,1); |
| jonmarsh | 0:f4922a2a0292 | 378 | |
| jonmarsh | 0:f4922a2a0292 | 379 | char lineState; |
| jonmarsh | 0:f4922a2a0292 | 380 | |
| jonmarsh | 0:f4922a2a0292 | 381 | switch (line) { |
| jonmarsh | 0:f4922a2a0292 | 382 | case 0: |
| jonmarsh | 0:f4922a2a0292 | 383 | lineState = 'N'; |
| jonmarsh | 0:f4922a2a0292 | 384 | break; |
| jonmarsh | 0:f4922a2a0292 | 385 | case 1: |
| jonmarsh | 0:f4922a2a0292 | 386 | lineState = 'F'; |
| jonmarsh | 0:f4922a2a0292 | 387 | break; |
| jonmarsh | 0:f4922a2a0292 | 388 | case -1: |
| jonmarsh | 0:f4922a2a0292 | 389 | lineState = 'B'; |
| jonmarsh | 0:f4922a2a0292 | 390 | break; |
| jonmarsh | 0:f4922a2a0292 | 391 | default: |
| jonmarsh | 0:f4922a2a0292 | 392 | break; |
| jonmarsh | 0:f4922a2a0292 | 393 | } |
| jonmarsh | 0:f4922a2a0292 | 394 | |
| jonmarsh | 0:f4922a2a0292 | 395 | char strat[6]; |
| jonmarsh | 0:f4922a2a0292 | 396 | switch (stratergy) { |
| jonmarsh | 0:f4922a2a0292 | 397 | case Off: |
| jonmarsh | 0:f4922a2a0292 | 398 | sprintf(strat,"OFF "); |
| jonmarsh | 0:f4922a2a0292 | 399 | break; |
| jonmarsh | 0:f4922a2a0292 | 400 | case Dormant: |
| jonmarsh | 0:f4922a2a0292 | 401 | sprintf(strat,"DORMNT"); |
| jonmarsh | 0:f4922a2a0292 | 402 | break; |
| jonmarsh | 0:f4922a2a0292 | 403 | case RunMid: |
| jonmarsh | 0:f4922a2a0292 | 404 | sprintf(strat,"RUNMID"); |
| jonmarsh | 0:f4922a2a0292 | 405 | |
| jonmarsh | 0:f4922a2a0292 | 406 | break; |
| jonmarsh | 0:f4922a2a0292 | 407 | case RunNear: |
| jonmarsh | 0:f4922a2a0292 | 408 | sprintf(strat,"RNNEAR"); |
| jonmarsh | 0:f4922a2a0292 | 409 | |
| jonmarsh | 0:f4922a2a0292 | 410 | break; |
| jonmarsh | 0:f4922a2a0292 | 411 | case AttackMid: |
| jonmarsh | 0:f4922a2a0292 | 412 | sprintf(strat,"TAKMID"); |
| jonmarsh | 0:f4922a2a0292 | 413 | |
| jonmarsh | 0:f4922a2a0292 | 414 | break; |
| jonmarsh | 0:f4922a2a0292 | 415 | case AttackFar: |
| jonmarsh | 0:f4922a2a0292 | 416 | sprintf(strat,"TAKFAR"); |
| jonmarsh | 0:f4922a2a0292 | 417 | |
| jonmarsh | 0:f4922a2a0292 | 418 | break; |
| jonmarsh | 0:f4922a2a0292 | 419 | case Hunt: |
| jonmarsh | 0:f4922a2a0292 | 420 | sprintf(strat,"HUNTER"); |
| jonmarsh | 0:f4922a2a0292 | 421 | |
| jonmarsh | 0:f4922a2a0292 | 422 | break; |
| jonmarsh | 0:f4922a2a0292 | 423 | } |
| jonmarsh | 0:f4922a2a0292 | 424 | |
| jonmarsh | 0:f4922a2a0292 | 425 | m3pi.printf("%s %c",strat,lineState); |
| jonmarsh | 0:f4922a2a0292 | 426 | } |