Allows the M3Pi to be used as a Sumo robot, using the sharp 100 distance sensors on the front. Run away strategy
Diff: main.cpp
- Revision:
- 0:11d0f3e0d1ad
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jun 18 09:38:17 2012 +0000 @@ -0,0 +1,179 @@ +/** +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; +} +