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@0:8626b3f15c02, 2016-01-30 (annotated)

Committer:
hayama_lab
Date:
Sat Jan 30 12:39:11 2016 +0000
Revision:
0:8626b3f15c02
Child:
1:3f0aea9f0622
mbedLinetracer uing RTOS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hayama_lab 0:8626b3f15c02 1 //**********************************
hayama_lab 0:8626b3f15c02 2 //
hayama_lab 0:8626b3f15c02 3 // mbed Robotracer 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 0:8626b3f15c02 89 if (sens1>STH ) sensD |= 0x20;
hayama_lab 0:8626b3f15c02 90 else sensD &= ~(0x20);
hayama_lab 0:8626b3f15c02 91 if (sens2>STH ) sensD |= 0x10;
hayama_lab 0:8626b3f15c02 92 else sensD &= ~(0x10);
hayama_lab 0:8626b3f15c02 93 if (sens3>STH ) sensD |= 0x08;
hayama_lab 0:8626b3f15c02 94 else sensD &= ~(0x08);
hayama_lab 0:8626b3f15c02 95 if (sens4>STH ) sensD |= 0x04;
hayama_lab 0:8626b3f15c02 96 else sensD &= ~(0x04);
hayama_lab 0:8626b3f15c02 97 if (sens5>STH ) sensD |= 0x02;
hayama_lab 0:8626b3f15c02 98 else sensD &= ~(0x02);
hayama_lab 0:8626b3f15c02 99 if (sens6>STH ) sensD |= 0x01;
hayama_lab 0:8626b3f15c02 100 else sensD &= ~(0x01);
hayama_lab 0:8626b3f15c02 101 sensDout=sensArray[sensD];
hayama_lab 0:8626b3f15c02 102
hayama_lab 0:8626b3f15c02 103 if (sensSG>STH) markerSG++;
hayama_lab 0:8626b3f15c02 104 else if (markerSG>0) markerSG--;
hayama_lab 0:8626b3f15c02 105 if (sensC>CTH ) markerC++;
hayama_lab 0:8626b3f15c02 106 else if (markerC>0) markerC--;
hayama_lab 0:8626b3f15c02 107 // cross line detection
hayama_lab 0:8626b3f15c02 108 if (markerSG>1 && markerC>1) markerX=1; // both marker
hayama_lab 0:8626b3f15c02 109 if (markerX==1 && markerSG==0 && markerC==0) markerX=0; // ignore cross line
hayama_lab 0:8626b3f15c02 110
hayama_lab 0:8626b3f15c02 111 // buzzer
hayama_lab 0:8626b3f15c02 112 if (bp>0) {
hayama_lab 0:8626b3f15c02 113 bp--;
hayama_lab 0:8626b3f15c02 114 if (buzzer==1) buzzer=0;
hayama_lab 0:8626b3f15c02 115 else buzzer=1; // alternate ON-OFF
hayama_lab 0:8626b3f15c02 116 }
hayama_lab 0:8626b3f15c02 117 }
hayama_lab 0:8626b3f15c02 118
hayama_lab 0:8626b3f15c02 119 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 120 // Motor speed control
hayama_lab 0:8626b3f15c02 121 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 122 int Motor_Control()
hayama_lab 0:8626b3f15c02 123 {
hayama_lab 0:8626b3f15c02 124
hayama_lab 0:8626b3f15c02 125 switch(sensDout) {
hayama_lab 0:8626b3f15c02 126 case -5:
hayama_lab 0:8626b3f15c02 127 r=3;
hayama_lab 0:8626b3f15c02 128 l=20;
hayama_lab 0:8626b3f15c02 129 break;
hayama_lab 0:8626b3f15c02 130 case -4:
hayama_lab 0:8626b3f15c02 131 r=3;
hayama_lab 0:8626b3f15c02 132 l=15;
hayama_lab 0:8626b3f15c02 133 break;
hayama_lab 0:8626b3f15c02 134 case -3:
hayama_lab 0:8626b3f15c02 135 r=3;
hayama_lab 0:8626b3f15c02 136 l=15;
hayama_lab 0:8626b3f15c02 137 break;
hayama_lab 0:8626b3f15c02 138 case -2:
hayama_lab 0:8626b3f15c02 139 r=5;
hayama_lab 0:8626b3f15c02 140 l=10;
hayama_lab 0:8626b3f15c02 141 break;
hayama_lab 0:8626b3f15c02 142 case -1:
hayama_lab 0:8626b3f15c02 143 r=5;
hayama_lab 0:8626b3f15c02 144 l=10;
hayama_lab 0:8626b3f15c02 145 break;
hayama_lab 0:8626b3f15c02 146 case 0:
hayama_lab 0:8626b3f15c02 147 r=5;
hayama_lab 0:8626b3f15c02 148 l=5;
hayama_lab 0:8626b3f15c02 149 break;
hayama_lab 0:8626b3f15c02 150 case 1:
hayama_lab 0:8626b3f15c02 151 r=10;
hayama_lab 0:8626b3f15c02 152 l=5;
hayama_lab 0:8626b3f15c02 153 break;
hayama_lab 0:8626b3f15c02 154 case 2:
hayama_lab 0:8626b3f15c02 155 r=10;
hayama_lab 0:8626b3f15c02 156 l=5;
hayama_lab 0:8626b3f15c02 157 break;
hayama_lab 0:8626b3f15c02 158 case 3:
hayama_lab 0:8626b3f15c02 159 r=15;
hayama_lab 0:8626b3f15c02 160 l=3;
hayama_lab 0:8626b3f15c02 161 break;
hayama_lab 0:8626b3f15c02 162 case 4:
hayama_lab 0:8626b3f15c02 163 r=15;
hayama_lab 0:8626b3f15c02 164 l=3;
hayama_lab 0:8626b3f15c02 165 break;
hayama_lab 0:8626b3f15c02 166 case 5:
hayama_lab 0:8626b3f15c02 167 r=20;
hayama_lab 0:8626b3f15c02 168 l=3;
hayama_lab 0:8626b3f15c02 169 break;
hayama_lab 0:8626b3f15c02 170 }
hayama_lab 0:8626b3f15c02 171 if(f==1) return(r);
hayama_lab 0:8626b3f15c02 172 else return(l);
hayama_lab 0:8626b3f15c02 173 }
hayama_lab 0:8626b3f15c02 174
hayama_lab 0:8626b3f15c02 175 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 176 // thread of right motor rotation
hayama_lab 0:8626b3f15c02 177 // motor rotation, mode = 0: free，1: forward，2: reverse，3: break
hayama_lab 0:8626b3f15c02 178 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 179 void motorR_thread(void const *argsument)
hayama_lab 0:8626b3f15c02 180 {
hayama_lab 0:8626b3f15c02 181
hayama_lab 0:8626b3f15c02 182 Thread::signal_wait(0x1);
hayama_lab 0:8626b3f15c02 183
hayama_lab 0:8626b3f15c02 184 while (true) {
hayama_lab 0:8626b3f15c02 185 f=1;
hayama_lab 0:8626b3f15c02 186 if(modeR==1) {
hayama_lab 0:8626b3f15c02 187 if (patR < 3) patR++;
hayama_lab 0:8626b3f15c02 188 else patR = 0;
hayama_lab 0:8626b3f15c02 189 }
hayama_lab 0:8626b3f15c02 190 if(modeR==2) {
hayama_lab 0:8626b3f15c02 191 if (patR > 0) patR--;
hayama_lab 0:8626b3f15c02 192 else patR = 3;
hayama_lab 0:8626b3f15c02 193 }
hayama_lab 0:8626b3f15c02 194 cntR++; // count up right moter step
hayama_lab 0:8626b3f15c02 195 if (modeR==0) patR=4; // motor free when mode=0
hayama_lab 0:8626b3f15c02 196 motorR= RMOTOR[patR];
hayama_lab 0:8626b3f15c02 197 waitR=Motor_Control();
hayama_lab 0:8626b3f15c02 198 Thread::wait(waitR);
hayama_lab 0:8626b3f15c02 199 }
hayama_lab 0:8626b3f15c02 200 }
hayama_lab 0:8626b3f15c02 201
hayama_lab 0:8626b3f15c02 202 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 203 // thread of right motor rotation
hayama_lab 0:8626b3f15c02 204 // motor rotation, mode = 0: free，1: forward，2: reverse，3: break
hayama_lab 0:8626b3f15c02 205 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 206 void motorL_thread(void const *argument)
hayama_lab 0:8626b3f15c02 207 {
hayama_lab 0:8626b3f15c02 208
hayama_lab 0:8626b3f15c02 209 Thread::signal_wait(0x1);
hayama_lab 0:8626b3f15c02 210
hayama_lab 0:8626b3f15c02 211 while (true) {
hayama_lab 0:8626b3f15c02 212 f=0; //runの戻り値が左モータ専用
hayama_lab 0:8626b3f15c02 213 if(modeL==1) {
hayama_lab 0:8626b3f15c02 214 if (patL < 3) patL++;
hayama_lab 0:8626b3f15c02 215 else patL = 0;
hayama_lab 0:8626b3f15c02 216 }
hayama_lab 0:8626b3f15c02 217 if(modeL==2) {
hayama_lab 0:8626b3f15c02 218 if (patL > 0) patL--;
hayama_lab 0:8626b3f15c02 219 else patL = 3;
hayama_lab 0:8626b3f15c02 220 }
hayama_lab 0:8626b3f15c02 221 if(modeL==3) patL=3;
hayama_lab 0:8626b3f15c02 222 cntL++; // count up left moter step
hayama_lab 0:8626b3f15c02 223 if (modeL==0) patL=4; // motor free when mode=0
hayama_lab 0:8626b3f15c02 224 motorL= LMOTOR[patL];
hayama_lab 0:8626b3f15c02 225 waitL=Motor_Control();
hayama_lab 0:8626b3f15c02 226 Thread::wait(waitL);
hayama_lab 0:8626b3f15c02 227 }
hayama_lab 0:8626b3f15c02 228 }
hayama_lab 0:8626b3f15c02 229
hayama_lab 0:8626b3f15c02 230 // -----------------------------------------------
hayama_lab 0:8626b3f15c02 231 // beep
hayama_lab 0:8626b3f15c02 232 // -----------------------------------------------
hayama_lab 0:8626b3f15c02 233 void beep(int n)
hayama_lab 0:8626b3f15c02 234 {
hayama_lab 0:8626b3f15c02 235 bp=n; // set beep couter
hayama_lab 0:8626b3f15c02 236 }
hayama_lab 0:8626b3f15c02 237
hayama_lab 0:8626b3f15c02 238 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 239 // check sensor value using serial port
hayama_lab 0:8626b3f15c02 240 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 241 void check_sens()
hayama_lab 0:8626b3f15c02 242 {
hayama_lab 0:8626b3f15c02 243 while (1) {
hayama_lab 0:8626b3f15c02 244 pc.printf("\f");
hayama_lab 0:8626b3f15c02 245 pc.printf("Sensor C:%f \n",sensC);
hayama_lab 0:8626b3f15c02 246 pc.printf("Sensor SG:%f \n",sensSG);
hayama_lab 0:8626b3f15c02 247 pc.printf("Sensor 1:%f \n",sens1);
hayama_lab 0:8626b3f15c02 248 pc.printf("Sensor 2:%f \n",sens2);
hayama_lab 0:8626b3f15c02 249 pc.printf("Sensor 3:%f \n",sens3);
hayama_lab 0:8626b3f15c02 250 pc.printf("Sensor 4:%f \n",sens4);
hayama_lab 0:8626b3f15c02 251 pc.printf("Sensor 5:%f \n",sens5);
hayama_lab 0:8626b3f15c02 252 pc.printf("Sensor 6:%f \n",sens6);
hayama_lab 0:8626b3f15c02 253 wait (1);
hayama_lab 0:8626b3f15c02 254 }
hayama_lab 0:8626b3f15c02 255 }
hayama_lab 0:8626b3f15c02 256
hayama_lab 0:8626b3f15c02 257 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 258 // break and release motors
hayama_lab 0:8626b3f15c02 259 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 260 void run_release()
hayama_lab 0:8626b3f15c02 261 {
hayama_lab 0:8626b3f15c02 262 modeR=0;
hayama_lab 0:8626b3f15c02 263 modeL=0; // motor release
hayama_lab 0:8626b3f15c02 264 }
hayama_lab 0:8626b3f15c02 265 void run_break()
hayama_lab 0:8626b3f15c02 266 {
hayama_lab 0:8626b3f15c02 267 modeR=3;
hayama_lab 0:8626b3f15c02 268 modeL=3; // mode 0 means break the motor
hayama_lab 0:8626b3f15c02 269 wait(0.5);
hayama_lab 0:8626b3f15c02 270 run_release();
hayama_lab 0:8626b3f15c02 271 }
hayama_lab 0:8626b3f15c02 272
hayama_lab 0:8626b3f15c02 273 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 274 // run and turn
hayama_lab 0:8626b3f15c02 275 // (mR,mL)=(1,1):forward, (2,1): turn right, (1,2): turn left, (2,2): Reverse
hayama_lab 0:8626b3f15c02 276 // nstep: number of step
hayama_lab 0:8626b3f15c02 277 //--------------------------------------------------------------------------
hayama_lab 0:8626b3f15c02 278 void runTurn(int mR,int mL , int nstep )
hayama_lab 0:8626b3f15c02 279 {
hayama_lab 0:8626b3f15c02 280 modeR=mR;
hayama_lab 0:8626b3f15c02 281 modeL=mL;
hayama_lab 0:8626b3f15c02 282 cntR=0;
hayama_lab 0:8626b3f15c02 283 stepR=nstep;
hayama_lab 0:8626b3f15c02 284 while (cntR<stepR);
hayama_lab 0:8626b3f15c02 285 run_break();
hayama_lab 0:8626b3f15c02 286 }
hayama_lab 0:8626b3f15c02 287
hayama_lab 0:8626b3f15c02 288 //-----------------------
hayama_lab 0:8626b3f15c02 289 // run
hayama_lab 0:8626b3f15c02 290 //----------------------
hayama_lab 0:8626b3f15c02 291 void run()
hayama_lab 0:8626b3f15c02 292 {
hayama_lab 0:8626b3f15c02 293
hayama_lab 0:8626b3f15c02 294 markerSG=0;
hayama_lab 0:8626b3f15c02 295 markerC=0;
hayama_lab 0:8626b3f15c02 296 markerX=0;
hayama_lab 0:8626b3f15c02 297 fmarker=0;
hayama_lab 0:8626b3f15c02 298 frun=0;
hayama_lab 0:8626b3f15c02 299 modeR=modeL=1;
hayama_lab 0:8626b3f15c02 300
hayama_lab 0:8626b3f15c02 301 while(startSw==1 ) {
hayama_lab 0:8626b3f15c02 302
hayama_lab 0:8626b3f15c02 303 // corner marker check
hayama_lab 0:8626b3f15c02 304 if (markerX==1) fmarker=0;
hayama_lab 0:8626b3f15c02 305 if (markerX==0 && fmarker==0 && markerC>5) fmarker=1;
hayama_lab 0:8626b3f15c02 306 if (markerX==0 && fmarker==1 && markerC==0) {
hayama_lab 0:8626b3f15c02 307 fmarker=0;
hayama_lab 0:8626b3f15c02 308 beep(100);
hayama_lab 0:8626b3f15c02 309 }
hayama_lab 0:8626b3f15c02 310
hayama_lab 0:8626b3f15c02 311 // start/goal marker check
hayama_lab 0:8626b3f15c02 312 if (frun==0 && sensSG>SGTH) {
hayama_lab 0:8626b3f15c02 313 frun=1; // start marker detect
hayama_lab 0:8626b3f15c02 314 beep(100);
hayama_lab 0:8626b3f15c02 315 }
hayama_lab 0:8626b3f15c02 316 if (frun==1 && markerSG==0) frun=2; // start marker fix
hayama_lab 0:8626b3f15c02 317 if (frun==2 && sensSG>SGTH) frun=3; // goal marker detect
hayama_lab 0:8626b3f15c02 318 if (frun==3 && markerX==1) {
hayama_lab 0:8626b3f15c02 319 frun=2; // ignor cross line
hayama_lab 0:8626b3f15c02 320 led3=1;
hayama_lab 0:8626b3f15c02 321 }
hayama_lab 0:8626b3f15c02 322 if (frun==3 && markerSG==0) { // goal marker fix
hayama_lab 0:8626b3f15c02 323 frun=4;
hayama_lab 0:8626b3f15c02 324 beep(50);
hayama_lab 0:8626b3f15c02 325 break;
hayama_lab 0:8626b3f15c02 326 }
hayama_lab 0:8626b3f15c02 327 }
hayama_lab 0:8626b3f15c02 328 run_break();
hayama_lab 0:8626b3f15c02 329 }
hayama_lab 0:8626b3f15c02 330
hayama_lab 0:8626b3f15c02 331 int main()
hayama_lab 0:8626b3f15c02 332 {
hayama_lab 0:8626b3f15c02 333
hayama_lab 0:8626b3f15c02 334 Thread thread1(motorR_thread , NULL , osPriorityHigh); // thread of right motor control
hayama_lab 0:8626b3f15c02 335 Thread thread2(motorL_thread , NULL , osPriorityHigh); // thread of left motor control
hayama_lab 0:8626b3f15c02 336
hayama_lab 0:8626b3f15c02 337 RtosTimer sensor_timer(Sensor, osTimerPeriodic, (void *)0); // set RTOS timer for sensor
hayama_lab 0:8626b3f15c02 338 sensor_timer.start(5); // start RTOS timer for sensor
hayama_lab 0:8626b3f15c02 339
hayama_lab 0:8626b3f15c02 340 while (1) {
hayama_lab 0:8626b3f15c02 341 while (startSw==1) { // program mode selection
hayama_lab 0:8626b3f15c02 342 if (setSw==0) {
hayama_lab 0:8626b3f15c02 343 wait(0.1);
hayama_lab 0:8626b3f15c02 344 beep(50);
hayama_lab 0:8626b3f15c02 345 while (setSw==0);
hayama_lab 0:8626b3f15c02 346 wait(0.1);
hayama_lab 0:8626b3f15c02 347 pmode++;
hayama_lab 0:8626b3f15c02 348 if (pmode>3) pmode=0;
hayama_lab 0:8626b3f15c02 349 }
hayama_lab 0:8626b3f15c02 350 leds=pmode;
hayama_lab 0:8626b3f15c02 351 }
hayama_lab 0:8626b3f15c02 352 leds=0;
hayama_lab 0:8626b3f15c02 353 beep(50);
hayama_lab 0:8626b3f15c02 354 wait(0.5);
hayama_lab 0:8626b3f15c02 355
hayama_lab 0:8626b3f15c02 356 // run selected functions
hayama_lab 0:8626b3f15c02 357 switch(pmode) {
hayama_lab 0:8626b3f15c02 358 case 0:
hayama_lab 0:8626b3f15c02 359 check_sens();
hayama_lab 0:8626b3f15c02 360 break; // check sensors
hayama_lab 0:8626b3f15c02 361 case 1:
hayama_lab 0:8626b3f15c02 362 thread1.signal_set(0x1); // run forward
hayama_lab 0:8626b3f15c02 363 thread2.signal_set(0x1);
hayama_lab 0:8626b3f15c02 364 runTurn(1,1,500);
hayama_lab 0:8626b3f15c02 365 break;
hayama_lab 0:8626b3f15c02 366 case 2:
hayama_lab 0:8626b3f15c02 367 thread1.signal_set(0x1); // turn right
hayama_lab 0:8626b3f15c02 368 thread2.signal_set(0x1);
hayama_lab 0:8626b3f15c02 369 runTurn(2,1,500);
hayama_lab 0:8626b3f15c02 370 break;
hayama_lab 0:8626b3f15c02 371 case 3:
hayama_lab 0:8626b3f15c02 372 thread1.signal_set(0x1); // trace run
hayama_lab 0:8626b3f15c02 373 thread2.signal_set(0x1);
hayama_lab 0:8626b3f15c02 374 run();
hayama_lab 0:8626b3f15c02 375 break;
hayama_lab 0:8626b3f15c02 376 }
hayama_lab 0:8626b3f15c02 377 }
hayama_lab 0:8626b3f15c02 378 }
hayama_lab 0:8626b3f15c02 379
hayama_lab 0:8626b3f15c02 380