Oskar Weigl / Mbed 2 deprecated ICRSEurobot13

This is some awesome robot code

Dependencies:   mbed-rtos mbed QEI

Fork of ICRSEurobot13 by Thomas Branch

main.cpp@21:167dacfe0b14, 2013-04-09 (annotated)

Committer:
madcowswe
Date:
Tue Apr 09 19:24:31 2013 +0000
Revision:
21:167dacfe0b14
Parent:
20:70d651156779
Child:
22:6e3218cf75f8
Working state UI

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