Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

main.cpp@43:c592bf6a6a2d, 2013-04-12 (annotated)

Committer:
xiaxia686
Date:
Fri Apr 12 16:24:25 2013 +0000
Revision:
43:c592bf6a6a2d
Parent:
20:70d651156779
Child:
44:555136de33e4
Colour sensors calibrated

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