Jon Marsh / Mbed 2 deprecated m3pi_runner

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

Dependencies:   mbed

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

Committer:
jonmarsh
Date:
Mon Jun 18 09:38:17 2012 +0000
Revision:
0:11d0f3e0d1ad
Not tested

Who changed what in which revision?

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