hayama _lab / Mbed 2 deprecated mbedLinetracer_RTOS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

main.cpp@1:3f0aea9f0622, 2016-02-01 (annotated)

Committer:
hayama_lab
Date:
Mon Feb 01 17:36:43 2016 +0000
Revision:
1:3f0aea9f0622
Parent:
0:8626b3f15c02 
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hayama_lab 0:8626b3f15c02 1 //**********************************
hayama_lab 0:8626b3f15c02 2 //
hayama_lab 1:3f0aea9f0622 3 // mbed Linetracer using RTOS
hayama_lab 0:8626b3f15c02 4 //
hayama_lab 0:8626b3f15c02 5 // Rion Yamada(National Institute of Technology)
hayama_lab 0:8626b3f15c02 6 //
hayama_lab 0:8626b3f15c02 7 //**********************************
hayama_lab 0:8626b3f15c02 8 #include "mbed.h"
hayama_lab 0:8626b3f15c02 9 #include "rtos.h"
hayama_lab 0:8626b3f15c02 10
hayama_lab 0:8626b3f15c02 11 Serial pc(USBTX, USBRX); // serial port
hayama_lab 0:8626b3f15c02 12
hayama_lab 0:8626b3f15c02 13 // run parameters
hayama_lab 0:8626b3f15c02 14 #define STH 0.2 // threshold value for digital photo sensor
hayama_lab 0:8626b3f15c02 15 #define SGTH 0.2 // threshold value for start/goal marker
hayama_lab 0:8626b3f15c02 16 #define CTH 0.1 // threshold value for corner marker
hayama_lab 0:8626b3f15c02 17
hayama_lab 0:8626b3f15c02 18 // pattern table for stepping motor
hayama_lab 0:8626b3f15c02 19 const unsigned char RMOTOR[]= {0x09, 0x0C, 0x06, 0x03, 0x00}; // magnetization pattern for left motor
hayama_lab 0:8626b3f15c02 20 const unsigned char LMOTOR[]= {0x03, 0x06, 0x0C, 0x09, 0x00}; // magnetization pattern for right motor
hayama_lab 0:8626b3f15c02 21
hayama_lab 0:8626b3f15c02 22 const int sensArray[64]= {0,5,3,4,1,0,2,0,-1,0,0,0,0,0,0,0,-3,0,0,0,0,0,0,0,-2,0,0,0,0,0,0,0,-5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
hayama_lab 0:8626b3f15c02 23
hayama_lab 0:8626b3f15c02 24 unsigned char pmode=0; // program mode
hayama_lab 0:8626b3f15c02 25
hayama_lab 0:8626b3f15c02 26 volatile float sens1, sens2, sens3, sens4, sens5, sens6, sensSG, sensC; // sensor values
hayama_lab 0:8626b3f15c02 27 volatile int sensD,sensDout=0;
hayama_lab 0:8626b3f15c02 28
hayama_lab 0:8626b3f15c02 29 unsigned char frun, fmarker;
hayama_lab 0:8626b3f15c02 30 volatile int markerSG, markerC, markerX;
hayama_lab 0:8626b3f15c02 31
hayama_lab 0:8626b3f15c02 32 int cntGoal=0; // couter for run after goal
hayama_lab 0:8626b3f15c02 33
hayama_lab 0:8626b3f15c02 34
hayama_lab 0:8626b3f15c02 35 volatile unsigned char modeR=0, modeL=0;
hayama_lab 0:8626b3f15c02 36 volatile int stepR, stepL; // varilable for set step of motor
hayama_lab 0:8626b3f15c02 37 volatile unsigned char patR=0, patL=0; // index of motor pattern
hayama_lab 0:8626b3f15c02 38 volatile int cntR, cntL; // count of motor steps
hayama_lab 0:8626b3f15c02 39 volatile int waitR,waitL; //count of thread wait
hayama_lab 0:8626b3f15c02 40
hayama_lab 0:8626b3f15c02 41 volatile int bp=0; // couter for beep
hayama_lab 0:8626b3f15c02 42 volatile int f;
hayama_lab 0:8626b3f15c02 43 volatile int r,l;
hayama_lab 0:8626b3f15c02 44
hayama_lab 0:8626b3f15c02 45
hayama_lab 0:8626b3f15c02 46 BusOut leds( LED4, LED3, LED2, LED1 ); // for LED display
hayama_lab 0:8626b3f15c02 47 BusOut motorR(p5, p6, p7, p8 ); // output for right motor
hayama_lab 0:8626b3f15c02 48 BusOut motorL(p11, p12, p13, p14 ); // output for left motor
hayama_lab 0:8626b3f15c02 49
hayama_lab 0:8626b3f15c02 50 AnalogIn pt12(p19); // front right sensor, analog input
hayama_lab 0:8626b3f15c02 51 AnalogIn pt34(p18); // front left sensor, analog input
hayama_lab 0:8626b3f15c02 52 AnalogIn pt56(p17); // right sensor, analog input
hayama_lab 0:8626b3f15c02 53 AnalogIn ptC(p20); // left sensor, analog input
hayama_lab 0:8626b3f15c02 54 AnalogIn ptSG(p16); // left sensor, analog input
hayama_lab 0:8626b3f15c02 55 AnalogIn gyro(p15); // for Gyro, analog input, reserved
hayama_lab 0:8626b3f15c02 56
hayama_lab 0:8626b3f15c02 57 DigitalOut led1(LED1);
hayama_lab 0:8626b3f15c02 58 DigitalOut led2(LED2);
hayama_lab 0:8626b3f15c02 59 DigitalOut led3(LED3);
hayama_lab 0:8626b3f15c02 60 DigitalOut led4(LED4);
hayama_lab 0:8626b3f15c02 61
hayama_lab 0:8626b3f15c02 62 DigitalIn setSw(p21); // set-switch, digital input
hayama_lab 0:8626b3f15c02 63 DigitalIn startSw(p22); // start-switch, digital input
hayama_lab 0:8626b3f15c02 64 DigitalOut ledCout(p9); // LED output signal for corner marker
hayama_lab 0:8626b3f15c02 65 DigitalOut ledSGout(p10); // LED output signal for start/goal marker
hayama_lab 0:8626b3f15c02 66 DigitalOut buzzer(p23); // buzzer out
hayama_lab 0:8626b3f15c02 67
hayama_lab 0:8626b3f15c02 68 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 69 // RTOS Timer
hayama_lab 0:8626b3f15c02 70 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 71 void Sensor(void const *argument)
hayama_lab 0:8626b3f15c02 72 {
hayama_lab 0:8626b3f15c02 73
hayama_lab 0:8626b3f15c02 74 ledSGout=1; // LED-ON
hayama_lab 0:8626b3f15c02 75 sens1=pt12; // measure all sensor values
hayama_lab 0:8626b3f15c02 76 sens3=pt34;
hayama_lab 0:8626b3f15c02 77 sens5=pt56;
hayama_lab 0:8626b3f15c02 78 sensSG=ptSG;
hayama_lab 0:8626b3f15c02 79 ledSGout=0; // LED-OFF
hayama_lab 0:8626b3f15c02 80
hayama_lab 0:8626b3f15c02 81 ledCout=1; // LED-ON
hayama_lab 0:8626b3f15c02 82 sens2=pt12; // measure all sensor values
hayama_lab 0:8626b3f15c02 83 sens4=pt34;
hayama_lab 0:8626b3f15c02 84 sens6=pt56;
hayama_lab 0:8626b3f15c02 85 sensC=ptC;
hayama_lab 0:8626b3f15c02 86 ledCout=0; // LED-OFF
hayama_lab 0:8626b3f15c02 87
hayama_lab 0:8626b3f15c02 88 sensD=0;
hayama_lab 1:3f0aea9f0622 89 if (sens1>STH ) sensD |= 0x20; else sensD &= ~(0x20);
hayama_lab 1:3f0aea9f0622 90 if (sens2>STH ) sensD |= 0x10; else sensD &= ~(0x10);
hayama_lab 1:3f0aea9f0622 91 if (sens3>STH ) sensD |= 0x08; else sensD &= ~(0x08);
hayama_lab 1:3f0aea9f0622 92 if (sens4>STH ) sensD |= 0x04; else sensD &= ~(0x04);
hayama_lab 1:3f0aea9f0622 93 if (sens5>STH ) sensD |= 0x02; else sensD &= ~(0x02);
hayama_lab 1:3f0aea9f0622 94 if (sens6>STH ) sensD |= 0x01; else sensD &= ~(0x01);
hayama_lab 0:8626b3f15c02 95 sensDout=sensArray[sensD];
hayama_lab 0:8626b3f15c02 96
hayama_lab 0:8626b3f15c02 97 if (sensSG>STH) markerSG++;
hayama_lab 0:8626b3f15c02 98 else if (markerSG>0) markerSG--;
hayama_lab 0:8626b3f15c02 99 if (sensC>CTH ) markerC++;
hayama_lab 0:8626b3f15c02 100 else if (markerC>0) markerC--;
hayama_lab 0:8626b3f15c02 101 // cross line detection
hayama_lab 0:8626b3f15c02 102 if (markerSG>1 && markerC>1) markerX=1; // both marker
hayama_lab 0:8626b3f15c02 103 if (markerX==1 && markerSG==0 && markerC==0) markerX=0; // ignore cross line
hayama_lab 0:8626b3f15c02 104
hayama_lab 0:8626b3f15c02 105 // buzzer
hayama_lab 0:8626b3f15c02 106 if (bp>0) {
hayama_lab 0:8626b3f15c02 107 bp--;
hayama_lab 0:8626b3f15c02 108 if (buzzer==1) buzzer=0;
hayama_lab 0:8626b3f15c02 109 else buzzer=1; // alternate ON-OFF
hayama_lab 0:8626b3f15c02 110 }
hayama_lab 0:8626b3f15c02 111 }
hayama_lab 0:8626b3f15c02 112
hayama_lab 0:8626b3f15c02 113 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 114 // Motor speed control
hayama_lab 0:8626b3f15c02 115 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 116 int Motor_Control()
hayama_lab 0:8626b3f15c02 117 {
hayama_lab 0:8626b3f15c02 118
hayama_lab 0:8626b3f15c02 119 switch(sensDout) {
hayama_lab 1:3f0aea9f0622 120 case -5: r=3; l=20; break;
hayama_lab 1:3f0aea9f0622 121 case -4: r=3; l=15; break;
hayama_lab 1:3f0aea9f0622 122 case -3: r=3; l=15; break;
hayama_lab 1:3f0aea9f0622 123 case -2: r=5; l=10; break;
hayama_lab 1:3f0aea9f0622 124 case -1: r=5; l=10; break;
hayama_lab 1:3f0aea9f0622 125 case 0: r=5; l=5; break;
hayama_lab 1:3f0aea9f0622 126 case 1: r=10; l=5; break;
hayama_lab 1:3f0aea9f0622 127 case 2: r=10; l=5; break;
hayama_lab 1:3f0aea9f0622 128 case 3: r=15; l=3; break;
hayama_lab 1:3f0aea9f0622 129 case 4: r=15; l=3; break;
hayama_lab 1:3f0aea9f0622 130 case 5: r=20; l=3; break;
hayama_lab 0:8626b3f15c02 131 }
hayama_lab 0:8626b3f15c02 132 if(f==1) return(r);
hayama_lab 0:8626b3f15c02 133 else return(l);
hayama_lab 0:8626b3f15c02 134 }
hayama_lab 0:8626b3f15c02 135
hayama_lab 0:8626b3f15c02 136 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 137 // thread of right motor rotation
hayama_lab 0:8626b3f15c02 138 // motor rotation, mode = 0: free，1: forward，2: reverse，3: break
hayama_lab 0:8626b3f15c02 139 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 140 void motorR_thread(void const *argsument)
hayama_lab 0:8626b3f15c02 141 {
hayama_lab 0:8626b3f15c02 142
hayama_lab 0:8626b3f15c02 143 Thread::signal_wait(0x1);
hayama_lab 0:8626b3f15c02 144
hayama_lab 0:8626b3f15c02 145 while (true) {
hayama_lab 0:8626b3f15c02 146 f=1;
hayama_lab 0:8626b3f15c02 147 if(modeR==1) {
hayama_lab 0:8626b3f15c02 148 if (patR < 3) patR++;
hayama_lab 0:8626b3f15c02 149 else patR = 0;
hayama_lab 0:8626b3f15c02 150 }
hayama_lab 0:8626b3f15c02 151 if(modeR==2) {
hayama_lab 0:8626b3f15c02 152 if (patR > 0) patR--;
hayama_lab 0:8626b3f15c02 153 else patR = 3;
hayama_lab 0:8626b3f15c02 154 }
hayama_lab 0:8626b3f15c02 155 cntR++; // count up right moter step
hayama_lab 0:8626b3f15c02 156 if (modeR==0) patR=4; // motor free when mode=0
hayama_lab 0:8626b3f15c02 157 motorR= RMOTOR[patR];
hayama_lab 0:8626b3f15c02 158 waitR=Motor_Control();
hayama_lab 0:8626b3f15c02 159 Thread::wait(waitR);
hayama_lab 0:8626b3f15c02 160 }
hayama_lab 0:8626b3f15c02 161 }
hayama_lab 0:8626b3f15c02 162
hayama_lab 0:8626b3f15c02 163 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 164 // thread of right motor rotation
hayama_lab 0:8626b3f15c02 165 // motor rotation, mode = 0: free，1: forward，2: reverse，3: break
hayama_lab 0:8626b3f15c02 166 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 167 void motorL_thread(void const *argument)
hayama_lab 0:8626b3f15c02 168 {
hayama_lab 0:8626b3f15c02 169
hayama_lab 0:8626b3f15c02 170 Thread::signal_wait(0x1);
hayama_lab 0:8626b3f15c02 171
hayama_lab 0:8626b3f15c02 172 while (true) {
hayama_lab 0:8626b3f15c02 173 f=0; //runの戻り値が左モータ専用
hayama_lab 0:8626b3f15c02 174 if(modeL==1) {
hayama_lab 0:8626b3f15c02 175 if (patL < 3) patL++;
hayama_lab 0:8626b3f15c02 176 else patL = 0;
hayama_lab 0:8626b3f15c02 177 }
hayama_lab 0:8626b3f15c02 178 if(modeL==2) {
hayama_lab 0:8626b3f15c02 179 if (patL > 0) patL--;
hayama_lab 0:8626b3f15c02 180 else patL = 3;
hayama_lab 0:8626b3f15c02 181 }
hayama_lab 0:8626b3f15c02 182 if(modeL==3) patL=3;
hayama_lab 0:8626b3f15c02 183 cntL++; // count up left moter step
hayama_lab 0:8626b3f15c02 184 if (modeL==0) patL=4; // motor free when mode=0
hayama_lab 0:8626b3f15c02 185 motorL= LMOTOR[patL];
hayama_lab 0:8626b3f15c02 186 waitL=Motor_Control();
hayama_lab 0:8626b3f15c02 187 Thread::wait(waitL);
hayama_lab 0:8626b3f15c02 188 }
hayama_lab 0:8626b3f15c02 189 }
hayama_lab 0:8626b3f15c02 190
hayama_lab 0:8626b3f15c02 191 // -----------------------------------------------
hayama_lab 0:8626b3f15c02 192 // beep
hayama_lab 0:8626b3f15c02 193 // -----------------------------------------------
hayama_lab 0:8626b3f15c02 194 void beep(int n)
hayama_lab 0:8626b3f15c02 195 {
hayama_lab 0:8626b3f15c02 196 bp=n; // set beep couter
hayama_lab 0:8626b3f15c02 197 }
hayama_lab 0:8626b3f15c02 198
hayama_lab 0:8626b3f15c02 199 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 200 // check sensor value using serial port
hayama_lab 0:8626b3f15c02 201 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 202 void check_sens()
hayama_lab 0:8626b3f15c02 203 {
hayama_lab 0:8626b3f15c02 204 while (1) {
hayama_lab 0:8626b3f15c02 205 pc.printf("\f");
hayama_lab 0:8626b3f15c02 206 pc.printf("Sensor C:%f \n",sensC);
hayama_lab 0:8626b3f15c02 207 pc.printf("Sensor SG:%f \n",sensSG);
hayama_lab 0:8626b3f15c02 208 pc.printf("Sensor 1:%f \n",sens1);
hayama_lab 0:8626b3f15c02 209 pc.printf("Sensor 2:%f \n",sens2);
hayama_lab 0:8626b3f15c02 210 pc.printf("Sensor 3:%f \n",sens3);
hayama_lab 0:8626b3f15c02 211 pc.printf("Sensor 4:%f \n",sens4);
hayama_lab 0:8626b3f15c02 212 pc.printf("Sensor 5:%f \n",sens5);
hayama_lab 0:8626b3f15c02 213 pc.printf("Sensor 6:%f \n",sens6);
hayama_lab 0:8626b3f15c02 214 wait (1);
hayama_lab 0:8626b3f15c02 215 }
hayama_lab 0:8626b3f15c02 216 }
hayama_lab 0:8626b3f15c02 217
hayama_lab 0:8626b3f15c02 218 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 219 // break and release motors
hayama_lab 0:8626b3f15c02 220 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 221 void run_release()
hayama_lab 0:8626b3f15c02 222 {
hayama_lab 0:8626b3f15c02 223 modeR=0;
hayama_lab 0:8626b3f15c02 224 modeL=0; // motor release
hayama_lab 0:8626b3f15c02 225 }
hayama_lab 0:8626b3f15c02 226 void run_break()
hayama_lab 0:8626b3f15c02 227 {
hayama_lab 0:8626b3f15c02 228 modeR=3;
hayama_lab 0:8626b3f15c02 229 modeL=3; // mode 0 means break the motor
hayama_lab 0:8626b3f15c02 230 wait(0.5);
hayama_lab 0:8626b3f15c02 231 run_release();
hayama_lab 0:8626b3f15c02 232 }
hayama_lab 0:8626b3f15c02 233
hayama_lab 0:8626b3f15c02 234 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 235 // run and turn
hayama_lab 0:8626b3f15c02 236 // (mR,mL)=(1,1):forward, (2,1): turn right, (1,2): turn left, (2,2): Reverse
hayama_lab 0:8626b3f15c02 237 // nstep: number of step
hayama_lab 0:8626b3f15c02 238 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 239 void runTurn(int mR,int mL , int nstep )
hayama_lab 0:8626b3f15c02 240 {
hayama_lab 0:8626b3f15c02 241 modeR=mR;
hayama_lab 0:8626b3f15c02 242 modeL=mL;
hayama_lab 0:8626b3f15c02 243 cntR=0;
hayama_lab 0:8626b3f15c02 244 stepR=nstep;
hayama_lab 0:8626b3f15c02 245 while (cntR<stepR);
hayama_lab 0:8626b3f15c02 246 run_break();
hayama_lab 0:8626b3f15c02 247 }
hayama_lab 0:8626b3f15c02 248
hayama_lab 0:8626b3f15c02 249 //-----------------------
hayama_lab 0:8626b3f15c02 250 // run
hayama_lab 0:8626b3f15c02 251 //----------------------
hayama_lab 0:8626b3f15c02 252 void run()
hayama_lab 0:8626b3f15c02 253 {
hayama_lab 0:8626b3f15c02 254
hayama_lab 0:8626b3f15c02 255 markerSG=0;
hayama_lab 0:8626b3f15c02 256 markerC=0;
hayama_lab 0:8626b3f15c02 257 markerX=0;
hayama_lab 0:8626b3f15c02 258 fmarker=0;
hayama_lab 0:8626b3f15c02 259 frun=0;
hayama_lab 0:8626b3f15c02 260 modeR=modeL=1;
hayama_lab 0:8626b3f15c02 261
hayama_lab 0:8626b3f15c02 262 while(startSw==1 ) {
hayama_lab 0:8626b3f15c02 263
hayama_lab 0:8626b3f15c02 264 // corner marker check
hayama_lab 0:8626b3f15c02 265 if (markerX==1) fmarker=0;
hayama_lab 0:8626b3f15c02 266 if (markerX==0 && fmarker==0 && markerC>5) fmarker=1;
hayama_lab 0:8626b3f15c02 267 if (markerX==0 && fmarker==1 && markerC==0) {
hayama_lab 0:8626b3f15c02 268 fmarker=0;
hayama_lab 0:8626b3f15c02 269 beep(100);
hayama_lab 0:8626b3f15c02 270 }
hayama_lab 0:8626b3f15c02 271
hayama_lab 0:8626b3f15c02 272 // start/goal marker check
hayama_lab 0:8626b3f15c02 273 if (frun==0 && sensSG>SGTH) {
hayama_lab 0:8626b3f15c02 274 frun=1; // start marker detect
hayama_lab 0:8626b3f15c02 275 beep(100);
hayama_lab 0:8626b3f15c02 276 }
hayama_lab 0:8626b3f15c02 277 if (frun==1 && markerSG==0) frun=2; // start marker fix
hayama_lab 0:8626b3f15c02 278 if (frun==2 && sensSG>SGTH) frun=3; // goal marker detect
hayama_lab 0:8626b3f15c02 279 if (frun==3 && markerX==1) {
hayama_lab 0:8626b3f15c02 280 frun=2; // ignor cross line
hayama_lab 0:8626b3f15c02 281 led3=1;
hayama_lab 0:8626b3f15c02 282 }
hayama_lab 0:8626b3f15c02 283 if (frun==3 && markerSG==0) { // goal marker fix
hayama_lab 0:8626b3f15c02 284 frun=4;
hayama_lab 0:8626b3f15c02 285 beep(50);
hayama_lab 0:8626b3f15c02 286 break;
hayama_lab 0:8626b3f15c02 287 }
hayama_lab 0:8626b3f15c02 288 }
hayama_lab 0:8626b3f15c02 289 run_break();
hayama_lab 0:8626b3f15c02 290 }
hayama_lab 0:8626b3f15c02 291
hayama_lab 0:8626b3f15c02 292 int main()
hayama_lab 0:8626b3f15c02 293 {
hayama_lab 0:8626b3f15c02 294
hayama_lab 0:8626b3f15c02 295 Thread thread1(motorR_thread , NULL , osPriorityHigh); // thread of right motor control
hayama_lab 0:8626b3f15c02 296 Thread thread2(motorL_thread , NULL , osPriorityHigh); // thread of left motor control
hayama_lab 0:8626b3f15c02 297
hayama_lab 0:8626b3f15c02 298 RtosTimer sensor_timer(Sensor, osTimerPeriodic, (void *)0); // set RTOS timer for sensor
hayama_lab 0:8626b3f15c02 299 sensor_timer.start(5); // start RTOS timer for sensor
hayama_lab 0:8626b3f15c02 300
hayama_lab 0:8626b3f15c02 301 while (1) {
hayama_lab 0:8626b3f15c02 302 while (startSw==1) { // program mode selection
hayama_lab 0:8626b3f15c02 303 if (setSw==0) {
hayama_lab 0:8626b3f15c02 304 wait(0.1);
hayama_lab 0:8626b3f15c02 305 beep(50);
hayama_lab 0:8626b3f15c02 306 while (setSw==0);
hayama_lab 0:8626b3f15c02 307 wait(0.1);
hayama_lab 0:8626b3f15c02 308 pmode++;
hayama_lab 0:8626b3f15c02 309 if (pmode>3) pmode=0;
hayama_lab 0:8626b3f15c02 310 }
hayama_lab 0:8626b3f15c02 311 leds=pmode;
hayama_lab 0:8626b3f15c02 312 }
hayama_lab 0:8626b3f15c02 313 leds=0;
hayama_lab 0:8626b3f15c02 314 beep(50);
hayama_lab 0:8626b3f15c02 315 wait(0.5);
hayama_lab 0:8626b3f15c02 316
hayama_lab 0:8626b3f15c02 317 // run selected functions
hayama_lab 0:8626b3f15c02 318 switch(pmode) {
hayama_lab 1:3f0aea9f0622 319 case 0: check_sens(); // check sensors
hayama_lab 1:3f0aea9f0622 320 break;
hayama_lab 1:3f0aea9f0622 321 case 1: thread1.signal_set(0x1); // run forward
hayama_lab 1:3f0aea9f0622 322 thread2.signal_set(0x1);
hayama_lab 1:3f0aea9f0622 323 runTurn(1,1,500);
hayama_lab 1:3f0aea9f0622 324 break;
hayama_lab 1:3f0aea9f0622 325 case 2: thread1.signal_set(0x1); // turn right
hayama_lab 1:3f0aea9f0622 326 thread2.signal_set(0x1);
hayama_lab 1:3f0aea9f0622 327 runTurn(2,1,500);
hayama_lab 1:3f0aea9f0622 328 break;
hayama_lab 1:3f0aea9f0622 329 case 3: thread1.signal_set(0x1); // trace run
hayama_lab 1:3f0aea9f0622 330 thread2.signal_set(0x1);
hayama_lab 1:3f0aea9f0622 331 run();
hayama_lab 1:3f0aea9f0622 332 break;
hayama_lab 0:8626b3f15c02 333 }
hayama_lab 0:8626b3f15c02 334 }
hayama_lab 0:8626b3f15c02 335 }
hayama_lab 0:8626b3f15c02 336
hayama_lab 0:8626b3f15c02 337