Colour sensors calibrated
Dependencies: mbed-rtos mbed Servo QEI
Fork of ICRSEurobot13 by
main.cpp@8:69bdf20cb525, 2013-04-04 (annotated)
- Committer:
- twighk
- Date:
- Thu Apr 04 19:37:05 2013 +0000
- Revision:
- 8:69bdf20cb525
- Parent:
- 5:56a5fdd373c9
- Child:
- 10:1f0cf0182067
CakeSensor, may need recalibration for cake;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
twighk | 8:69bdf20cb525 | 1 | //#pragma Otime // Compiler Optimisations |
twighk | 0:200635fa1b08 | 2 | |
twighk | 0:200635fa1b08 | 3 | // Eurobot13 main.cpp |
twighk | 0:200635fa1b08 | 4 | |
twighk | 0:200635fa1b08 | 5 | #include "mbed.h" |
twighk | 3:717de74f6ebd | 6 | Serial pc(USBTX, USBRX); |
twighk | 0:200635fa1b08 | 7 | |
twighk | 4:1be0f6c6ceae | 8 | #include "Actuators/Arms/Arm.h" |
twighk | 1:8119211eae14 | 9 | #include "Actuators/MainMotors/MainMotor.h" |
twighk | 1:8119211eae14 | 10 | #include "Sensors/Encoders/Encoder.h" |
twighk | 4:1be0f6c6ceae | 11 | #include "Sensors/Colour/Colour.h" |
twighk | 3:717de74f6ebd | 12 | #include "Sensors/Colour/Led.h" |
twighk | 3:717de74f6ebd | 13 | #include "Sensors/Colour/Phototransistor.h" |
twighk | 8:69bdf20cb525 | 14 | #include "Sensors/CakeSensor/CakeSensor.h" |
twighk | 0:200635fa1b08 | 15 | |
twighk | 2:45da48fab346 | 16 | |
twighk | 0:200635fa1b08 | 17 | |
twighk | 0:200635fa1b08 | 18 | void motortest(); |
twighk | 0:200635fa1b08 | 19 | void encodertest(); |
twighk | 0:200635fa1b08 | 20 | void motorencodetest(); |
twighk | 0:200635fa1b08 | 21 | void motorencodetestline(); |
twighk | 0:200635fa1b08 | 22 | void motorsandservostest(); |
twighk | 1:8119211eae14 | 23 | void armtest(); |
twighk | 2:45da48fab346 | 24 | void motortestline(); |
twighk | 3:717de74f6ebd | 25 | void ledtest(); |
twighk | 3:717de74f6ebd | 26 | void phototransistortest(); |
twighk | 3:717de74f6ebd | 27 | void ledphototransistortest(); |
twighk | 3:717de74f6ebd | 28 | void colourtest(); |
twighk | 8:69bdf20cb525 | 29 | void cakesensortest(); |
twighk | 0:200635fa1b08 | 30 | |
twighk | 0:200635fa1b08 | 31 | int main() { |
twighk | 2:45da48fab346 | 32 | |
twighk | 3:717de74f6ebd | 33 | /***************** |
twighk | 3:717de74f6ebd | 34 | * Test Code * |
twighk | 3:717de74f6ebd | 35 | *****************/ |
twighk | 0:200635fa1b08 | 36 | //motortest(); |
twighk | 0:200635fa1b08 | 37 | //encodertest(); |
twighk | 8:69bdf20cb525 | 38 | //motorencodetest(); |
twighk | 1:8119211eae14 | 39 | //motorencodetestline(); |
twighk | 0:200635fa1b08 | 40 | //motorsandservostest(); |
twighk | 3:717de74f6ebd | 41 | //armtest(); |
twighk | 2:45da48fab346 | 42 | //motortestline(); |
twighk | 3:717de74f6ebd | 43 | //ledtest(); |
twighk | 3:717de74f6ebd | 44 | //phototransistortest(); |
twighk | 3:717de74f6ebd | 45 | //ledphototransistortest(); |
madcowswe | 5:56a5fdd373c9 | 46 | //colourtest(); // Red SnR too low |
twighk | 8:69bdf20cb525 | 47 | cakesensortest(); |
twighk | 8:69bdf20cb525 | 48 | } |
twighk | 8:69bdf20cb525 | 49 | |
twighk | 8:69bdf20cb525 | 50 | void cakesensortest(){ |
twighk | 8:69bdf20cb525 | 51 | wait(1); |
twighk | 8:69bdf20cb525 | 52 | pc.printf("cakesensortest"); |
twighk | 8:69bdf20cb525 | 53 | |
twighk | 8:69bdf20cb525 | 54 | CakeSensor cs(p20); |
twighk | 8:69bdf20cb525 | 55 | while(true){ |
twighk | 8:69bdf20cb525 | 56 | wait(0.1); |
twighk | 8:69bdf20cb525 | 57 | pc.printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm()); |
twighk | 8:69bdf20cb525 | 58 | } |
twighk | 3:717de74f6ebd | 59 | } |
twighk | 3:717de74f6ebd | 60 | |
twighk | 3:717de74f6ebd | 61 | void colourtest(){ |
twighk | 3:717de74f6ebd | 62 | Colour c(p9,p10,p20); |
twighk | 3:717de74f6ebd | 63 | c.Calibrate(); |
twighk | 3:717de74f6ebd | 64 | while(true){ |
twighk | 3:717de74f6ebd | 65 | wait(0.1); |
twighk | 3:717de74f6ebd | 66 | ColourEnum ce = c.getColour(); |
twighk | 3:717de74f6ebd | 67 | switch(ce){ |
twighk | 3:717de74f6ebd | 68 | case BLUE : |
twighk | 3:717de74f6ebd | 69 | pc.printf("BLUE\n\r"); |
twighk | 3:717de74f6ebd | 70 | break; |
twighk | 3:717de74f6ebd | 71 | case RED: |
twighk | 3:717de74f6ebd | 72 | pc.printf("RED\n\r"); |
twighk | 3:717de74f6ebd | 73 | break; |
twighk | 3:717de74f6ebd | 74 | case WHITE: |
twighk | 3:717de74f6ebd | 75 | pc.printf("WHITE\n\r"); |
twighk | 3:717de74f6ebd | 76 | break; |
twighk | 3:717de74f6ebd | 77 | case INCONCLUSIVE: |
twighk | 3:717de74f6ebd | 78 | pc.printf("INCONCLUSIVE\n\r"); |
twighk | 3:717de74f6ebd | 79 | break; |
twighk | 3:717de74f6ebd | 80 | default: |
twighk | 3:717de74f6ebd | 81 | pc.printf("BUG\n\r"); |
twighk | 3:717de74f6ebd | 82 | } |
twighk | 2:45da48fab346 | 83 | } |
twighk | 0:200635fa1b08 | 84 | |
twighk | 3:717de74f6ebd | 85 | } |
twighk | 3:717de74f6ebd | 86 | |
twighk | 3:717de74f6ebd | 87 | |
twighk | 3:717de74f6ebd | 88 | void ledphototransistortest(){ |
twighk | 3:717de74f6ebd | 89 | Led blue(p9), red(p10); |
twighk | 3:717de74f6ebd | 90 | Phototransistor pt(p20); |
twighk | 3:717de74f6ebd | 91 | Serial pc(USBTX, USBRX); |
twighk | 3:717de74f6ebd | 92 | |
twighk | 3:717de74f6ebd | 93 | while(true){ |
twighk | 3:717de74f6ebd | 94 | blue.on();red.off(); |
twighk | 3:717de74f6ebd | 95 | for(int i = 0; i != 5; i++){ |
twighk | 3:717de74f6ebd | 96 | wait(0.1); |
twighk | 3:717de74f6ebd | 97 | pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read()); |
twighk | 3:717de74f6ebd | 98 | } |
twighk | 3:717de74f6ebd | 99 | blue.off();red.on(); |
twighk | 3:717de74f6ebd | 100 | for(int i = 0; i != 5; i++){ |
twighk | 3:717de74f6ebd | 101 | wait(0.1); |
twighk | 3:717de74f6ebd | 102 | pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read()); |
twighk | 3:717de74f6ebd | 103 | } |
twighk | 3:717de74f6ebd | 104 | } |
twighk | 3:717de74f6ebd | 105 | } |
twighk | 3:717de74f6ebd | 106 | |
twighk | 3:717de74f6ebd | 107 | void phototransistortest(){ |
twighk | 3:717de74f6ebd | 108 | Phototransistor pt(p20); |
twighk | 3:717de74f6ebd | 109 | while(true){ |
twighk | 3:717de74f6ebd | 110 | wait(0.1); |
twighk | 3:717de74f6ebd | 111 | pc.printf("Phototransistor Analog is: %f \n\r", pt.read()); |
twighk | 3:717de74f6ebd | 112 | } |
twighk | 3:717de74f6ebd | 113 | |
twighk | 3:717de74f6ebd | 114 | } |
twighk | 3:717de74f6ebd | 115 | |
twighk | 3:717de74f6ebd | 116 | void ledtest(){ |
twighk | 3:717de74f6ebd | 117 | Led blue(p9), red(p10); |
twighk | 3:717de74f6ebd | 118 | while(true){ |
twighk | 3:717de74f6ebd | 119 | blue.on();red.off(); |
twighk | 3:717de74f6ebd | 120 | wait(0.2); |
twighk | 3:717de74f6ebd | 121 | blue.off();red.on(); |
twighk | 3:717de74f6ebd | 122 | wait(0.2); |
twighk | 3:717de74f6ebd | 123 | |
twighk | 3:717de74f6ebd | 124 | } |
twighk | 3:717de74f6ebd | 125 | } |
twighk | 3:717de74f6ebd | 126 | |
twighk | 1:8119211eae14 | 127 | void armtest(){ |
twighk | 3:717de74f6ebd | 128 | Arm white(p26), black(p25, false, 0.0005, 180); |
twighk | 3:717de74f6ebd | 129 | while(true){ |
twighk | 1:8119211eae14 | 130 | white(0); |
twighk | 1:8119211eae14 | 131 | black(0); |
twighk | 1:8119211eae14 | 132 | wait(1); |
twighk | 1:8119211eae14 | 133 | white(1); |
twighk | 1:8119211eae14 | 134 | black(1); |
twighk | 1:8119211eae14 | 135 | wait(1); |
twighk | 1:8119211eae14 | 136 | } |
twighk | 1:8119211eae14 | 137 | } |
twighk | 1:8119211eae14 | 138 | |
twighk | 1:8119211eae14 | 139 | |
twighk | 0:200635fa1b08 | 140 | void motorsandservostest(){ |
twighk | 0:200635fa1b08 | 141 | Encoder Eleft(p27, p28), Eright(p30, p29); |
twighk | 0:200635fa1b08 | 142 | MainMotor mleft(p24,p23), mright(p21,p22); |
twighk | 1:8119211eae14 | 143 | Arm sTop(p25), sBottom(p26); |
twighk | 4:1be0f6c6ceae | 144 | //Serial pc(USBTX, USBRX); |
twighk | 0:200635fa1b08 | 145 | const float speed = 0.0; |
twighk | 0:200635fa1b08 | 146 | const float dspeed = 0.0; |
twighk | 0:200635fa1b08 | 147 | |
twighk | 0:200635fa1b08 | 148 | Timer servoTimer; |
twighk | 0:200635fa1b08 | 149 | mleft(speed); mright(speed); |
twighk | 0:200635fa1b08 | 150 | servoTimer.start(); |
twighk | 0:200635fa1b08 | 151 | while (true){ |
twighk | 0:200635fa1b08 | 152 | pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint()); |
twighk | 0:200635fa1b08 | 153 | if (Eleft.getPoint() < Eright.getPoint()){ |
twighk | 0:200635fa1b08 | 154 | mleft(speed); |
twighk | 0:200635fa1b08 | 155 | mright(speed - dspeed); |
twighk | 0:200635fa1b08 | 156 | } else { |
twighk | 0:200635fa1b08 | 157 | mright(speed); |
twighk | 0:200635fa1b08 | 158 | mleft(speed - dspeed); |
twighk | 0:200635fa1b08 | 159 | } |
twighk | 0:200635fa1b08 | 160 | if (servoTimer.read() < 1){ |
twighk | 0:200635fa1b08 | 161 | sTop.clockwise(); |
twighk | 0:200635fa1b08 | 162 | } else if (servoTimer.read() < 4) { |
twighk | 2:45da48fab346 | 163 | sTop.halt(); |
twighk | 0:200635fa1b08 | 164 | } else if (servoTimer.read() < 5) { |
twighk | 0:200635fa1b08 | 165 | sBottom.anticlockwise(); |
twighk | 0:200635fa1b08 | 166 | //Led=1; |
twighk | 0:200635fa1b08 | 167 | } else if (servoTimer.read() < 6) { |
twighk | 0:200635fa1b08 | 168 | sBottom.clockwise(); |
twighk | 0:200635fa1b08 | 169 | //Led=0; |
twighk | 0:200635fa1b08 | 170 | } else if (servoTimer.read() < 7) { |
twighk | 2:45da48fab346 | 171 | sBottom.halt(); |
twighk | 0:200635fa1b08 | 172 | }else { |
twighk | 0:200635fa1b08 | 173 | sTop.anticlockwise(); |
twighk | 0:200635fa1b08 | 174 | } |
twighk | 0:200635fa1b08 | 175 | if (servoTimer.read() >= 9) servoTimer.reset(); |
twighk | 0:200635fa1b08 | 176 | } |
twighk | 0:200635fa1b08 | 177 | } |
twighk | 0:200635fa1b08 | 178 | |
twighk | 2:45da48fab346 | 179 | void motortestline(){ |
twighk | 2:45da48fab346 | 180 | MainMotor mleft(p24,p23), mright(p21,p22); |
twighk | 2:45da48fab346 | 181 | const float speed = 0.2; |
twighk | 2:45da48fab346 | 182 | mleft(speed); mright(speed); |
twighk | 2:45da48fab346 | 183 | while(true) wait(1); |
twighk | 2:45da48fab346 | 184 | } |
twighk | 2:45da48fab346 | 185 | |
twighk | 0:200635fa1b08 | 186 | void motorencodetestline(){ |
twighk | 0:200635fa1b08 | 187 | Encoder Eleft(p27, p28), Eright(p30, p29); |
twighk | 0:200635fa1b08 | 188 | MainMotor mleft(p24,p23), mright(p21,p22); |
twighk | 4:1be0f6c6ceae | 189 | //Serial pc(USBTX, USBRX); |
twighk | 0:200635fa1b08 | 190 | const float speed = 0.2; |
twighk | 0:200635fa1b08 | 191 | const float dspeed = 0.1; |
twighk | 0:200635fa1b08 | 192 | |
twighk | 0:200635fa1b08 | 193 | mleft(speed); mright(speed); |
twighk | 0:200635fa1b08 | 194 | while (true){ |
twighk | 0:200635fa1b08 | 195 | //left 27 cm = 113 -> 0.239 cm/pulse |
twighk | 0:200635fa1b08 | 196 | //right 27 cm = 72 -> 0.375 cm/pulse |
twighk | 0:200635fa1b08 | 197 | pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375)); |
twighk | 0:200635fa1b08 | 198 | if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){ |
twighk | 0:200635fa1b08 | 199 | mright(speed - dspeed); |
twighk | 0:200635fa1b08 | 200 | } else { |
twighk | 0:200635fa1b08 | 201 | mright(speed + dspeed); |
twighk | 0:200635fa1b08 | 202 | } |
twighk | 0:200635fa1b08 | 203 | } |
twighk | 0:200635fa1b08 | 204 | |
twighk | 0:200635fa1b08 | 205 | } |
twighk | 0:200635fa1b08 | 206 | |
twighk | 0:200635fa1b08 | 207 | void motorencodetest(){ |
twighk | 0:200635fa1b08 | 208 | Encoder Eleft(p28, p27), Eright(p29, p30); |
twighk | 0:200635fa1b08 | 209 | MainMotor mleft(p23,p24), mright(p22,p21); |
twighk | 0:200635fa1b08 | 210 | Serial pc(USBTX, USBRX); |
twighk | 0:200635fa1b08 | 211 | |
twighk | 0:200635fa1b08 | 212 | const float speed = -0.3; |
twighk | 0:200635fa1b08 | 213 | const int enc = -38; |
twighk | 0:200635fa1b08 | 214 | while(true){ |
twighk | 0:200635fa1b08 | 215 | mleft(speed); mright(0); |
twighk | 0:200635fa1b08 | 216 | while(Eleft.getPoint()>enc){ |
twighk | 0:200635fa1b08 | 217 | pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint()); |
twighk | 0:200635fa1b08 | 218 | } |
twighk | 0:200635fa1b08 | 219 | Eleft.reset(); Eright.reset(); |
twighk | 0:200635fa1b08 | 220 | mleft(0); mright(speed); |
twighk | 0:200635fa1b08 | 221 | while(Eright.getPoint()>enc){ |
twighk | 0:200635fa1b08 | 222 | pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint()); |
twighk | 0:200635fa1b08 | 223 | } |
twighk | 0:200635fa1b08 | 224 | Eleft.reset(); Eright.reset(); |
twighk | 0:200635fa1b08 | 225 | } |
twighk | 0:200635fa1b08 | 226 | } |
twighk | 0:200635fa1b08 | 227 | |
twighk | 0:200635fa1b08 | 228 | void encodertest(){ |
twighk | 0:200635fa1b08 | 229 | Encoder E1(p28, p27); |
twighk | 0:200635fa1b08 | 230 | Encoder E2(p29, p30); |
twighk | 0:200635fa1b08 | 231 | Serial pc(USBTX, USBRX); |
twighk | 3:717de74f6ebd | 232 | while(true){ |
twighk | 0:200635fa1b08 | 233 | wait(0.1); |
twighk | 0:200635fa1b08 | 234 | pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), E2.getPoint()); |
twighk | 0:200635fa1b08 | 235 | } |
twighk | 0:200635fa1b08 | 236 | |
twighk | 0:200635fa1b08 | 237 | } |
twighk | 0:200635fa1b08 | 238 | void motortest(){ |
twighk | 0:200635fa1b08 | 239 | MainMotor mright(p22,p21), mleft(p23,p24); |
twighk | 3:717de74f6ebd | 240 | while(true) { |
twighk | 0:200635fa1b08 | 241 | wait(1); |
twighk | 0:200635fa1b08 | 242 | mleft(0.8); mright(0.8); |
twighk | 0:200635fa1b08 | 243 | wait(1); |
twighk | 0:200635fa1b08 | 244 | mleft(-0.2); mright(0.2); |
twighk | 0:200635fa1b08 | 245 | wait(1); |
twighk | 0:200635fa1b08 | 246 | mleft(0); mright(0); |
twighk | 0:200635fa1b08 | 247 | } |
twighk | 0:200635fa1b08 | 248 | } |