Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

main.cpp@44:555136de33e4, 2013-04-12 (annotated)

Committer:
xiaxia686
Date:
Fri Apr 12 16:46:42 2013 +0000
Revision:
44:555136de33e4
Parent:
41:26e5f24b55b3
Parent:
43:c592bf6a6a2d
Child:
45:77cf6375348a
colour sensors calibrated and merged with main stuff;

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