Jon Marsh / Mbed 2 deprecated m3pi_attack

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

Dependencies:   mbed

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

Committer:
jonmarsh
Date:
Mon Jun 18 09:54:54 2012 +0000
Revision:
0:dda4072af5ff
not tested

Who changed what in which revision?

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