Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

main.cpp@24:50805ef8c499, 2013-04-09 (annotated)

Committer:
rsavitski
Date:
Tue Apr 09 20:37:59 2013 +0000
Revision:
24:50805ef8c499
Parent:
20:70d651156779
Child:
25:b16f1045108f
Motion control branch

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