Cooper Liu / Mbed 2 deprecated ICRSEurobot13

Colour sensors calibrated

Dependencies:   mbed-rtos mbed Servo QEI

Fork of ICRSEurobot13 by Thomas Branch

main.cpp@24:5cfc4789e00b, 2013-04-09 (annotated)

Committer:
madcowswe
Date:
Tue Apr 09 20:47:28 2013 +0000
Revision:
24:5cfc4789e00b
Parent:
23:6e3218cf75f8
Child:
26:b16f1045108f
added a ticker but it doesnt run yet

Who changed what in which revision?

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