Giles Barton-Owen / Mbed 2 deprecated MbedRanger

Allows the M3Pi to be used as a Sumo robot, using the sharp 100 distance sensors on the front.

Dependencies:   mbed

main.cpp@0:342c14fb10c0, 2012-03-14 (annotated)

Committer:
p07gbar
Date:
Wed Mar 14 20:31:09 2012 +0000
Revision:
0:342c14fb10c0
First upload

Who changed what in which revision?

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