Jon Marsh
Testcode for LVC Robot club
Diff: main.cpp
- Revision:
- 1:fe52aa73cd6a
- Parent:
- 0:f4922a2a0292
--- a/main.cpp Mon Apr 16 12:25:11 2012 +0000
+++ b/main.cpp Mon Apr 23 09:32:50 2012 +0000
@@ -1,21 +1,9 @@
/**
-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).
+This program uses a Sharp Digital Distance sensor (GP2Y0D810) attached to pin 30 and 11
+(the two headers on the right and left of the board).
**/
@@ -32,55 +20,18 @@
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("This is us");
-
- m3pi.get_white_levels(); // Saves the current levels of the sensors to know what is white
+ m3pi.printf("Sonar");
- // 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
+ // m3pi.get_white_levels(); // Saves the current levels of the sensors to know what is white
while (1) {
- //pc.printf("Loop\n\r");
+
+// This tells it what to do when it detects something on the right-hand sensor
+
switch (right.getDistance()) { // Sets up the distance indicator LEDs for the right side
case SharpDigiDist100::Far :
Right[0] = true;
@@ -97,6 +48,9 @@
default:
break;
}
+
+// This tells it what to do when it detects something on the left-hand sensor
+
switch (left.getDistance()) { // Sets up the distance indicator LEDs for the left side
case SharpDigiDist100::Far :
Left[0] = true;
@@ -116,311 +70,3 @@
}
}
-
-
-
-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