Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

main.cpp@12:d4b5851742a3, 2013-04-05 (annotated)

Committer:
twighk
Date:
Fri Apr 05 21:49:23 2013 +0000
Revision:
12:d4b5851742a3
Parent:
11:bbddc908c78c
Child:
14:c638d4b9ee94
Printing Thread prints stuff

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 12:d4b5851742a3 40 #include "rtos.h"
twighk 3:717de74f6ebd 41 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 42
twighk 11:bbddc908c78c 43 const PinName P_SERVO_LOWER_ARM = p5;
twighk 11:bbddc908c78c 44 const PinName P_SERVO_UPPER_ARM = p6;
twighk 11:bbddc908c78c 45
twighk 11:bbddc908c78c 46 const PinName P_SERIAL_RX = p14;
twighk 11:bbddc908c78c 47 const PinName P_DISTANCE_SENSOR = p15;
twighk 11:bbddc908c78c 48
twighk 11:bbddc908c78c 49 const PinName P_COLOR_SENSOR_IN = p20;
twighk 11:bbddc908c78c 50
twighk 11:bbddc908c78c 51 const PinName P_MOT_RIGHT_A = p21;
twighk 11:bbddc908c78c 52 const PinName P_MOT_RIGHT_B = p22;
twighk 11:bbddc908c78c 53 const PinName P_MOT_LEFT_A = p23;
twighk 11:bbddc908c78c 54 const PinName P_MOT_LEFT_B = p24;
twighk 11:bbddc908c78c 55
twighk 11:bbddc908c78c 56 const PinName P_ENC_RIGHT_A = p28;
twighk 11:bbddc908c78c 57 const PinName P_ENC_RIGHT_B = p27;
twighk 11:bbddc908c78c 58 const PinName P_ENC_LEFT_A = p25;
twighk 11:bbddc908c78c 59 const PinName P_ENC_LEFT_B = p26;
twighk 11:bbddc908c78c 60
twighk 11:bbddc908c78c 61 const PinName P_COLOR_SENSOR_RED = p29;
twighk 11:bbddc908c78c 62 const PinName P_COLOR_SENSOR_BLUE = p30;
twighk 11:bbddc908c78c 63
twighk 4:1be0f6c6ceae 64 #include "Actuators/Arms/Arm.h"
twighk 1:8119211eae14 65 #include "Actuators/MainMotors/MainMotor.h"
twighk 1:8119211eae14 66 #include "Sensors/Encoders/Encoder.h"
twighk 4:1be0f6c6ceae 67 #include "Sensors/Colour/Colour.h"
twighk 8:69bdf20cb525 68 #include "Sensors/CakeSensor/CakeSensor.h"
twighk 12:d4b5851742a3 69 #include "Processes/Printing/Printing.h"
twighk 0:200635fa1b08 70
twighk 2:45da48fab346 71
twighk 0:200635fa1b08 72
twighk 0:200635fa1b08 73 void motortest();
twighk 0:200635fa1b08 74 void encodertest();
twighk 0:200635fa1b08 75 void motorencodetest();
twighk 0:200635fa1b08 76 void motorencodetestline();
twighk 0:200635fa1b08 77 void motorsandservostest();
twighk 1:8119211eae14 78 void armtest();
twighk 2:45da48fab346 79 void motortestline();
twighk 3:717de74f6ebd 80 void ledtest();
twighk 3:717de74f6ebd 81 void phototransistortest();
twighk 3:717de74f6ebd 82 void ledphototransistortest();
twighk 3:717de74f6ebd 83 void colourtest();
twighk 8:69bdf20cb525 84 void cakesensortest();
twighk 12:d4b5851742a3 85 void printingtestthread(void const*);
twighk 12:d4b5851742a3 86 void printingtestthread2(void const*);
twighk 0:200635fa1b08 87
twighk 0:200635fa1b08 88 int main() {
twighk 2:45da48fab346 89
twighk 3:717de74f6ebd 90 /*****************
twighk 3:717de74f6ebd 91 * Test Code *
twighk 3:717de74f6ebd 92 *****************/
twighk 0:200635fa1b08 93 //motortest();
twighk 0:200635fa1b08 94 //encodertest();
twighk 8:69bdf20cb525 95 //motorencodetest();
twighk 1:8119211eae14 96 //motorencodetestline();
twighk 0:200635fa1b08 97 //motorsandservostest();
twighk 3:717de74f6ebd 98 //armtest();
twighk 2:45da48fab346 99 //motortestline();
twighk 11:bbddc908c78c 100 //ledtest();
twighk 3:717de74f6ebd 101 //phototransistortest();
twighk 12:d4b5851742a3 102 //ledphototransistortest();
madcowswe 5:56a5fdd373c9 103 //colourtest(); // Red SnR too low
twighk 12:d4b5851742a3 104 //cakesensortest();
twighk 12:d4b5851742a3 105 DigitalOut l1(LED1);
twighk 12:d4b5851742a3 106 Thread p(printingThread, NULL, osPriorityNormal, 2048);
twighk 12:d4b5851742a3 107 l1=1;
twighk 12:d4b5851742a3 108 Thread a(printingtestthread, NULL, osPriorityNormal, 1024);
twighk 12:d4b5851742a3 109 Thread b(printingtestthread2, NULL, osPriorityNormal, 1024);
twighk 12:d4b5851742a3 110 Thread::wait(osWaitForever);
twighk 12:d4b5851742a3 111 }
twighk 12:d4b5851742a3 112
twighk 12:d4b5851742a3 113 #include <cstdlib>
twighk 12:d4b5851742a3 114 using namespace std;
twighk 12:d4b5851742a3 115
twighk 12:d4b5851742a3 116 void printingtestthread(void const*){
twighk 12:d4b5851742a3 117 const char ID = 1;
twighk 12:d4b5851742a3 118 float buffer[3] = {ID};
twighk 12:d4b5851742a3 119 registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
twighk 12:d4b5851742a3 120 while (true){
twighk 12:d4b5851742a3 121 for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
twighk 12:d4b5851742a3 122 buffer[i] =ID ;
twighk 12:d4b5851742a3 123 }
twighk 12:d4b5851742a3 124 updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
twighk 12:d4b5851742a3 125 Thread::wait(200);
twighk 12:d4b5851742a3 126 }
twighk 12:d4b5851742a3 127 }
twighk 12:d4b5851742a3 128 void printingtestthread2(void const*){
twighk 12:d4b5851742a3 129 const char ID = 2;
twighk 12:d4b5851742a3 130 float buffer[5] = {ID};
twighk 12:d4b5851742a3 131 registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
twighk 12:d4b5851742a3 132 while (true){
twighk 12:d4b5851742a3 133 for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
twighk 12:d4b5851742a3 134 buffer[i] = ID;
twighk 12:d4b5851742a3 135 }
twighk 12:d4b5851742a3 136 updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
twighk 12:d4b5851742a3 137 Thread::wait(500);
twighk 12:d4b5851742a3 138 }
twighk 8:69bdf20cb525 139 }
twighk 8:69bdf20cb525 140
twighk 8:69bdf20cb525 141 void cakesensortest(){
twighk 8:69bdf20cb525 142 wait(1);
twighk 8:69bdf20cb525 143 pc.printf("cakesensortest");
twighk 8:69bdf20cb525 144
twighk 11:bbddc908c78c 145 CakeSensor cs(P_COLOR_SENSOR_IN);
twighk 8:69bdf20cb525 146 while(true){
twighk 8:69bdf20cb525 147 wait(0.1);
twighk 8:69bdf20cb525 148 pc.printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
twighk 8:69bdf20cb525 149 }
twighk 3:717de74f6ebd 150 }
twighk 3:717de74f6ebd 151
twighk 3:717de74f6ebd 152 void colourtest(){
madcowswe 7:4340355261f9 153 Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 154 c.Calibrate();
twighk 3:717de74f6ebd 155 while(true){
twighk 3:717de74f6ebd 156 wait(0.1);
twighk 3:717de74f6ebd 157 ColourEnum ce = c.getColour();
twighk 3:717de74f6ebd 158 switch(ce){
twighk 3:717de74f6ebd 159 case BLUE :
twighk 3:717de74f6ebd 160 pc.printf("BLUE\n\r");
twighk 3:717de74f6ebd 161 break;
twighk 3:717de74f6ebd 162 case RED:
twighk 3:717de74f6ebd 163 pc.printf("RED\n\r");
twighk 3:717de74f6ebd 164 break;
twighk 3:717de74f6ebd 165 case WHITE:
twighk 3:717de74f6ebd 166 pc.printf("WHITE\n\r");
twighk 3:717de74f6ebd 167 break;
twighk 3:717de74f6ebd 168 case INCONCLUSIVE:
twighk 3:717de74f6ebd 169 pc.printf("INCONCLUSIVE\n\r");
twighk 3:717de74f6ebd 170 break;
twighk 3:717de74f6ebd 171 default:
twighk 3:717de74f6ebd 172 pc.printf("BUG\n\r");
twighk 3:717de74f6ebd 173 }
twighk 2:45da48fab346 174 }
twighk 0:200635fa1b08 175
twighk 3:717de74f6ebd 176 }
twighk 3:717de74f6ebd 177
twighk 3:717de74f6ebd 178
twighk 3:717de74f6ebd 179 void ledphototransistortest(){
madcowswe 7:4340355261f9 180 DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
madcowswe 7:4340355261f9 181 AnalogIn pt(P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 182 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 183
twighk 3:717de74f6ebd 184 while(true){
twighk 11:bbddc908c78c 185 blue = 0; red = 0;
twighk 11:bbddc908c78c 186 for(int i = 0; i != 5; i++){
twighk 11:bbddc908c78c 187 wait(0.1);
twighk 11:bbddc908c78c 188 pc.printf("Phototransistor Analog is (none): %f \n\r", pt.read());
twighk 11:bbddc908c78c 189 }
twighk 11:bbddc908c78c 190
madcowswe 7:4340355261f9 191 blue = 1; red = 0;
twighk 3:717de74f6ebd 192 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 193 wait(0.1);
twighk 3:717de74f6ebd 194 pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
twighk 3:717de74f6ebd 195 }
madcowswe 7:4340355261f9 196 blue = 0; red = 1;
twighk 3:717de74f6ebd 197 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 198 wait(0.1);
twighk 3:717de74f6ebd 199 pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
twighk 3:717de74f6ebd 200 }
twighk 11:bbddc908c78c 201 blue = 1; red = 1;
twighk 11:bbddc908c78c 202 for(int i = 0; i != 5; i++){
twighk 11:bbddc908c78c 203 wait(0.1);
twighk 11:bbddc908c78c 204 pc.printf("Phototransistor Analog is (both): %f \n\r", pt.read());
twighk 11:bbddc908c78c 205 }
twighk 3:717de74f6ebd 206 }
twighk 3:717de74f6ebd 207 }
twighk 3:717de74f6ebd 208
twighk 3:717de74f6ebd 209 void phototransistortest(){
madcowswe 7:4340355261f9 210 AnalogIn pt(P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 211 while(true){
twighk 3:717de74f6ebd 212 wait(0.1);
twighk 3:717de74f6ebd 213 pc.printf("Phototransistor Analog is: %f \n\r", pt.read());
twighk 3:717de74f6ebd 214 }
twighk 3:717de74f6ebd 215
twighk 3:717de74f6ebd 216 }
twighk 3:717de74f6ebd 217
twighk 3:717de74f6ebd 218 void ledtest(){
madcowswe 7:4340355261f9 219 DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
twighk 3:717de74f6ebd 220 while(true){
madcowswe 7:4340355261f9 221 blue = 1; red = 0;
twighk 3:717de74f6ebd 222 wait(0.2);
madcowswe 7:4340355261f9 223 blue = 0; red = 1;
twighk 3:717de74f6ebd 224 wait(0.2);
twighk 3:717de74f6ebd 225
twighk 3:717de74f6ebd 226 }
twighk 3:717de74f6ebd 227 }
twighk 3:717de74f6ebd 228
twighk 1:8119211eae14 229 void armtest(){
twighk 3:717de74f6ebd 230 Arm white(p26), black(p25, false, 0.0005, 180);
twighk 3:717de74f6ebd 231 while(true){
twighk 1:8119211eae14 232 white(0);
twighk 1:8119211eae14 233 black(0);
twighk 1:8119211eae14 234 wait(1);
twighk 1:8119211eae14 235 white(1);
twighk 1:8119211eae14 236 black(1);
twighk 1:8119211eae14 237 wait(1);
twighk 1:8119211eae14 238 }
twighk 1:8119211eae14 239 }
twighk 1:8119211eae14 240
twighk 1:8119211eae14 241
twighk 0:200635fa1b08 242 void motorsandservostest(){
twighk 0:200635fa1b08 243 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 244 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 1:8119211eae14 245 Arm sTop(p25), sBottom(p26);
twighk 4:1be0f6c6ceae 246 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 247 const float speed = 0.0;
twighk 0:200635fa1b08 248 const float dspeed = 0.0;
twighk 0:200635fa1b08 249
twighk 0:200635fa1b08 250 Timer servoTimer;
twighk 0:200635fa1b08 251 mleft(speed); mright(speed);
twighk 0:200635fa1b08 252 servoTimer.start();
twighk 0:200635fa1b08 253 while (true){
twighk 0:200635fa1b08 254 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 255 if (Eleft.getPoint() < Eright.getPoint()){
twighk 0:200635fa1b08 256 mleft(speed);
twighk 0:200635fa1b08 257 mright(speed - dspeed);
twighk 0:200635fa1b08 258 } else {
twighk 0:200635fa1b08 259 mright(speed);
twighk 0:200635fa1b08 260 mleft(speed - dspeed);
twighk 0:200635fa1b08 261 }
twighk 0:200635fa1b08 262 if (servoTimer.read() < 1){
twighk 0:200635fa1b08 263 sTop.clockwise();
twighk 0:200635fa1b08 264 } else if (servoTimer.read() < 4) {
twighk 2:45da48fab346 265 sTop.halt();
twighk 0:200635fa1b08 266 } else if (servoTimer.read() < 5) {
twighk 0:200635fa1b08 267 sBottom.anticlockwise();
twighk 0:200635fa1b08 268 //Led=1;
twighk 0:200635fa1b08 269 } else if (servoTimer.read() < 6) {
twighk 0:200635fa1b08 270 sBottom.clockwise();
twighk 0:200635fa1b08 271 //Led=0;
twighk 0:200635fa1b08 272 } else if (servoTimer.read() < 7) {
twighk 2:45da48fab346 273 sBottom.halt();
twighk 0:200635fa1b08 274 }else {
twighk 0:200635fa1b08 275 sTop.anticlockwise();
twighk 0:200635fa1b08 276 }
twighk 0:200635fa1b08 277 if (servoTimer.read() >= 9) servoTimer.reset();
twighk 0:200635fa1b08 278 }
twighk 0:200635fa1b08 279 }
twighk 0:200635fa1b08 280
twighk 2:45da48fab346 281 void motortestline(){
twighk 2:45da48fab346 282 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 2:45da48fab346 283 const float speed = 0.2;
twighk 2:45da48fab346 284 mleft(speed); mright(speed);
twighk 2:45da48fab346 285 while(true) wait(1);
twighk 2:45da48fab346 286 }
twighk 2:45da48fab346 287
twighk 0:200635fa1b08 288 void motorencodetestline(){
twighk 0:200635fa1b08 289 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 290 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 4:1be0f6c6ceae 291 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 292 const float speed = 0.2;
twighk 0:200635fa1b08 293 const float dspeed = 0.1;
twighk 0:200635fa1b08 294
twighk 0:200635fa1b08 295 mleft(speed); mright(speed);
twighk 0:200635fa1b08 296 while (true){
twighk 0:200635fa1b08 297 //left 27 cm = 113 -> 0.239 cm/pulse
twighk 0:200635fa1b08 298 //right 27 cm = 72 -> 0.375 cm/pulse
twighk 0:200635fa1b08 299 pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
twighk 0:200635fa1b08 300 if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
twighk 0:200635fa1b08 301 mright(speed - dspeed);
twighk 0:200635fa1b08 302 } else {
twighk 0:200635fa1b08 303 mright(speed + dspeed);
twighk 0:200635fa1b08 304 }
twighk 0:200635fa1b08 305 }
twighk 0:200635fa1b08 306
twighk 0:200635fa1b08 307 }
twighk 0:200635fa1b08 308
twighk 0:200635fa1b08 309 void motorencodetest(){
madcowswe 7:4340355261f9 310 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 7:4340355261f9 311 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
twighk 0:200635fa1b08 312 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 313
twighk 0:200635fa1b08 314 const float speed = -0.3;
twighk 0:200635fa1b08 315 const int enc = -38;
twighk 0:200635fa1b08 316 while(true){
twighk 0:200635fa1b08 317 mleft(speed); mright(0);
twighk 0:200635fa1b08 318 while(Eleft.getPoint()>enc){
twighk 0:200635fa1b08 319 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 320 }
twighk 0:200635fa1b08 321 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 322 mleft(0); mright(speed);
twighk 0:200635fa1b08 323 while(Eright.getPoint()>enc){
twighk 0:200635fa1b08 324 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 325 }
twighk 0:200635fa1b08 326 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 327 }
twighk 0:200635fa1b08 328 }
twighk 0:200635fa1b08 329
twighk 0:200635fa1b08 330 void encodertest(){
twighk 0:200635fa1b08 331 Encoder E1(p28, p27);
twighk 0:200635fa1b08 332 Encoder E2(p29, p30);
twighk 0:200635fa1b08 333 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 334 while(true){
twighk 0:200635fa1b08 335 wait(0.1);
twighk 0:200635fa1b08 336 pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), E2.getPoint());
twighk 0:200635fa1b08 337 }
twighk 0:200635fa1b08 338
twighk 0:200635fa1b08 339 }
twighk 0:200635fa1b08 340 void motortest(){
twighk 0:200635fa1b08 341 MainMotor mright(p22,p21), mleft(p23,p24);
twighk 3:717de74f6ebd 342 while(true) {
twighk 0:200635fa1b08 343 wait(1);
twighk 0:200635fa1b08 344 mleft(0.8); mright(0.8);
twighk 0:200635fa1b08 345 wait(1);
twighk 0:200635fa1b08 346 mleft(-0.2); mright(0.2);
twighk 0:200635fa1b08 347 wait(1);
twighk 0:200635fa1b08 348 mleft(0); mright(0);
twighk 0:200635fa1b08 349 }
twighk 0:200635fa1b08 350 }