Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

main.cpp@13:76c9915db820, 2013-04-06 (annotated)

Committer:
madcowswe
Date:
Sat Apr 06 14:41:35 2013 +0000
Revision:
13:76c9915db820
Parent:
11:bbddc908c78c
Child:
14:c638d4b9ee94
Still working on feedback test

Who changed what in which revision?

UserRevisionLine numberNew 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
madcowswe 6:995b3679155f 5
madcowswe 6:995b3679155f 6
madcowswe 6:995b3679155f 7 /*
madcowswe 6:995b3679155f 8 PINOUT Sensors
madcowswe 6:995b3679155f 9 5: RF:SDI
madcowswe 6:995b3679155f 10 6 SDO
madcowswe 6:995b3679155f 11 7 SCK
madcowswe 6:995b3679155f 12 8 NCS
madcowswe 6:995b3679155f 13 9 NIRQ
madcowswe 6:995b3679155f 14 10-15 6 echo pins
madcowswe 6:995b3679155f 15 16 trig
madcowswe 6:995b3679155f 16 17 IRin
madcowswe 6:995b3679155f 17 18-20 unused
madcowswe 6:995b3679155f 18 21 stepper step
madcowswe 6:995b3679155f 19 22-27 unused
madcowswe 6:995b3679155f 20 28 Serial TX
madcowswe 6:995b3679155f 21 29-30 unused
madcowswe 6:995b3679155f 22
madcowswe 6:995b3679155f 23
madcowswe 6:995b3679155f 24 PINOUT Main
madcowswe 6:995b3679155f 25 5: Lower arm servo
madcowswe 6:995b3679155f 26 6: Upper arm servo
madcowswe 6:995b3679155f 27
madcowswe 6:995b3679155f 28 14: Serial RX
madcowswe 6:995b3679155f 29 15: Distance sensor
madcowswe 6:995b3679155f 30
madcowswe 6:995b3679155f 31 20: color sensor in
madcowswe 6:995b3679155f 32 21-24: Motors PWM IN 1-4
madcowswe 6:995b3679155f 33 25-26: Encoders
madcowswe 6:995b3679155f 34 27-28: Encoders
madcowswe 6:995b3679155f 35 29: Color sensor RED LED
madcowswe 6:995b3679155f 36 30: Color sensor BLUE LED
madcowswe 6:995b3679155f 37
madcowswe 6:995b3679155f 38 */
twighk 0:200635fa1b08 39 #include "mbed.h"
twighk 3:717de74f6ebd 40 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 41
twighk 11:bbddc908c78c 42 const PinName P_SERVO_LOWER_ARM = p5;
twighk 11:bbddc908c78c 43 const PinName P_SERVO_UPPER_ARM = p6;
twighk 11:bbddc908c78c 44
twighk 11:bbddc908c78c 45 const PinName P_SERIAL_RX = p14;
twighk 11:bbddc908c78c 46 const PinName P_DISTANCE_SENSOR = p15;
twighk 11:bbddc908c78c 47
twighk 11:bbddc908c78c 48 const PinName P_COLOR_SENSOR_IN = p20;
twighk 11:bbddc908c78c 49
twighk 11:bbddc908c78c 50 const PinName P_MOT_RIGHT_A = p21;
twighk 11:bbddc908c78c 51 const PinName P_MOT_RIGHT_B = p22;
twighk 11:bbddc908c78c 52 const PinName P_MOT_LEFT_A = p23;
twighk 11:bbddc908c78c 53 const PinName P_MOT_LEFT_B = p24;
twighk 11:bbddc908c78c 54
twighk 11:bbddc908c78c 55 const PinName P_ENC_RIGHT_A = p28;
twighk 11:bbddc908c78c 56 const PinName P_ENC_RIGHT_B = p27;
twighk 11:bbddc908c78c 57 const PinName P_ENC_LEFT_A = p25;
twighk 11:bbddc908c78c 58 const PinName P_ENC_LEFT_B = p26;
twighk 11:bbddc908c78c 59
twighk 11:bbddc908c78c 60 const PinName P_COLOR_SENSOR_RED = p29;
twighk 11:bbddc908c78c 61 const PinName P_COLOR_SENSOR_BLUE = p30;
twighk 11:bbddc908c78c 62
twighk 4:1be0f6c6ceae 63 #include "Actuators/Arms/Arm.h"
twighk 1:8119211eae14 64 #include "Actuators/MainMotors/MainMotor.h"
twighk 1:8119211eae14 65 #include "Sensors/Encoders/Encoder.h"
twighk 4:1be0f6c6ceae 66 #include "Sensors/Colour/Colour.h"
twighk 8:69bdf20cb525 67 #include "Sensors/CakeSensor/CakeSensor.h"
madcowswe 13:76c9915db820 68 #include <algorithm>
twighk 0:200635fa1b08 69
twighk 0:200635fa1b08 70 void motortest();
twighk 0:200635fa1b08 71 void encodertest();
twighk 0:200635fa1b08 72 void motorencodetest();
twighk 0:200635fa1b08 73 void motorencodetestline();
twighk 0:200635fa1b08 74 void motorsandservostest();
twighk 1:8119211eae14 75 void armtest();
twighk 2:45da48fab346 76 void motortestline();
twighk 3:717de74f6ebd 77 void ledtest();
twighk 3:717de74f6ebd 78 void phototransistortest();
twighk 3:717de74f6ebd 79 void ledphototransistortest();
twighk 3:717de74f6ebd 80 void colourtest();
twighk 8:69bdf20cb525 81 void cakesensortest();
madcowswe 13:76c9915db820 82 void feedbacktest();
twighk 0:200635fa1b08 83
twighk 0:200635fa1b08 84 int main() {
twighk 2:45da48fab346 85
twighk 3:717de74f6ebd 86 /*****************
twighk 3:717de74f6ebd 87 * Test Code *
twighk 3:717de74f6ebd 88 *****************/
twighk 0:200635fa1b08 89 //motortest();
twighk 0:200635fa1b08 90 //encodertest();
twighk 8:69bdf20cb525 91 //motorencodetest();
twighk 1:8119211eae14 92 //motorencodetestline();
twighk 0:200635fa1b08 93 //motorsandservostest();
twighk 3:717de74f6ebd 94 //armtest();
twighk 2:45da48fab346 95 //motortestline();
twighk 11:bbddc908c78c 96 //ledtest();
twighk 3:717de74f6ebd 97 //phototransistortest();
madcowswe 13:76c9915db820 98 //ledphototransistortest();
madcowswe 5:56a5fdd373c9 99 //colourtest(); // Red SnR too low
madcowswe 13:76c9915db820 100 //cakesensortest();
madcowswe 13:76c9915db820 101 //feedbacktest();
madcowswe 13:76c9915db820 102 }
madcowswe 13:76c9915db820 103
madcowswe 13:76c9915db820 104 void feedbacktest(){
madcowswe 13:76c9915db820 105 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 13:76c9915db820 106 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
madcowswe 13:76c9915db820 107
madcowswe 13:76c9915db820 108 float Pgain = -0.002;
madcowswe 13:76c9915db820 109 float fwdspeed = -200/3.0f;
madcowswe 13:76c9915db820 110 Timer timer;
madcowswe 13:76c9915db820 111 timer.start();
madcowswe 13:76c9915db820 112
madcowswe 13:76c9915db820 113 while(true){
madcowswe 13:76c9915db820 114 float expecdist = fwdspeed * timer.read();
madcowswe 13:76c9915db820 115 float errleft = Eleft.getPoint() - (expecdist*1.07f);
madcowswe 13:76c9915db820 116 float errright = Eright.getPoint() - expecdist;
madcowswe 13:76c9915db820 117
madcowswe 13:76c9915db820 118 mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
madcowswe 13:76c9915db820 119 mright(max(min(errright*Pgain, 0.4f), -0.4f));
madcowswe 13:76c9915db820 120 }
twighk 8:69bdf20cb525 121 }
twighk 8:69bdf20cb525 122
twighk 8:69bdf20cb525 123 void cakesensortest(){
twighk 8:69bdf20cb525 124 wait(1);
twighk 8:69bdf20cb525 125 pc.printf("cakesensortest");
twighk 8:69bdf20cb525 126
twighk 11:bbddc908c78c 127 CakeSensor cs(P_COLOR_SENSOR_IN);
twighk 8:69bdf20cb525 128 while(true){
twighk 8:69bdf20cb525 129 wait(0.1);
twighk 8:69bdf20cb525 130 pc.printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
twighk 8:69bdf20cb525 131 }
twighk 3:717de74f6ebd 132 }
twighk 3:717de74f6ebd 133
twighk 3:717de74f6ebd 134 void colourtest(){
madcowswe 7:4340355261f9 135 Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 136 c.Calibrate();
twighk 3:717de74f6ebd 137 while(true){
twighk 3:717de74f6ebd 138 wait(0.1);
twighk 3:717de74f6ebd 139 ColourEnum ce = c.getColour();
twighk 3:717de74f6ebd 140 switch(ce){
twighk 3:717de74f6ebd 141 case BLUE :
twighk 3:717de74f6ebd 142 pc.printf("BLUE\n\r");
twighk 3:717de74f6ebd 143 break;
twighk 3:717de74f6ebd 144 case RED:
twighk 3:717de74f6ebd 145 pc.printf("RED\n\r");
twighk 3:717de74f6ebd 146 break;
twighk 3:717de74f6ebd 147 case WHITE:
twighk 3:717de74f6ebd 148 pc.printf("WHITE\n\r");
twighk 3:717de74f6ebd 149 break;
twighk 3:717de74f6ebd 150 case INCONCLUSIVE:
twighk 3:717de74f6ebd 151 pc.printf("INCONCLUSIVE\n\r");
twighk 3:717de74f6ebd 152 break;
twighk 3:717de74f6ebd 153 default:
twighk 3:717de74f6ebd 154 pc.printf("BUG\n\r");
twighk 3:717de74f6ebd 155 }
twighk 2:45da48fab346 156 }
twighk 0:200635fa1b08 157
twighk 3:717de74f6ebd 158 }
twighk 3:717de74f6ebd 159
twighk 3:717de74f6ebd 160
twighk 3:717de74f6ebd 161 void ledphototransistortest(){
madcowswe 7:4340355261f9 162 DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
madcowswe 7:4340355261f9 163 AnalogIn pt(P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 164 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 165
twighk 3:717de74f6ebd 166 while(true){
twighk 11:bbddc908c78c 167 blue = 0; red = 0;
twighk 11:bbddc908c78c 168 for(int i = 0; i != 5; i++){
twighk 11:bbddc908c78c 169 wait(0.1);
twighk 11:bbddc908c78c 170 pc.printf("Phototransistor Analog is (none): %f \n\r", pt.read());
twighk 11:bbddc908c78c 171 }
twighk 11:bbddc908c78c 172
madcowswe 7:4340355261f9 173 blue = 1; red = 0;
twighk 3:717de74f6ebd 174 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 175 wait(0.1);
twighk 3:717de74f6ebd 176 pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
twighk 3:717de74f6ebd 177 }
madcowswe 7:4340355261f9 178 blue = 0; red = 1;
twighk 3:717de74f6ebd 179 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 180 wait(0.1);
twighk 3:717de74f6ebd 181 pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
twighk 3:717de74f6ebd 182 }
twighk 11:bbddc908c78c 183 blue = 1; red = 1;
twighk 11:bbddc908c78c 184 for(int i = 0; i != 5; i++){
twighk 11:bbddc908c78c 185 wait(0.1);
twighk 11:bbddc908c78c 186 pc.printf("Phototransistor Analog is (both): %f \n\r", pt.read());
twighk 11:bbddc908c78c 187 }
twighk 3:717de74f6ebd 188 }
twighk 3:717de74f6ebd 189 }
twighk 3:717de74f6ebd 190
twighk 3:717de74f6ebd 191 void phototransistortest(){
madcowswe 7:4340355261f9 192 AnalogIn pt(P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 193 while(true){
twighk 3:717de74f6ebd 194 wait(0.1);
twighk 3:717de74f6ebd 195 pc.printf("Phototransistor Analog is: %f \n\r", pt.read());
twighk 3:717de74f6ebd 196 }
twighk 3:717de74f6ebd 197
twighk 3:717de74f6ebd 198 }
twighk 3:717de74f6ebd 199
twighk 3:717de74f6ebd 200 void ledtest(){
madcowswe 7:4340355261f9 201 DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
twighk 3:717de74f6ebd 202 while(true){
madcowswe 7:4340355261f9 203 blue = 1; red = 0;
twighk 3:717de74f6ebd 204 wait(0.2);
madcowswe 7:4340355261f9 205 blue = 0; red = 1;
twighk 3:717de74f6ebd 206 wait(0.2);
twighk 3:717de74f6ebd 207
twighk 3:717de74f6ebd 208 }
twighk 3:717de74f6ebd 209 }
twighk 3:717de74f6ebd 210
twighk 1:8119211eae14 211 void armtest(){
twighk 3:717de74f6ebd 212 Arm white(p26), black(p25, false, 0.0005, 180);
twighk 3:717de74f6ebd 213 while(true){
twighk 1:8119211eae14 214 white(0);
twighk 1:8119211eae14 215 black(0);
twighk 1:8119211eae14 216 wait(1);
twighk 1:8119211eae14 217 white(1);
twighk 1:8119211eae14 218 black(1);
twighk 1:8119211eae14 219 wait(1);
twighk 1:8119211eae14 220 }
twighk 1:8119211eae14 221 }
twighk 1:8119211eae14 222
twighk 1:8119211eae14 223
twighk 0:200635fa1b08 224 void motorsandservostest(){
twighk 0:200635fa1b08 225 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 226 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 1:8119211eae14 227 Arm sTop(p25), sBottom(p26);
twighk 4:1be0f6c6ceae 228 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 229 const float speed = 0.0;
twighk 0:200635fa1b08 230 const float dspeed = 0.0;
twighk 0:200635fa1b08 231
twighk 0:200635fa1b08 232 Timer servoTimer;
twighk 0:200635fa1b08 233 mleft(speed); mright(speed);
twighk 0:200635fa1b08 234 servoTimer.start();
twighk 0:200635fa1b08 235 while (true){
twighk 0:200635fa1b08 236 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 237 if (Eleft.getPoint() < Eright.getPoint()){
twighk 0:200635fa1b08 238 mleft(speed);
twighk 0:200635fa1b08 239 mright(speed - dspeed);
twighk 0:200635fa1b08 240 } else {
twighk 0:200635fa1b08 241 mright(speed);
twighk 0:200635fa1b08 242 mleft(speed - dspeed);
twighk 0:200635fa1b08 243 }
twighk 0:200635fa1b08 244 if (servoTimer.read() < 1){
twighk 0:200635fa1b08 245 sTop.clockwise();
twighk 0:200635fa1b08 246 } else if (servoTimer.read() < 4) {
twighk 2:45da48fab346 247 sTop.halt();
twighk 0:200635fa1b08 248 } else if (servoTimer.read() < 5) {
twighk 0:200635fa1b08 249 sBottom.anticlockwise();
twighk 0:200635fa1b08 250 //Led=1;
twighk 0:200635fa1b08 251 } else if (servoTimer.read() < 6) {
twighk 0:200635fa1b08 252 sBottom.clockwise();
twighk 0:200635fa1b08 253 //Led=0;
twighk 0:200635fa1b08 254 } else if (servoTimer.read() < 7) {
twighk 2:45da48fab346 255 sBottom.halt();
twighk 0:200635fa1b08 256 }else {
twighk 0:200635fa1b08 257 sTop.anticlockwise();
twighk 0:200635fa1b08 258 }
twighk 0:200635fa1b08 259 if (servoTimer.read() >= 9) servoTimer.reset();
twighk 0:200635fa1b08 260 }
twighk 0:200635fa1b08 261 }
twighk 0:200635fa1b08 262
twighk 2:45da48fab346 263 void motortestline(){
twighk 2:45da48fab346 264 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 2:45da48fab346 265 const float speed = 0.2;
twighk 2:45da48fab346 266 mleft(speed); mright(speed);
twighk 2:45da48fab346 267 while(true) wait(1);
twighk 2:45da48fab346 268 }
twighk 2:45da48fab346 269
twighk 0:200635fa1b08 270 void motorencodetestline(){
madcowswe 13:76c9915db820 271 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 13:76c9915db820 272 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
twighk 4:1be0f6c6ceae 273 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 274 const float speed = 0.2;
twighk 0:200635fa1b08 275 const float dspeed = 0.1;
twighk 0:200635fa1b08 276
twighk 0:200635fa1b08 277 mleft(speed); mright(speed);
twighk 0:200635fa1b08 278 while (true){
twighk 0:200635fa1b08 279 //left 27 cm = 113 -> 0.239 cm/pulse
twighk 0:200635fa1b08 280 //right 27 cm = 72 -> 0.375 cm/pulse
twighk 0:200635fa1b08 281 pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
twighk 0:200635fa1b08 282 if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
twighk 0:200635fa1b08 283 mright(speed - dspeed);
twighk 0:200635fa1b08 284 } else {
twighk 0:200635fa1b08 285 mright(speed + dspeed);
twighk 0:200635fa1b08 286 }
twighk 0:200635fa1b08 287 }
twighk 0:200635fa1b08 288
twighk 0:200635fa1b08 289 }
twighk 0:200635fa1b08 290
twighk 0:200635fa1b08 291 void motorencodetest(){
madcowswe 7:4340355261f9 292 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 7:4340355261f9 293 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
twighk 0:200635fa1b08 294 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 295
twighk 0:200635fa1b08 296 const float speed = -0.3;
twighk 0:200635fa1b08 297 const int enc = -38;
twighk 0:200635fa1b08 298 while(true){
twighk 0:200635fa1b08 299 mleft(speed); mright(0);
twighk 0:200635fa1b08 300 while(Eleft.getPoint()>enc){
twighk 0:200635fa1b08 301 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 302 }
twighk 0:200635fa1b08 303 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 304 mleft(0); mright(speed);
twighk 0:200635fa1b08 305 while(Eright.getPoint()>enc){
twighk 0:200635fa1b08 306 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 307 }
twighk 0:200635fa1b08 308 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 309 }
twighk 0:200635fa1b08 310 }
twighk 0:200635fa1b08 311
twighk 0:200635fa1b08 312 void encodertest(){
twighk 0:200635fa1b08 313 Encoder E1(p28, p27);
twighk 0:200635fa1b08 314 Encoder E2(p29, p30);
twighk 0:200635fa1b08 315 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 316 while(true){
twighk 0:200635fa1b08 317 wait(0.1);
twighk 0:200635fa1b08 318 pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), E2.getPoint());
twighk 0:200635fa1b08 319 }
twighk 0:200635fa1b08 320
twighk 0:200635fa1b08 321 }
twighk 0:200635fa1b08 322 void motortest(){
twighk 0:200635fa1b08 323 MainMotor mright(p22,p21), mleft(p23,p24);
twighk 3:717de74f6ebd 324 while(true) {
twighk 0:200635fa1b08 325 wait(1);
twighk 0:200635fa1b08 326 mleft(0.8); mright(0.8);
twighk 0:200635fa1b08 327 wait(1);
twighk 0:200635fa1b08 328 mleft(-0.2); mright(0.2);
twighk 0:200635fa1b08 329 wait(1);
twighk 0:200635fa1b08 330 mleft(0); mright(0);
twighk 0:200635fa1b08 331 }
twighk 0:200635fa1b08 332 }