![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Testcode for LVC Robot club
main.cpp
- Committer:
- jonmarsh
- Date:
- 2012-04-16
- Revision:
- 0:f4922a2a0292
- Child:
- 1:fe52aa73cd6a
File content as of revision 0:f4922a2a0292:
/** 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("This is us"); 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; } } } 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); }