![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Testcode for LVC Robot club
Diff: main.cpp
- Revision:
- 0:f4922a2a0292
- Child:
- 1:fe52aa73cd6a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Apr 16 12:25:11 2012 +0000 @@ -0,0 +1,426 @@ +/** +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); +} \ No newline at end of file