Allows the M3Pi to be used as a Sumo robot, using the sharp 100 distance sensors on the front. Run away strategy
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?
User | Revision | Line number | New 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 |