Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

main.cpp@65:4709ff6c753c, 2013-04-14 (annotated)

Committer:
rsavitski
Date:
Sun Apr 14 17:57:12 2013 +0000
Revision:
65:4709ff6c753c
Parent:
59:63609922579c
Child:
67:be3ea5450cc7
rewrote arm code, pushes top layer with uber paddle

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();
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 int main()
xiaxia686 43:c592bf6a6a2d 32 {
xiaxia686 43:c592bf6a6a2d 33
xiaxia686 43:c592bf6a6a2d 34 /*****************
xiaxia686 43:c592bf6a6a2d 35 * Test Code *
xiaxia686 43:c592bf6a6a2d 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();
rsavitski 65:4709ff6c753c 42 //armtest();
rsavitski 65:4709ff6c753c 43 //while(1);
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();
xiaxia686 43:c592bf6a6a2d 51
xiaxia686 43:c592bf6a6a2d 52 /*
twighk 13:d4b5851742a3 53 DigitalOut l1(LED1);
madcowswe 21:167dacfe0b14 54 Thread p(Printing::printingloop, 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 */
madcowswe 16:52250d8d8fce 60
madcowswe 22:6e3218cf75f8 61 SystemTime.start();
madcowswe 20:70d651156779 62
madcowswe 51:bc261eae004b 63 Serial pc(USBTX, USBRX);
madcowswe 25:b16f1045108f 64 pc.baud(115200);
xiaxia686 46:adcd57a5e402 65 wait(2);
xiaxia686 46:adcd57a5e402 66 InitSerial();
xiaxia686 46:adcd57a5e402 67 wait(3);
xiaxia686 46:adcd57a5e402 68 Kalman::KalmanInit();
madcowswe 20:70d651156779 69
xiaxia686 46:adcd57a5e402 70 Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
xiaxia686 46:adcd57a5e402 71 Kalman::start_predict_ticker(&predictthread);
madcowswe 20:70d651156779 72
xiaxia686 46:adcd57a5e402 73 Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
rsavitski 24:50805ef8c499 74
xiaxia686 46:adcd57a5e402 75 Ticker motorcontroltestticker;
xiaxia686 46:adcd57a5e402 76 motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
rsavitski 30:791739422122 77 // ai layer thread
xiaxia686 46:adcd57a5e402 78 Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
madcowswe 22:6e3218cf75f8 79
rsavitski 24:50805ef8c499 80 // motion layer periodic callback
xiaxia686 46:adcd57a5e402 81 RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
xiaxia686 46:adcd57a5e402 82 motion_timer.start(50);
xiaxia686 43:c592bf6a6a2d 83
xiaxia686 46:adcd57a5e402 84 Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
xiaxia686 43:c592bf6a6a2d 85
xiaxia686 46:adcd57a5e402 86 measureCPUidle(); //repurpose thread for idle measurement
madcowswe 21:167dacfe0b14 87 Thread::wait(osWaitForever);
madcowswe 51:bc261eae004b 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};
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){
madcowswe 20:70d651156779 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(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};
madcowswe 21:167dacfe0b14 112 Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
twighk 13:d4b5851742a3 113 while (true){
twighk 13:d4b5851742a3 114 for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
twighk 13:d4b5851742a3 115 buffer[i] = ID;
twighk 13:d4b5851742a3 116 }
madcowswe 21:167dacfe0b14 117 Printing::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
rsavitski 24:50805ef8c499 123 /*
madcowswe 12:76c9915db820 124 void feedbacktest(){
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 }
rsavitski 24:50805ef8c499 145 */
twighk 8:69bdf20cb525 146
xiaxia686 43:c592bf6a6a2d 147 void cakesensortest()
xiaxia686 43:c592bf6a6a2d 148 {
twighk 8:69bdf20cb525 149 wait(1);
madcowswe 20:70d651156779 150 printf("cakesensortest");
xiaxia686 43:c592bf6a6a2d 151
xiaxia686 43:c592bf6a6a2d 152 CakeSensor cs(P_DISTANCE_SENSOR);
xiaxia686 43:c592bf6a6a2d 153 while(true) {
twighk 8:69bdf20cb525 154 wait(0.1);
madcowswe 20:70d651156779 155 printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
xiaxia686 43:c592bf6a6a2d 156 }
twighk 3:717de74f6ebd 157 }
twighk 3:717de74f6ebd 158
xiaxia686 43:c592bf6a6a2d 159 void colourtest()
xiaxia686 43:c592bf6a6a2d 160 {
xiaxia686 45:77cf6375348a 161 Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER);
xiaxia686 45:77cf6375348a 162 Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER);
xiaxia686 43:c592bf6a6a2d 163
xiaxia686 43:c592bf6a6a2d 164 while(true) {
twighk 3:717de74f6ebd 165 wait(0.1);
xiaxia686 43:c592bf6a6a2d 166
xiaxia686 45:77cf6375348a 167 switch(c_lower.getColour()) {
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
madcowswe 51:bc261eae004b 188 /*
madcowswe 51:bc261eae004b 189
xiaxia686 43:c592bf6a6a2d 190 void pt_test()
xiaxia686 43:c592bf6a6a2d 191 {
xiaxia686 43:c592bf6a6a2d 192 DigitalOut _blue_led(p10);
xiaxia686 43:c592bf6a6a2d 193 DigitalOut _red_led(p11);
xiaxia686 45:77cf6375348a 194 AnalogIn _pt(p18);
xiaxia686 43:c592bf6a6a2d 195
xiaxia686 43:c592bf6a6a2d 196 bytepack_t datapackage;
xiaxia686 43:c592bf6a6a2d 197 // first 3 bytes of header is used for alignment
xiaxia686 43:c592bf6a6a2d 198 datapackage.data.header[0] = 0xFF;
xiaxia686 43:c592bf6a6a2d 199 datapackage.data.header[1] = 0xFF;
xiaxia686 43:c592bf6a6a2d 200 datapackage.data.header[2] = 0xFF;
xiaxia686 43:c592bf6a6a2d 201 while(true) {
xiaxia686 43:c592bf6a6a2d 202 //toggles leds for the next state
xiaxia686 43:c592bf6a6a2d 203 _blue_led = 1;
xiaxia686 43:c592bf6a6a2d 204 _red_led = 0;
xiaxia686 43:c592bf6a6a2d 205 wait(0.01);
xiaxia686 43:c592bf6a6a2d 206 volatile float blue_temp = _pt.read();
twighk 3:717de74f6ebd 207
xiaxia686 43:c592bf6a6a2d 208
xiaxia686 43:c592bf6a6a2d 209 datapackage.data.ID = (unsigned char)0;
xiaxia686 43:c592bf6a6a2d 210 datapackage.data.value = blue_temp;
xiaxia686 43:c592bf6a6a2d 211 datapackage.data.aux = 0;
xiaxia686 43:c592bf6a6a2d 212 for (int i = 0; i < sizeof(datapackage); i++) {
xiaxia686 43:c592bf6a6a2d 213 //mbed_serial.putc(datapackage.type_char[i]);
xiaxia686 43:c592bf6a6a2d 214 pc.putc(datapackage.type_char[i]);
twighk 3:717de74f6ebd 215 }
xiaxia686 43:c592bf6a6a2d 216
xiaxia686 43:c592bf6a6a2d 217 _blue_led = 0;
xiaxia686 43:c592bf6a6a2d 218 _red_led = 1;
xiaxia686 43:c592bf6a6a2d 219 wait(0.01);
xiaxia686 43:c592bf6a6a2d 220 volatile float red_temp = _pt.read();
xiaxia686 43:c592bf6a6a2d 221
xiaxia686 43:c592bf6a6a2d 222
xiaxia686 43:c592bf6a6a2d 223 datapackage.data.ID = (unsigned char)1;
xiaxia686 43:c592bf6a6a2d 224 datapackage.data.value = red_temp;
xiaxia686 43:c592bf6a6a2d 225 datapackage.data.aux = 0;
xiaxia686 43:c592bf6a6a2d 226 for (int i = 0; i < sizeof(datapackage); i++) {
xiaxia686 43:c592bf6a6a2d 227 //mbed_serial.putc(datapackage.type_char[i]);
xiaxia686 43:c592bf6a6a2d 228 pc.putc(datapackage.type_char[i]);
twighk 11:bbddc908c78c 229 }
twighk 3:717de74f6ebd 230
xiaxia686 43:c592bf6a6a2d 231 _blue_led = 0;
xiaxia686 43:c592bf6a6a2d 232 _red_led = 0;
xiaxia686 43:c592bf6a6a2d 233 wait(0.01);
xiaxia686 43:c592bf6a6a2d 234 volatile float noise_temp = _pt.read();
twighk 3:717de74f6ebd 235
twighk 3:717de74f6ebd 236
xiaxia686 43:c592bf6a6a2d 237 datapackage.data.ID = (unsigned char)2;
xiaxia686 43:c592bf6a6a2d 238 datapackage.data.value = noise_temp;
xiaxia686 43:c592bf6a6a2d 239 datapackage.data.aux = 0;
xiaxia686 43:c592bf6a6a2d 240 for (int i = 0; i < sizeof(datapackage); i++) {
xiaxia686 43:c592bf6a6a2d 241 //mbed_serial.putc(datapackage.type_char[i]);
xiaxia686 43:c592bf6a6a2d 242 pc.putc(datapackage.type_char[i]);
xiaxia686 43:c592bf6a6a2d 243 }
xiaxia686 43:c592bf6a6a2d 244
twighk 3:717de74f6ebd 245 }
twighk 3:717de74f6ebd 246 }
madcowswe 51:bc261eae004b 247 */
rsavitski 65:4709ff6c753c 248 #ifdef AGDHGADSYIGYJDGA
rsavitski 65:4709ff6c753c 249 PwmOut white(p26);
rsavitski 65:4709ff6c753c 250 PwmOut black(p25);
xiaxia686 43:c592bf6a6a2d 251
rsavitski 65:4709ff6c753c 252 void armtest()
rsavitski 65:4709ff6c753c 253 {
rsavitski 65:4709ff6c753c 254
rsavitski 65:4709ff6c753c 255 white.period(0.05);
rsavitski 65:4709ff6c753c 256 black.period(0.05);
rsavitski 65:4709ff6c753c 257
rsavitski 65:4709ff6c753c 258 /* float f=1;
rsavitski 65:4709ff6c753c 259 for (f=1; f<3; f+=0.1)
rsavitski 65:4709ff6c753c 260 {
rsavitski 65:4709ff6c753c 261 black.pulsewidth_us(f*1000);
rsavitski 65:4709ff6c753c 262 wait(1);
rsavitski 65:4709ff6c753c 263 printf("%f\r\n", f);
rsavitski 65:4709ff6c753c 264 }
rsavitski 65:4709ff6c753c 265 for (f=2; f>0; f-=0.1)
rsavitski 65:4709ff6c753c 266 {
rsavitski 65:4709ff6c753c 267 black.pulsewidth_us(f*1000);
rsavitski 65:4709ff6c753c 268 wait(1);
rsavitski 65:4709ff6c753c 269 printf("%f\r\n", f);
rsavitski 65:4709ff6c753c 270 }*/
rsavitski 65:4709ff6c753c 271
rsavitski 65:4709ff6c753c 272
rsavitski 65:4709ff6c753c 273 for(;;)
rsavitski 65:4709ff6c753c 274 {
rsavitski 65:4709ff6c753c 275 black.pulsewidth_us(2.0*1000);
rsavitski 65:4709ff6c753c 276 wait(2);
rsavitski 65:4709ff6c753c 277 black.pulsewidth_us(0.9*1000);//1.2
rsavitski 65:4709ff6c753c 278 wait(2);
rsavitski 65:4709ff6c753c 279 }
rsavitski 65:4709ff6c753c 280
rsavitski 65:4709ff6c753c 281 // white works
rsavitski 65:4709ff6c753c 282 /*for(;;)
rsavitski 65:4709ff6c753c 283 {
rsavitski 65:4709ff6c753c 284 white.pulsewidth_us(0.6*1000);
rsavitski 65:4709ff6c753c 285 wait(2);
rsavitski 65:4709ff6c753c 286 white.pulsewidth_us(2.5*1000);
rsavitski 65:4709ff6c753c 287 wait(2);
rsavitski 65:4709ff6c753c 288 }*/
rsavitski 65:4709ff6c753c 289
rsavitski 65:4709ff6c753c 290 /* while(1) //2.5 -> //0.6
rsavitski 65:4709ff6c753c 291 {
rsavitski 65:4709ff6c753c 292 white.pulsewidth_us(int(f*1000));
rsavitski 65:4709ff6c753c 293 printf("%f\r\n", f);
rsavitski 65:4709ff6c753c 294 f-=0.1;
rsavitski 65:4709ff6c753c 295 wait(1);
rsavitski 65:4709ff6c753c 296 }*/
rsavitski 65:4709ff6c753c 297 }
rsavitski 65:4709ff6c753c 298 #endif
rsavitski 65:4709ff6c753c 299 #ifdef FSDHGFSJDF
xiaxia686 43:c592bf6a6a2d 300 void armtest()
xiaxia686 43:c592bf6a6a2d 301 {
rsavitski 65:4709ff6c753c 302 Arm lower_arm(p25, 0.05, 0.9, 2.0);
rsavitski 65:4709ff6c753c 303 Arm upper_arm(p26, 0.05, 0.6, 2.5);
rsavitski 65:4709ff6c753c 304
rsavitski 65:4709ff6c753c 305 while(1)
rsavitski 65:4709ff6c753c 306 {
rsavitski 65:4709ff6c753c 307 upper_arm.go_up();
rsavitski 65:4709ff6c753c 308 wait(2);
rsavitski 65:4709ff6c753c 309 upper_arm.go_down();
rsavitski 65:4709ff6c753c 310 wait(2);
twighk 1:8119211eae14 311 }
twighk 1:8119211eae14 312 }
rsavitski 65:4709ff6c753c 313 #endif
rsavitski 65:4709ff6c753c 314 void armtest(){}
rsavitski 65:4709ff6c753c 315 /*
xiaxia686 43:c592bf6a6a2d 316 void motorsandservostest()
xiaxia686 43:c592bf6a6a2d 317 {
twighk 0:200635fa1b08 318 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 319 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 1:8119211eae14 320 Arm sTop(p25), sBottom(p26);
twighk 4:1be0f6c6ceae 321 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 322 const float speed = 0.0;
twighk 0:200635fa1b08 323 const float dspeed = 0.0;
xiaxia686 43:c592bf6a6a2d 324
twighk 0:200635fa1b08 325 Timer servoTimer;
xiaxia686 43:c592bf6a6a2d 326 mleft(speed);
xiaxia686 43:c592bf6a6a2d 327 mright(speed);
twighk 0:200635fa1b08 328 servoTimer.start();
xiaxia686 43:c592bf6a6a2d 329 while (true) {
madcowswe 20:70d651156779 330 printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
xiaxia686 43:c592bf6a6a2d 331 if (Eleft.getTicks() < Eright.getTicks()) {
twighk 0:200635fa1b08 332 mleft(speed);
twighk 0:200635fa1b08 333 mright(speed - dspeed);
twighk 0:200635fa1b08 334 } else {
twighk 0:200635fa1b08 335 mright(speed);
twighk 0:200635fa1b08 336 mleft(speed - dspeed);
twighk 0:200635fa1b08 337 }
xiaxia686 43:c592bf6a6a2d 338 if (servoTimer.read() < 1) {
twighk 0:200635fa1b08 339 sTop.clockwise();
twighk 0:200635fa1b08 340 } else if (servoTimer.read() < 4) {
twighk 2:45da48fab346 341 sTop.halt();
twighk 0:200635fa1b08 342 } else if (servoTimer.read() < 5) {
twighk 0:200635fa1b08 343 sBottom.anticlockwise();
twighk 0:200635fa1b08 344 //Led=1;
twighk 0:200635fa1b08 345 } else if (servoTimer.read() < 6) {
twighk 0:200635fa1b08 346 sBottom.clockwise();
twighk 0:200635fa1b08 347 //Led=0;
twighk 0:200635fa1b08 348 } else if (servoTimer.read() < 7) {
twighk 2:45da48fab346 349 sBottom.halt();
xiaxia686 43:c592bf6a6a2d 350 } else {
twighk 0:200635fa1b08 351 sTop.anticlockwise();
twighk 0:200635fa1b08 352 }
twighk 0:200635fa1b08 353 if (servoTimer.read() >= 9) servoTimer.reset();
twighk 0:200635fa1b08 354 }
twighk 0:200635fa1b08 355 }
rsavitski 65:4709ff6c753c 356 */
xiaxia686 43:c592bf6a6a2d 357 void motortestline()
xiaxia686 43:c592bf6a6a2d 358 {
twighk 2:45da48fab346 359 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 2:45da48fab346 360 const float speed = 0.2;
xiaxia686 43:c592bf6a6a2d 361 mleft(speed);
xiaxia686 43:c592bf6a6a2d 362 mright(speed);
twighk 2:45da48fab346 363 while(true) wait(1);
twighk 2:45da48fab346 364 }
twighk 2:45da48fab346 365
xiaxia686 43:c592bf6a6a2d 366 void motorencodetestline()
xiaxia686 43:c592bf6a6a2d 367 {
madcowswe 12:76c9915db820 368 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 12:76c9915db820 369 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
twighk 4:1be0f6c6ceae 370 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 371 const float speed = 0.2;
twighk 0:200635fa1b08 372 const float dspeed = 0.1;
twighk 0:200635fa1b08 373
xiaxia686 43:c592bf6a6a2d 374 mleft(speed);
xiaxia686 43:c592bf6a6a2d 375 mright(speed);
xiaxia686 43:c592bf6a6a2d 376 while (true) {
xiaxia686 43:c592bf6a6a2d 377 //left 27 cm = 113 -> 0.239 cm/pulse
xiaxia686 43:c592bf6a6a2d 378 //right 27 cm = 72 -> 0.375 cm/pulse
madcowswe 20:70d651156779 379 printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375));
xiaxia686 43:c592bf6a6a2d 380 if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375) {
twighk 0:200635fa1b08 381 mright(speed - dspeed);
twighk 0:200635fa1b08 382 } else {
twighk 0:200635fa1b08 383 mright(speed + dspeed);
twighk 0:200635fa1b08 384 }
twighk 0:200635fa1b08 385 }
twighk 0:200635fa1b08 386
twighk 0:200635fa1b08 387 }
twighk 0:200635fa1b08 388
xiaxia686 43:c592bf6a6a2d 389 void motorencodetest()
xiaxia686 43:c592bf6a6a2d 390 {
madcowswe 7:4340355261f9 391 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 7:4340355261f9 392 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
twighk 0:200635fa1b08 393 Serial pc(USBTX, USBRX);
xiaxia686 43:c592bf6a6a2d 394
twighk 0:200635fa1b08 395 const float speed = -0.3;
twighk 0:200635fa1b08 396 const int enc = -38;
xiaxia686 43:c592bf6a6a2d 397 while(true) {
xiaxia686 43:c592bf6a6a2d 398 mleft(speed);
xiaxia686 43:c592bf6a6a2d 399 mright(0);
xiaxia686 43:c592bf6a6a2d 400 while(Eleft.getTicks()>enc) {
madcowswe 20:70d651156779 401 printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
twighk 0:200635fa1b08 402 }
xiaxia686 43:c592bf6a6a2d 403 Eleft.reset();
xiaxia686 43:c592bf6a6a2d 404 Eright.reset();
xiaxia686 43:c592bf6a6a2d 405 mleft(0);
xiaxia686 43:c592bf6a6a2d 406 mright(speed);
xiaxia686 43:c592bf6a6a2d 407 while(Eright.getTicks()>enc) {
madcowswe 20:70d651156779 408 printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
twighk 0:200635fa1b08 409 }
xiaxia686 43:c592bf6a6a2d 410 Eleft.reset();
xiaxia686 43:c592bf6a6a2d 411 Eright.reset();
twighk 0:200635fa1b08 412 }
twighk 0:200635fa1b08 413 }
twighk 0:200635fa1b08 414
xiaxia686 43:c592bf6a6a2d 415 void encodertest()
xiaxia686 43:c592bf6a6a2d 416 {
madcowswe 15:9c5aaeda36dc 417 Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 15:9c5aaeda36dc 418 //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B);
twighk 0:200635fa1b08 419 Serial pc(USBTX, USBRX);
xiaxia686 43:c592bf6a6a2d 420 while(true) {
twighk 0:200635fa1b08 421 wait(0.1);
madcowswe 20:70d651156779 422 printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
twighk 0:200635fa1b08 423 }
twighk 0:200635fa1b08 424
twighk 0:200635fa1b08 425 }
xiaxia686 43:c592bf6a6a2d 426 void motortest()
xiaxia686 43:c592bf6a6a2d 427 {
twighk 0:200635fa1b08 428 MainMotor mright(p22,p21), mleft(p23,p24);
twighk 3:717de74f6ebd 429 while(true) {
twighk 0:200635fa1b08 430 wait(1);
xiaxia686 43:c592bf6a6a2d 431 mleft(0.8);
xiaxia686 43:c592bf6a6a2d 432 mright(0.8);
twighk 0:200635fa1b08 433 wait(1);
xiaxia686 43:c592bf6a6a2d 434 mleft(-0.2);
xiaxia686 43:c592bf6a6a2d 435 mright(0.2);
twighk 0:200635fa1b08 436 wait(1);
xiaxia686 43:c592bf6a6a2d 437 mleft(0);
xiaxia686 43:c592bf6a6a2d 438 mright(0);
twighk 0:200635fa1b08 439 }
twighk 0:200635fa1b08 440 }