Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

main.cpp@32:e3f633620816, 2013-04-10 (annotated)

Committer:
madcowswe
Date:
Wed Apr 10 18:25:16 2013 +0000
Revision:
32:e3f633620816
Parent:
31:ada943ecaceb
Parent:
30:791739422122
Child:
41:26e5f24b55b3
Merged ryan oskar

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