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@28:4e20b44251c6, 2013-04-10 (annotated)

Committer:
madcowswe
Date:
Wed Apr 10 04:20:40 2013 +0000
Revision:
28:4e20b44251c6
Parent:
25:b16f1045108f
Child:
31:ada943ecaceb
Idle CPU measure implemented, not tested

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