Allows the M3Pi to be used as a Sumo robot, using the sharp 100 distance sensors on the front. Run away strategy
main.cpp
- Committer:
- jonmarsh
- Date:
- 2012-06-18
- Revision:
- 0:11d0f3e0d1ad
File content as of revision 0:11d0f3e0d1ad:
/** Name: mpi hunter Description: Based on work done by Giles Barton-Owen Cut down for use at Linton Village College This program keeps a m3pi inside a ring and looks for the other robot (and runs away from it) **/ #include "mbed.h" #include "SharpDigiDist100.h" #include "m3pi.h" DigitalOut Left[2] = {LED1,LED2}; //Some indicator LEDs for the range finders DigitalOut Right[2] = {LED4,LED3}; SharpDigiDist100 right(p30); // The range finder class initialisations SharpDigiDist100 left(p11); m3pi m3pi; // Initialise the m3pi InterruptIn button(p21); // SW1 on the shield, for strategy switching Ticker guidance; // The main guidance caller Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging int previousLine; // A set of variables to sort out the line detection, previousLine is pretty much not used int isLine; int lineCatch; Timeout liner; // A timeout to stop it ignoring the line void CycleMode(); // Function defs void guideCall(); void clearLiner(); int main() { guidance.attach(&guideCall,0.1); // Sets up the control loop m3pi.locate(0,0); // Write the name to the screen m3pi.printf("m3PiRngr"); m3pi.get_white_levels(); // Saves the current levels of the sensors to know what is white while (1) { switch (right.getDistance()) { // Sets up the distance indicator LEDs for the right side case SharpDigiDist100::Far : Right[0] = true; Right[1] = false; break; case SharpDigiDist100::Near : Right[1] = true; Right[0] = false; break; case SharpDigiDist100::Mid : Right[0] = true; Right[1] = true; break; default: break; } switch (left.getDistance()) { // Sets up the distance indicator LEDs for the left side case SharpDigiDist100::Far : Left[0] = true; Left[1] = false; break; case SharpDigiDist100::Near : Left[1] = true; Left[0] = false; break; case SharpDigiDist100::Mid : Left[0] = true; Left[1] = true; break; default: break; } } } void guideCall() { isLine = m3pi.is_line(); // Gets whether the m3pi is on a line, and if so front/back if (lineCatch == 0) { // Has it been off a line for long enough? isLine = isLine; // Yes - then go ahead } else { isLine = lineCatch; // No - pretend to still be on that line } float position; switch (isLine) { case 0: // No line, not even recently so go ahead with the strategies { bool atRight = false; bool atLeft = false; if (right.getDistance() == SharpDigiDist100::Near) { atRight = true; } else atRight = false; if (left.getDistance() == SharpDigiDist100::Near) { atLeft = true; } else atLeft = false; if (atRight && atLeft) { m3pi.backward(0.5); } else { if (atRight == true) { m3pi.left_motor(-0.3); m3pi.right_motor(-0.5); } else { if (atLeft == true) { m3pi.left_motor(-0.5); m3pi.right_motor(-0.3); } else { m3pi.stop(); } } } } break; case 1: // Line in front, reverse if (lineCatch == 0) { lineCatch = 1; liner.attach(&clearLiner, 0.3); } position = m3pi.line_position(); if (position < 0) { m3pi.left_motor(-1); m3pi.right_motor(-0.8); } else if (position == 0) { m3pi.backward(1); } else if (position > 0) { m3pi.left_motor(-0.8); m3pi.right_motor(-1); } //m3pi.locate(0,1); //m3pi.printf("LINE_FWD"); break; case -1: // Line behind, forward if (lineCatch == 0) { lineCatch = -1; liner.attach(&clearLiner, 0.3); } position = m3pi.line_position(); if (position < 0) { m3pi.left_motor(1); m3pi.right_motor(0.8); } else if (position == 0) { m3pi.forward(1); } else if (position > 0) { m3pi.left_motor(0.8); m3pi.right_motor(1); } break; } //previousLine = isLine; } void clearLiner() { // Gets called a bit after a line is detected lineCatch = 0; }