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