Kiyoteru Hayama / mbedRobotracer_Edu

mbed robotracer for education.

Robotracer (line follower robot ) using stepper motor.

/media/uploads/hayama/cimg8463.jpg

/media/uploads/hayama/mbedrobotracer-manual-english.pdf

/media/uploads/hayama/mbedrobotracer-manual-japanese.pdf

/media/uploads/hayama/eagle-design-robotracer.zip

movie -> https://www.youtube.com/watch?v=INwun8gSds4

(for competition->) https://www.youtube.com/watch?v=l_gP2pUt4w0

main.cpp@0:da22b0b4395a, 2013-06-19 (annotated)

Committer:
hayama
Date:
Wed Jun 19 10:00:41 2013 +0000
Revision:
0:da22b0b4395a
Child:
1:200aad416161
mbed robotracer for education ver 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hayama 0:da22b0b4395a 1 //**************************************************************************
hayama 0:da22b0b4395a 2 //
hayama 0:da22b0b4395a 3 // mbed Robotracer for education
hayama 0:da22b0b4395a 4 // (c) Kiyoteru Hayama(Kumamoto National College of Technology)
hayama 0:da22b0b4395a 5 //
hayama 0:da22b0b4395a 6 //**************************************************************************
hayama 0:da22b0b4395a 7 #include "mbed.h"
hayama 0:da22b0b4395a 8
hayama 0:da22b0b4395a 9 Serial pc(USBTX, USBRX);
hayama 0:da22b0b4395a 10
hayama 0:da22b0b4395a 11 // run parameters
hayama 0:da22b0b4395a 12 #define STH 0.5 // threshold value for digital photo sensor
hayama 0:da22b0b4395a 13 #define SGTH 10 // threshold value for start/goal marker
hayama 0:da22b0b4395a 14 #define CTH 10 // threshold value for corner marker
hayama 0:da22b0b4395a 15
hayama 0:da22b0b4395a 16 // pattern table for stepping motor
hayama 0:da22b0b4395a 17 const unsigned char RMOTOR[]={0x09, 0x0C, 0x06, 0x03, 0x00}; // magnetization pattern for left motor
hayama 0:da22b0b4395a 18 const unsigned char LMOTOR[]={0x03, 0x06, 0x0C, 0x09, 0x00}; // magnetization pattern for right motor
hayama 0:da22b0b4395a 19
hayama 0:da22b0b4395a 20 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 0:da22b0b4395a 21
hayama 0:da22b0b4395a 22 unsigned char pmode=0; // program mode
hayama 0:da22b0b4395a 23
hayama 0:da22b0b4395a 24 volatile float pt1B, pt2B, pt3B, pt4B, pt5B, pt6B, ptSGB, ptCB; // sensor values during turn-off the LED
hayama 0:da22b0b4395a 25 volatile float sens1, sens2, sens3, sens4, sens5, sens6, sensSG, sensC; // sensor values
hayama 0:da22b0b4395a 26 volatile int sensD,sensDout=0;
hayama 0:da22b0b4395a 27
hayama 0:da22b0b4395a 28 volatile int markerSG, markerC, markerX;
hayama 0:da22b0b4395a 29 unsigned char frun, fmarker;
hayama 0:da22b0b4395a 30
hayama 0:da22b0b4395a 31 volatile int spdR,spdL;
hayama 0:da22b0b4395a 32 volatile unsigned char modeR=0, modeL=0; // run forward both motor
hayama 0:da22b0b4395a 33 volatile int stepR, stepL; // varilable for set step of motor
hayama 0:da22b0b4395a 34 volatile unsigned char patR=0, patL=0; // index of motor pattern
hayama 0:da22b0b4395a 35 volatile int cntR, cntL; // count of motor steps
hayama 0:da22b0b4395a 36
hayama 0:da22b0b4395a 37 volatile unsigned char timR=0, timL=0; // timer for motors
hayama 0:da22b0b4395a 38 volatile unsigned char timS=0; // timer for sensors
hayama 0:da22b0b4395a 39 volatile int bp=0; // couter for beep
hayama 0:da22b0b4395a 40
hayama 0:da22b0b4395a 41 Ticker timerS; // defince interval timer
hayama 0:da22b0b4395a 42 Ticker timerM; // defince interval timer
hayama 0:da22b0b4395a 43
hayama 0:da22b0b4395a 44 BusOut leds( LED4, LED3, LED2, LED1 ); // for LED display
hayama 0:da22b0b4395a 45 BusOut motorR(p5, p6, p7, p8 ); // output for right motor
hayama 0:da22b0b4395a 46 BusOut motorL(p11, p12, p13, p14 ); // output for left motor
hayama 0:da22b0b4395a 47
hayama 0:da22b0b4395a 48 AnalogIn pt12(p19); // front right sensor, analog input
hayama 0:da22b0b4395a 49 AnalogIn pt34(p18); // front left sensor, analog input
hayama 0:da22b0b4395a 50 AnalogIn pt56(p17); // right sensor, analog input
hayama 0:da22b0b4395a 51 AnalogIn ptC(p20); // left sensor, analog input
hayama 0:da22b0b4395a 52 AnalogIn ptSG(p16); // left sensor, analog input
hayama 0:da22b0b4395a 53 AnalogIn gyro(p15); // for Gyro, analog input, reserved
hayama 0:da22b0b4395a 54
hayama 0:da22b0b4395a 55 DigitalIn setSw(p21); // set-switch, digital input
hayama 0:da22b0b4395a 56 DigitalIn startSw(p22); // start-switch, digital input
hayama 0:da22b0b4395a 57 DigitalOut ledCout(p9); // LED output signal for corner marker
hayama 0:da22b0b4395a 58 DigitalOut ledSGout(p10); // LED output signal for start/goal marker
hayama 0:da22b0b4395a 59 DigitalOut buzzer(p23); // buzzer out
hayama 0:da22b0b4395a 60
hayama 0:da22b0b4395a 61 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 62 // interrupt by us timerS
hayama 0:da22b0b4395a 63 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 64 void Sensor() {
hayama 0:da22b0b4395a 65 // read sensors
hayama 0:da22b0b4395a 66 // 1st-step:measure background during LED-off, 2nd-step: measure reflecting light during LED-on. sensor value is differnce of both.
hayama 0:da22b0b4395a 67 timS = !timS;
hayama 0:da22b0b4395a 68 if (timS==0){
hayama 0:da22b0b4395a 69 pt1B=pt12; // measure all background values
hayama 0:da22b0b4395a 70 pt3B=pt34;
hayama 0:da22b0b4395a 71 pt5B=pt56;
hayama 0:da22b0b4395a 72 ptSGB=ptSG;
hayama 0:da22b0b4395a 73 ledSGout=1; // LED-ON
hayama 0:da22b0b4395a 74 wait_us(50); // delay
hayama 0:da22b0b4395a 75 sens1=abs(pt12-pt1B); //"sabunn"
hayama 0:da22b0b4395a 76 sens3=abs(pt34-pt3B);
hayama 0:da22b0b4395a 77 sens5=abs(pt56-pt5B);
hayama 0:da22b0b4395a 78 sensSG=abs(ptSG-ptSGB);
hayama 0:da22b0b4395a 79 ledSGout=0; // LED-OFF
hayama 0:da22b0b4395a 80 } else{
hayama 0:da22b0b4395a 81 pt2B=pt12; // measure all background values
hayama 0:da22b0b4395a 82 pt4B=pt34;
hayama 0:da22b0b4395a 83 pt6B=pt56;
hayama 0:da22b0b4395a 84 ptCB=ptC;
hayama 0:da22b0b4395a 85 ledCout=1; // LED-ON
hayama 0:da22b0b4395a 86 wait_us(50); // delay
hayama 0:da22b0b4395a 87 sens2=abs(pt12-pt2B);
hayama 0:da22b0b4395a 88 sens4=abs(pt34-pt4B);
hayama 0:da22b0b4395a 89 sens6=abs(pt56-pt6B);
hayama 0:da22b0b4395a 90 sensC=abs(ptC-ptCB);
hayama 0:da22b0b4395a 91 ledCout=0; // LED-OFF
hayama 0:da22b0b4395a 92 }
hayama 0:da22b0b4395a 93
hayama 0:da22b0b4395a 94 sensD=0;
hayama 0:da22b0b4395a 95 if (sens1>STH ) sensD |= 0x20; else sensD &= ~(0x20);
hayama 0:da22b0b4395a 96 if (sens2>STH ) sensD |= 0x10; else sensD &= ~(0x10);
hayama 0:da22b0b4395a 97 if (sens3>STH ) sensD |= 0x08; else sensD &= ~(0x08);
hayama 0:da22b0b4395a 98 if (sens4>STH ) sensD |= 0x04; else sensD &= ~(0x04);
hayama 0:da22b0b4395a 99 if (sens5>STH ) sensD |= 0x02; else sensD &= ~(0x02);
hayama 0:da22b0b4395a 100 if (sens6>STH ) sensD |= 0x01; else sensD &= ~(0x01);
hayama 0:da22b0b4395a 101 sensDout=sensArray[sensD];
hayama 0:da22b0b4395a 102
hayama 0:da22b0b4395a 103 // corner and start/goal marker detection
hayama 0:da22b0b4395a 104 if (sensSG>STH) markerSG++; else if (markerSG>0) markerSG--;
hayama 0:da22b0b4395a 105 if (sensC>STH ) markerC++; else if (markerC>0) markerC--;
hayama 0:da22b0b4395a 106 // cross line detection
hayama 0:da22b0b4395a 107 if (markerSG>1 && markerC>1) markerX=1; // both marker
hayama 0:da22b0b4395a 108 if (markerX==1 && markerSG==0 && markerC==0) markerX=0; // ignore cross line
hayama 0:da22b0b4395a 109
hayama 0:da22b0b4395a 110 // buzzer
hayama 0:da22b0b4395a 111 if (bp>0){ // high beep
hayama 0:da22b0b4395a 112 bp--;
hayama 0:da22b0b4395a 113 if (buzzer==1) buzzer=0; else buzzer=1; // alternate ON-OFF
hayama 0:da22b0b4395a 114 }
hayama 0:da22b0b4395a 115 }
hayama 0:da22b0b4395a 116
hayama 0:da22b0b4395a 117 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 118 // interrupt by us timerR/L
hayama 0:da22b0b4395a 119 // motor rotation, mode = 0: free，1: forward，2: reverse，3: break
hayama 0:da22b0b4395a 120 // right motor rotation
hayama 0:da22b0b4395a 121 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 122 void stepMotor(){
hayama 0:da22b0b4395a 123
hayama 0:da22b0b4395a 124 if (timR>0) timR--; //count down timR，when timR=0 do next process
hayama 0:da22b0b4395a 125 if (timR==0) {
hayama 0:da22b0b4395a 126 timR=spdR;
hayama 0:da22b0b4395a 127 if (modeR==1) {if (patR < 3) patR++; else patR = 0; }
hayama 0:da22b0b4395a 128 if (modeR==2) {if (patR > 0) patR--; else patR = 3; }
hayama 0:da22b0b4395a 129 cntR++; // count up right moter step
hayama 0:da22b0b4395a 130 }
hayama 0:da22b0b4395a 131
hayama 0:da22b0b4395a 132 // left motor rotation
hayama 0:da22b0b4395a 133 if (timL>0) timL--; //count down timL，when timL=0 do next process
hayama 0:da22b0b4395a 134 if (timL==0) {
hayama 0:da22b0b4395a 135 timL=spdL;
hayama 0:da22b0b4395a 136 //modeL==1;
hayama 0:da22b0b4395a 137 if (modeL==1) {if (patL < 3) patL++; else patL = 0; }
hayama 0:da22b0b4395a 138 if (modeL==2) {if (patL > 0) patL--; else patL = 3; }
hayama 0:da22b0b4395a 139 cntL++; // count up left moter step
hayama 0:da22b0b4395a 140 }
hayama 0:da22b0b4395a 141
hayama 0:da22b0b4395a 142 if (modeR==0 || modeL==0) { patR=4; patL=4; } // motor free when mode=0
hayama 0:da22b0b4395a 143 motorR= RMOTOR[patR]; // pattern output to right motor
hayama 0:da22b0b4395a 144 motorL= LMOTOR[patL]; // pattern output to left motor
hayama 0:da22b0b4395a 145 }
hayama 0:da22b0b4395a 146
hayama 0:da22b0b4395a 147 // -----------------------------------------------
hayama 0:da22b0b4395a 148 // beep
hayama 0:da22b0b4395a 149 // -----------------------------------------------
hayama 0:da22b0b4395a 150 void beep(int n){
hayama 0:da22b0b4395a 151 bp=n; // set beep couter
hayama 0:da22b0b4395a 152 }
hayama 0:da22b0b4395a 153
hayama 0:da22b0b4395a 154 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 155 // check sensor value using serial port
hayama 0:da22b0b4395a 156 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 157 void check_sens(){
hayama 0:da22b0b4395a 158 while (1){
hayama 0:da22b0b4395a 159 pc.printf("sensC :"); pc.printf("%f\n",sensC); //
hayama 0:da22b0b4395a 160 pc.printf("sensSG :"); pc.printf("%f\n",sensSG); //
hayama 0:da22b0b4395a 161 pc.printf("sens1 :"); pc.printf("%f\n",sens1); //
hayama 0:da22b0b4395a 162 pc.printf("sens2 :"); pc.printf("%f\n",sens2); //
hayama 0:da22b0b4395a 163 pc.printf("sens3 :"); pc.printf("%f\n",sens3); //
hayama 0:da22b0b4395a 164 pc.printf("sens4 :"); pc.printf("%f\n",sens4); //
hayama 0:da22b0b4395a 165 pc.printf("sens5 :"); pc.printf("%f\n",sens5); //
hayama 0:da22b0b4395a 166 pc.printf("sens6 :"); pc.printf("%f\n",sens6); //
hayama 0:da22b0b4395a 167 pc.printf("sensD :"); pc.printf("%d\n",sensD);
hayama 0:da22b0b4395a 168 pc.printf("sensDout :"); pc.printf("%d\n",sensDout);
hayama 0:da22b0b4395a 169 wait (0.5);
hayama 0:da22b0b4395a 170 }
hayama 0:da22b0b4395a 171 }
hayama 0:da22b0b4395a 172
hayama 0:da22b0b4395a 173 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 174 // break and release motors
hayama 0:da22b0b4395a 175 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 176 void run_release(){
hayama 0:da22b0b4395a 177 modeR=0; modeL=0; // motor release
hayama 0:da22b0b4395a 178 }
hayama 0:da22b0b4395a 179 void run_break(){
hayama 0:da22b0b4395a 180 modeR=3; modeL=3; // mode 0 means break the motor
hayama 0:da22b0b4395a 181 wait(0.5);
hayama 0:da22b0b4395a 182 run_release();
hayama 0:da22b0b4395a 183 }
hayama 0:da22b0b4395a 184
hayama 0:da22b0b4395a 185 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 186 // run and turn
hayama 0:da22b0b4395a 187 // (mR,mL)=(1,1):forward, (2,1): turn right, (1,2): turn left, (2,2): Reverse
hayama 0:da22b0b4395a 188 // spd: speed,
hayama 0:da22b0b4395a 189 // nstep: number of step
hayama 0:da22b0b4395a 190 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 191 void runTurn(int mR,int mL, int spd, int nstep ){
hayama 0:da22b0b4395a 192 modeR=mR;modeL=mL;
hayama 0:da22b0b4395a 193 spdR=spdL=spd;
hayama 0:da22b0b4395a 194 cntR=0; stepR=nstep;
hayama 0:da22b0b4395a 195 while (cntR<stepR);
hayama 0:da22b0b4395a 196 }
hayama 0:da22b0b4395a 197
hayama 0:da22b0b4395a 198 //-----------------------
hayama 0:da22b0b4395a 199 // run
hayama 0:da22b0b4395a 200 // n: run speed, m: factor of reduce speed
hayama 0:da22b0b4395a 201 //----------------------
hayama 0:da22b0b4395a 202 void run(int n, int m){
hayama 0:da22b0b4395a 203 int cntGoal=0; // couter for run after goal
hayama 0:da22b0b4395a 204
hayama 0:da22b0b4395a 205 markerSG=0; markerC=0; markerX=0; fmarker=0;
hayama 0:da22b0b4395a 206 frun=0;
hayama 0:da22b0b4395a 207 runTurn(1,1,n*2,50); // slow start
hayama 0:da22b0b4395a 208
hayama 0:da22b0b4395a 209 while(startSw==1 ){
hayama 0:da22b0b4395a 210
hayama 0:da22b0b4395a 211 spdR=spdL=n;
hayama 0:da22b0b4395a 212 if (sensDout>0){
hayama 0:da22b0b4395a 213 spdR+=sensDout*m;
hayama 0:da22b0b4395a 214 } else {
hayama 0:da22b0b4395a 215 spdL-=sensDout*m;
hayama 0:da22b0b4395a 216 }
hayama 0:da22b0b4395a 217
hayama 0:da22b0b4395a 218 // corner marker check
hayama 0:da22b0b4395a 219 if (markerX==1) fmarker=0;
hayama 0:da22b0b4395a 220 if (markerX==0 && fmarker==0 && markerC>5) fmarker=1;
hayama 0:da22b0b4395a 221 if (markerX==0 && fmarker==1 && markerC==0){
hayama 0:da22b0b4395a 222 fmarker=0; beep(50);
hayama 0:da22b0b4395a 223 }
hayama 0:da22b0b4395a 224
hayama 0:da22b0b4395a 225 // start/goal marker check
hayama 0:da22b0b4395a 226 if (frun==0 && markerSG>SGTH) frun=1; // start marker detect
hayama 0:da22b0b4395a 227 if (frun==1 && markerSG==0){ // start marker fix
hayama 0:da22b0b4395a 228 frun=2; beep(100);
hayama 0:da22b0b4395a 229 }
hayama 0:da22b0b4395a 230 if (frun==2 && markerSG>SGTH) frun=3; // goal marker detect
hayama 0:da22b0b4395a 231 if (frun==3 && markerX==1) frun=2; // ignor cross line
hayama 0:da22b0b4395a 232 if (frun==3 && markerSG==0){ // goal marker fix
hayama 0:da22b0b4395a 233 frun=4; beep(100);
hayama 0:da22b0b4395a 234 cntGoal=cntR;
hayama 0:da22b0b4395a 235 }
hayama 0:da22b0b4395a 236 if (frun==4 && cntR>(cntGoal+500)) break;
hayama 0:da22b0b4395a 237 wait(0.005); // wait 5ms for control loop
hayama 0:da22b0b4395a 238 }
hayama 0:da22b0b4395a 239 run_break();
hayama 0:da22b0b4395a 240 }
hayama 0:da22b0b4395a 241
hayama 0:da22b0b4395a 242 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 243 // main
hayama 0:da22b0b4395a 244 //--------------------------------------------------------------------------
hayama 0:da22b0b4395a 245 int main(){
hayama 0:da22b0b4395a 246
hayama 0:da22b0b4395a 247 timerS.attach_us(&Sensor, 2000); // set timer for sensor
hayama 0:da22b0b4395a 248 timerM.attach_us(&stepMotor, 400); // set timer for motor
hayama 0:da22b0b4395a 249
hayama 0:da22b0b4395a 250 while (1) {
hayama 0:da22b0b4395a 251
hayama 0:da22b0b4395a 252 // initialize motor
hayama 0:da22b0b4395a 253 run_release();
hayama 0:da22b0b4395a 254
hayama 0:da22b0b4395a 255 while (startSw==1) { // program mode selection
hayama 0:da22b0b4395a 256 if (setSw==0) {
hayama 0:da22b0b4395a 257 wait(0.01);
hayama 0:da22b0b4395a 258 beep(50);
hayama 0:da22b0b4395a 259 while (setSw==0);
hayama 0:da22b0b4395a 260 wait(0.01);
hayama 0:da22b0b4395a 261 pmode++;
hayama 0:da22b0b4395a 262 if (pmode>7) pmode=0;
hayama 0:da22b0b4395a 263 }
hayama 0:da22b0b4395a 264 leds=pmode;
hayama 0:da22b0b4395a 265 }
hayama 0:da22b0b4395a 266 leds=0; beep(50);
hayama 0:da22b0b4395a 267 wait(0.5);
hayama 0:da22b0b4395a 268
hayama 0:da22b0b4395a 269 // go selected program
hayama 0:da22b0b4395a 270 switch(pmode){
hayama 0:da22b0b4395a 271 case 0: check_sens(); break; // check sensors
hayama 0:da22b0b4395a 272 case 1: runTurn(1,1,15,500); break; // run forward
hayama 0:da22b0b4395a 273 case 2: runTurn(2,1,15,500); break; // turn right
hayama 0:da22b0b4395a 274 case 3: run(10,30); break; // trace run
hayama 0:da22b0b4395a 275 case 4: run(8,24); break;
hayama 0:da22b0b4395a 276 case 5: run(6,18); break;
hayama 0:da22b0b4395a 277 case 6: run(5,15); break;
hayama 0:da22b0b4395a 278 case 7: run(4,12); break;
hayama 0:da22b0b4395a 279 }
hayama 0:da22b0b4395a 280 }
hayama 0:da22b0b4395a 281 }