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@63:c2c6269767b8, 2013-04-14 (annotated)

Committer:
madcowswe
Date:
Sun Apr 14 14:32:43 2013 +0000
Revision:
63:c2c6269767b8
Parent:
62:78d99b781f02
Child:
64:c979fb1cd3b5
Workinf feed forward both turn and fwd

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