Giles Barton-Owen
Allows the M3Pi to be used as a Sumo robot, using the sharp 100 distance sensors on the front.
Diff: main.cpp
- Revision:
- 0:342c14fb10c0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Mar 14 20:31:09 2012 +0000
@@ -0,0 +1,438 @@
+/**
+Author: Giles Barton-Owen
+Name: mbed ranger
+Description:
+
+This program keeps a m3pi inside a ring, and has 6 (4 Active) different game modes.
+It relies on a Sharp Digital Distance sensor (GP2Y0D810) attached to pin 30 and 11
+(the two headers on the right and left of the board). The system refreshes at 10Hz
+when it reads the line sensors on the 3pi and the distance states. The states are
+then handled in a big FSM to reach the conclusions.
+
+To keep the robot saving itself for longer a timeout was programmed, such that the
+robot belives itself on a line (as was reported when the timeout was set) until
+0.3 seconds has elapsed. This could be adjusted.
+
+The fore-ground loop just switches the LEDs on the board as instructed. The current
+state of the robot is reported on line 2 of the LCD screen on the 3pi, displaying
+first mode and the last character being line detection state (none, front, back).
+
+
+**/
+
+#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 stratergy switching
+
+Ticker guidance; // The main guidance caller
+
+Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging
+
+Timer debounce; // To debounce the switch
+
+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();
+
+enum Stratergies { // The enum for stratergies
+ Off,
+ Dormant,
+ RunMid,
+ RunNear,
+ AttackMid,
+ AttackFar,
+ Hunt
+};
+
+void updateScreen(int line, enum Stratergies stratergy);
+
+enum Stratergies stratMode = Off; // The current stratergy
+
+int main() {
+ guidance.attach(&guideCall,0.1); // Sets up the control loop
+
+ button.mode(PullUp); // Sets the internal pull up on SW1
+ button.fall(&CycleMode); // Attaches an interupt for when it is pressed
+
+ 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
+
+ debounce.start(); // Starts the debounce timer so that the switch thinks it isnt a bounce first time
+
+ //pc.printf("\n\n\rMbedRanger\n\r"); // Prints a hello to the computer
+
+ while (1) {
+ //pc.printf("Loop\n\r");
+ 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;
+ }
+ /*int calibrated[5];
+ m3pi.get_raw_sensors(calibrated);
+ pc.printf("Sensors read:");
+ for (int i = 0; i<5;i++) {
+ pc.printf(" %i ",calibrated[i]);
+ }
+ if (m3pi.is_line() > 0) {
+ pc.printf("LINE");
+ m3pi.locate(0,1);
+ m3pi.printf("LINE");
+ }
+ pc.printf("\n\r");
+ wait(0.1);*/
+ }
+}
+
+
+
+void CycleMode() { // Cycles through the modes, probably could have written this with a simple ++ statement...
+ debounce.stop(); // Stops and reads the timer since the last press of the button
+ if (debounce.read_ms() > 100) { // If it was more than 100ms ago it treats it as a button press, otherwise it just ignores it
+ switch (stratMode) {
+ case Off:
+ stratMode = Dormant;
+ m3pi.locate(0,1);
+ m3pi.printf("\nDormant ");
+ break;
+ case Dormant:
+ stratMode = RunMid;
+ m3pi.locate(0,1);
+ m3pi.printf("\nRunMid ");
+ break;
+ case RunMid:
+ stratMode = RunNear;
+ m3pi.locate(0,1);
+ m3pi.printf("\nRunNear ");
+ break;
+ case RunNear:
+ stratMode = AttackMid;
+ m3pi.locate(0,1);
+ m3pi.printf("\nTakNear ");
+ break;
+ case AttackMid:
+ stratMode = AttackFar;
+ m3pi.locate(0,1);
+ m3pi.printf("\nTakFar ");
+ break;
+ case AttackFar:
+ stratMode = Hunt;
+ m3pi.locate(0,1);
+ m3pi.printf("\nHUNTING! ");
+ break;
+ case Hunt:
+ stratMode = Off;
+ m3pi.printf("\nOFF ");
+ break;
+ }
+ }
+ debounce.reset();
+ debounce.start();
+}
+
+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 stratergies
+
+ updateScreen(isLine, stratMode);
+ bool atRight = false;
+ bool atLeft = false;
+
+ switch (stratMode) {
+ case Off: // No motors
+ case Dormant: // Will take action against lines though
+ m3pi.stop();
+ break;
+
+ case RunNear: // Runs if something is near
+ if (right.getDistance() == SharpDigiDist100::Near) {
+ atRight = true;
+ } else atRight = false;
+ if (left.getDistance() == SharpDigiDist100::Near) {
+ atLeft = true;
+ } else atLeft = false;
+
+ case RunMid: // Runs if something is in the middle distance
+
+ if (right.getDistance() == SharpDigiDist100::Mid) {
+ atRight = true;
+ }
+ if (left.getDistance() == SharpDigiDist100::Mid) {
+ atLeft = true;
+ }
+
+
+ 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 AttackMid: // Attacks something in the middle distance
+ if (right.getDistance() == SharpDigiDist100::Mid) {
+ atRight = true;
+ }
+ if (left.getDistance() == SharpDigiDist100::Mid) {
+ atLeft = true;
+ }
+
+
+ if (atRight && atLeft) {
+ m3pi.forward(0.6);
+ } else {
+ if (atRight == true) {
+ m3pi.left_motor(0.5);
+ m3pi.right_motor(0.7);
+ } else {
+ if (atLeft == true) {
+ m3pi.left_motor(0.7);
+ m3pi.right_motor(0.5);
+ } else {
+ m3pi.stop();
+ }
+ }
+ }
+
+ if (right.getDistance() == SharpDigiDist100::Near) {
+ atRight = true;
+ } else atRight = false;
+ if (left.getDistance() == SharpDigiDist100::Near) {
+ atLeft = true;
+ } else atLeft = false;
+
+
+ if (atRight && atLeft) {
+ m3pi.forward(0.5);
+ } else {
+ if (atRight == true) {
+ m3pi.left_motor(0.1);
+ m3pi.right_motor(0.2);
+ } else {
+ if (atLeft == true) {
+ m3pi.left_motor(0.2);
+ m3pi.right_motor(0.1);
+ }
+ }
+ }
+
+ break;
+
+ case AttackFar:
+
+ break;
+ case Hunt: // Runs forward until something is really close
+
+ if (right.getDistance() == SharpDigiDist100::Mid || right.getDistance() == SharpDigiDist100::Near) {
+ atRight = true;
+ } else atRight = false;
+ if (left.getDistance() == SharpDigiDist100::Mid || left.getDistance() == SharpDigiDist100::Near) {
+ atLeft = true;
+ } else atLeft = false;
+
+ if (atRight && atLeft) {
+ m3pi.stop();
+ } else {
+ if (atRight == true) {
+ m3pi.left_motor(0.1);
+ m3pi.right_motor(0.2);
+ } else {
+ if (atLeft == true) {
+ m3pi.left_motor(0.2);
+ m3pi.right_motor(0.1);
+ } else {
+ m3pi.forward(0.3);
+ }
+ }
+ }
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case 1: // Line in front, reverse
+ if(stratMode != Off)
+ {
+ 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");
+ }
+ else
+ {
+ m3pi.stop();
+ }
+ updateScreen(isLine, stratMode);
+ break;
+
+ case -1: // Line behind, forward
+ if(stratMode != Off)
+ {
+ 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);
+ }
+ }
+ else
+ {
+ m3pi.stop();
+ }
+ //m3pi.locate(0,1);
+ //m3pi.printf("LINE_BKD");
+ updateScreen(isLine, stratMode);
+ break;
+ }
+
+ //previousLine = isLine;
+}
+
+void clearLiner() { // Gets called a bit after a line is detected
+ lineCatch = 0;
+ //pc.printf("Cleared liner\n\r");
+}
+
+void updateScreen(int line, enum Stratergies stratergy) { // Update the bottom line with the running info
+ m3pi.locate(0,1);
+
+ char lineState;
+
+ switch (line) {
+ case 0:
+ lineState = 'N';
+ break;
+ case 1:
+ lineState = 'F';
+ break;
+ case -1:
+ lineState = 'B';
+ break;
+ default:
+ break;
+ }
+
+ char strat[6];
+ switch (stratergy) {
+ case Off:
+ sprintf(strat,"OFF ");
+ break;
+ case Dormant:
+ sprintf(strat,"DORMNT");
+ break;
+ case RunMid:
+ sprintf(strat,"RUNMID");
+
+ break;
+ case RunNear:
+ sprintf(strat,"RNNEAR");
+
+ break;
+ case AttackMid:
+ sprintf(strat,"TAKMID");
+
+ break;
+ case AttackFar:
+ sprintf(strat,"TAKFAR");
+
+ break;
+ case Hunt:
+ sprintf(strat,"HUNTER");
+
+ break;
+ }
+
+ m3pi.printf("%s %c",strat,lineState);
+}
\ No newline at end of file