Jon Marsh / Mbed 2 deprecated m3pi_hunter

Allows the M3Pi to be used as a Sumo robot, using the sharp 100 distance sensors on the front. Hunter strategy

Dependencies:   mbed

main.cpp@0:a29bcf098632, 2012-06-18 (annotated)

Committer:
jonmarsh
Date:
Mon Jun 18 09:27:10 2012 +0000
Revision:
0:a29bcf098632
1st pass - not tested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jonmarsh 0:a29bcf098632 1 /**
jonmarsh 0:a29bcf098632 2 Name: mpi hunter
jonmarsh 0:a29bcf098632 3 Description:
jonmarsh 0:a29bcf098632 4
jonmarsh 0:a29bcf098632 5 Based on work done by Giles Barton-Owen
jonmarsh 0:a29bcf098632 6 Cut down for use at Linton Village College
jonmarsh 0:a29bcf098632 7
jonmarsh 0:a29bcf098632 8 This program keeps a m3pi inside a ring and looks for the other robot
jonmarsh 0:a29bcf098632 9
jonmarsh 0:a29bcf098632 10 **/
jonmarsh 0:a29bcf098632 11
jonmarsh 0:a29bcf098632 12 #include "mbed.h"
jonmarsh 0:a29bcf098632 13 #include "SharpDigiDist100.h"
jonmarsh 0:a29bcf098632 14 #include "m3pi.h"
jonmarsh 0:a29bcf098632 15
jonmarsh 0:a29bcf098632 16 DigitalOut Left[2] = {LED1,LED2}; //Some indicator LEDs for the range finders
jonmarsh 0:a29bcf098632 17 DigitalOut Right[2] = {LED4,LED3};
jonmarsh 0:a29bcf098632 18
jonmarsh 0:a29bcf098632 19 SharpDigiDist100 right(p30); // The range finder class initialisations
jonmarsh 0:a29bcf098632 20 SharpDigiDist100 left(p11);
jonmarsh 0:a29bcf098632 21
jonmarsh 0:a29bcf098632 22 m3pi m3pi; // Initialise the m3pi
jonmarsh 0:a29bcf098632 23
jonmarsh 0:a29bcf098632 24 InterruptIn button(p21); // SW1 on the shield, for strategy switching
jonmarsh 0:a29bcf098632 25
jonmarsh 0:a29bcf098632 26 Ticker guidance; // The main guidance caller
jonmarsh 0:a29bcf098632 27
jonmarsh 0:a29bcf098632 28 Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging
jonmarsh 0:a29bcf098632 29
jonmarsh 0:a29bcf098632 30 int previousLine; // A set of variables to sort out the line detection, previousLine is pretty much not used
jonmarsh 0:a29bcf098632 31 int isLine;
jonmarsh 0:a29bcf098632 32 int lineCatch;
jonmarsh 0:a29bcf098632 33
jonmarsh 0:a29bcf098632 34 Timeout liner; // A timeout to stop it ignoring the line
jonmarsh 0:a29bcf098632 35
jonmarsh 0:a29bcf098632 36 void CycleMode(); // Function defs
jonmarsh 0:a29bcf098632 37 void guideCall();
jonmarsh 0:a29bcf098632 38 void clearLiner();
jonmarsh 0:a29bcf098632 39
jonmarsh 0:a29bcf098632 40
jonmarsh 0:a29bcf098632 41 int main() {
jonmarsh 0:a29bcf098632 42 guidance.attach(&guideCall,0.1); // Sets up the control loop
jonmarsh 0:a29bcf098632 43
jonmarsh 0:a29bcf098632 44 m3pi.locate(0,0); // Write the name to the screen
jonmarsh 0:a29bcf098632 45 m3pi.printf("m3PiRngr");
jonmarsh 0:a29bcf098632 46
jonmarsh 0:a29bcf098632 47 m3pi.get_white_levels(); // Saves the current levels of the sensors to know what is white
jonmarsh 0:a29bcf098632 48
jonmarsh 0:a29bcf098632 49 while (1) {
jonmarsh 0:a29bcf098632 50
jonmarsh 0:a29bcf098632 51 switch (right.getDistance()) { // Sets up the distance indicator LEDs for the right side
jonmarsh 0:a29bcf098632 52 case SharpDigiDist100::Far :
jonmarsh 0:a29bcf098632 53 Right[0] = true;
jonmarsh 0:a29bcf098632 54 Right[1] = false;
jonmarsh 0:a29bcf098632 55 break;
jonmarsh 0:a29bcf098632 56 case SharpDigiDist100::Near :
jonmarsh 0:a29bcf098632 57 Right[1] = true;
jonmarsh 0:a29bcf098632 58 Right[0] = false;
jonmarsh 0:a29bcf098632 59 break;
jonmarsh 0:a29bcf098632 60 case SharpDigiDist100::Mid :
jonmarsh 0:a29bcf098632 61 Right[0] = true;
jonmarsh 0:a29bcf098632 62 Right[1] = true;
jonmarsh 0:a29bcf098632 63 break;
jonmarsh 0:a29bcf098632 64 default:
jonmarsh 0:a29bcf098632 65 break;
jonmarsh 0:a29bcf098632 66 }
jonmarsh 0:a29bcf098632 67 switch (left.getDistance()) { // Sets up the distance indicator LEDs for the left side
jonmarsh 0:a29bcf098632 68 case SharpDigiDist100::Far :
jonmarsh 0:a29bcf098632 69 Left[0] = true;
jonmarsh 0:a29bcf098632 70 Left[1] = false;
jonmarsh 0:a29bcf098632 71 break;
jonmarsh 0:a29bcf098632 72 case SharpDigiDist100::Near :
jonmarsh 0:a29bcf098632 73 Left[1] = true;
jonmarsh 0:a29bcf098632 74 Left[0] = false;
jonmarsh 0:a29bcf098632 75 break;
jonmarsh 0:a29bcf098632 76 case SharpDigiDist100::Mid :
jonmarsh 0:a29bcf098632 77 Left[0] = true;
jonmarsh 0:a29bcf098632 78 Left[1] = true;
jonmarsh 0:a29bcf098632 79 break;
jonmarsh 0:a29bcf098632 80 default:
jonmarsh 0:a29bcf098632 81 break;
jonmarsh 0:a29bcf098632 82 }
jonmarsh 0:a29bcf098632 83 }
jonmarsh 0:a29bcf098632 84 }
jonmarsh 0:a29bcf098632 85 void guideCall() {
jonmarsh 0:a29bcf098632 86
jonmarsh 0:a29bcf098632 87 isLine = m3pi.is_line(); // Gets whether the m3pi is on a line, and if so front/back
jonmarsh 0:a29bcf098632 88
jonmarsh 0:a29bcf098632 89 if (lineCatch == 0) { // Has it been off a line for long enough?
jonmarsh 0:a29bcf098632 90 isLine = isLine; // Yes - then go ahead
jonmarsh 0:a29bcf098632 91 } else {
jonmarsh 0:a29bcf098632 92 isLine = lineCatch; // No - pretend to still be on that line
jonmarsh 0:a29bcf098632 93
jonmarsh 0:a29bcf098632 94 }
jonmarsh 0:a29bcf098632 95 float position;
jonmarsh 0:a29bcf098632 96
jonmarsh 0:a29bcf098632 97 switch (isLine) {
jonmarsh 0:a29bcf098632 98 case 0: // No line, not even recently so go ahead with the strategies
jonmarsh 0:a29bcf098632 99 {
jonmarsh 0:a29bcf098632 100 bool atRight = false;
jonmarsh 0:a29bcf098632 101 bool atLeft = false;
jonmarsh 0:a29bcf098632 102
jonmarsh 0:a29bcf098632 103 // Runs forward until something is really close
jonmarsh 0:a29bcf098632 104
jonmarsh 0:a29bcf098632 105 if (right.getDistance() == SharpDigiDist100::Mid || right.getDistance() == SharpDigiDist100::Near) {
jonmarsh 0:a29bcf098632 106 atRight = true;
jonmarsh 0:a29bcf098632 107 } else atRight = false;
jonmarsh 0:a29bcf098632 108 if (left.getDistance() == SharpDigiDist100::Mid || left.getDistance() == SharpDigiDist100::Near) {
jonmarsh 0:a29bcf098632 109 atLeft = true;
jonmarsh 0:a29bcf098632 110 } else atLeft = false;
jonmarsh 0:a29bcf098632 111
jonmarsh 0:a29bcf098632 112 if (atRight && atLeft) {
jonmarsh 0:a29bcf098632 113 m3pi.stop();
jonmarsh 0:a29bcf098632 114 } else {
jonmarsh 0:a29bcf098632 115 if (atRight == true) {
jonmarsh 0:a29bcf098632 116 m3pi.left_motor(0.1);
jonmarsh 0:a29bcf098632 117 m3pi.right_motor(0.2);
jonmarsh 0:a29bcf098632 118 } else {
jonmarsh 0:a29bcf098632 119 if (atLeft == true) {
jonmarsh 0:a29bcf098632 120 m3pi.left_motor(0.2);
jonmarsh 0:a29bcf098632 121 m3pi.right_motor(0.1);
jonmarsh 0:a29bcf098632 122 } else {
jonmarsh 0:a29bcf098632 123 m3pi.forward(0.3);
jonmarsh 0:a29bcf098632 124 }
jonmarsh 0:a29bcf098632 125 }
jonmarsh 0:a29bcf098632 126 }}
jonmarsh 0:a29bcf098632 127 break;
jonmarsh 0:a29bcf098632 128 case 1: // Line in front, reverse
jonmarsh 0:a29bcf098632 129 if (lineCatch == 0) {
jonmarsh 0:a29bcf098632 130 lineCatch = 1;
jonmarsh 0:a29bcf098632 131
jonmarsh 0:a29bcf098632 132 liner.attach(&clearLiner, 0.3);
jonmarsh 0:a29bcf098632 133 }
jonmarsh 0:a29bcf098632 134
jonmarsh 0:a29bcf098632 135 position = m3pi.line_position();
jonmarsh 0:a29bcf098632 136 if (position < 0) {
jonmarsh 0:a29bcf098632 137 m3pi.left_motor(-1);
jonmarsh 0:a29bcf098632 138 m3pi.right_motor(-0.8);
jonmarsh 0:a29bcf098632 139 } else if (position == 0) {
jonmarsh 0:a29bcf098632 140 m3pi.backward(1);
jonmarsh 0:a29bcf098632 141 } else if (position > 0) {
jonmarsh 0:a29bcf098632 142 m3pi.left_motor(-0.8);
jonmarsh 0:a29bcf098632 143 m3pi.right_motor(-1);
jonmarsh 0:a29bcf098632 144 }
jonmarsh 0:a29bcf098632 145
jonmarsh 0:a29bcf098632 146 //m3pi.locate(0,1);
jonmarsh 0:a29bcf098632 147 //m3pi.printf("LINE_FWD");
jonmarsh 0:a29bcf098632 148
jonmarsh 0:a29bcf098632 149
jonmarsh 0:a29bcf098632 150 break;
jonmarsh 0:a29bcf098632 151
jonmarsh 0:a29bcf098632 152 case -1: // Line behind, forward
jonmarsh 0:a29bcf098632 153
jonmarsh 0:a29bcf098632 154 if (lineCatch == 0) {
jonmarsh 0:a29bcf098632 155 lineCatch = -1;
jonmarsh 0:a29bcf098632 156 liner.attach(&clearLiner, 0.3);
jonmarsh 0:a29bcf098632 157 }
jonmarsh 0:a29bcf098632 158
jonmarsh 0:a29bcf098632 159
jonmarsh 0:a29bcf098632 160 position = m3pi.line_position();
jonmarsh 0:a29bcf098632 161 if (position < 0) {
jonmarsh 0:a29bcf098632 162 m3pi.left_motor(1);
jonmarsh 0:a29bcf098632 163 m3pi.right_motor(0.8);
jonmarsh 0:a29bcf098632 164 } else if (position == 0) {
jonmarsh 0:a29bcf098632 165 m3pi.forward(1);
jonmarsh 0:a29bcf098632 166 } else if (position > 0) {
jonmarsh 0:a29bcf098632 167 m3pi.left_motor(0.8);
jonmarsh 0:a29bcf098632 168 m3pi.right_motor(1);
jonmarsh 0:a29bcf098632 169 }
jonmarsh 0:a29bcf098632 170
jonmarsh 0:a29bcf098632 171 break;
jonmarsh 0:a29bcf098632 172 }
jonmarsh 0:a29bcf098632 173
jonmarsh 0:a29bcf098632 174 //previousLine = isLine;
jonmarsh 0:a29bcf098632 175 }
jonmarsh 0:a29bcf098632 176
jonmarsh 0:a29bcf098632 177 void clearLiner() { // Gets called a bit after a line is detected
jonmarsh 0:a29bcf098632 178 lineCatch = 0;
jonmarsh 0:a29bcf098632 179 }
jonmarsh 0:a29bcf098632 180