Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

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?

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
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 }