Kiyoteru Hayama / Mbed 2 deprecated mbedMicromouse

This micromouse is for educational use in our College. The hardware and software is very simple.

Dependencies:   mbed

/media/uploads/hayama/cimg6733.jpg

/media/uploads/hayama/mbedmicromouse-manual-japanese-only.pdf

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

details (in Japanese), http://plaza.rakuten.co.jp/CPU4Edu/20018

you can see the movie on youtube (for education) -> http://youtu.be/UYi81i8WVtI

(for competition using high torque motor) -> http://youtu.be/fJDyqnC91YY

main.cpp@0:c154c65c5cc7, 2013-07-04 (annotated)

Committer:
hayama
Date:
Thu Jul 04 16:01:36 2013 +0000
Revision:
0:c154c65c5cc7
Child:
1:4f623bfc5fdd
mbed Micromouse for education

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hayama 0:c154c65c5cc7 1 //**************************************************************************
hayama 0:c154c65c5cc7 2 //
hayama 0:c154c65c5cc7 3 // KNCT-MMEdu for mbed
hayama 0:c154c65c5cc7 4 // (c) Kiyoteru Hayama(Kumamoto National College of Technology)
hayama 0:c154c65c5cc7 5 //
hayama 0:c154c65c5cc7 6 //**************************************************************************
hayama 0:c154c65c5cc7 7 #include "mbed.h"
hayama 0:c154c65c5cc7 8
hayama 0:c154c65c5cc7 9 // run parameters
hayama 0:c154c65c5cc7 10 #define LSPD 5 // timer count for low speed
hayama 0:c154c65c5cc7 11 #define HSPD 3 // timer count for high speed
hayama 0:c154c65c5cc7 12 #define STEP1 665 // number of step for 1 maze area
hayama 0:c154c65c5cc7 13 #define R90 265 // number of step for 90 degree right turn
hayama 0:c154c65c5cc7 14 #define L90 265 // number of step for 90 degree left turn
hayama 0:c154c65c5cc7 15 #define R180 530 // number of step for 180 degree u-turn
hayama 0:c154c65c5cc7 16 #define DISFR 16 // front right sensor value in normal micromouse position
hayama 0:c154c65c5cc7 17 #define DISFL 16 // front left sensor value in normal micromouse position
hayama 0:c154c65c5cc7 18 #define DISR 8 // right sensor value in normal micromouse position
hayama 0:c154c65c5cc7 19 #define DISL 8 // left sensor value in normal micromouse position
hayama 0:c154c65c5cc7 20 #define DISFMAX 2 // threshold of sensor value for front wall detection
hayama 0:c154c65c5cc7 21 #define DISRMAX 2 // threshold of sensor value for right wall detection
hayama 0:c154c65c5cc7 22 #define DISLMAX 2 // threshold of sensor value for left wall detection
hayama 0:c154c65c5cc7 23
hayama 0:c154c65c5cc7 24 // pattern table for stepping motor
hayama 0:c154c65c5cc7 25 const unsigned char RMOTOR[]={0x03, 0x06, 0x0C, 0x09, 0x00}; // magnetization pattern for right motor
hayama 0:c154c65c5cc7 26 const unsigned char LMOTOR[]={0x09, 0x0C, 0x06, 0x03, 0x00}; // magnetization pattern for left motor
hayama 0:c154c65c5cc7 27
hayama 0:c154c65c5cc7 28 const unsigned char DtoR[]={0,2,4,0,8,0,0,0,1}; // table indicating to the right direction
hayama 0:c154c65c5cc7 29 const unsigned char DtoL[]={0,8,1,0,2,0,0,0,4}; // table indicating to the left direction
hayama 0:c154c65c5cc7 30
hayama 0:c154c65c5cc7 31 unsigned char pmode=0; // program mode
hayama 0:c154c65c5cc7 32
hayama 0:c154c65c5cc7 33 // Variables. It is necessary to define as a Volatile when the variable used in interrupt.
hayama 0:c154c65c5cc7 34 volatile float ptFRB, ptFLB, ptRB, ptLB; // sensor values during turn-off the LED
hayama 0:c154c65c5cc7 35 volatile float sensFR, sensFL, sensR, sensL; // sensor values
hayama 0:c154c65c5cc7 36
hayama 0:c154c65c5cc7 37 volatile unsigned char modeR=0, modeL=0; // run forward both motor
hayama 0:c154c65c5cc7 38 volatile int stepR, stepL; // varilable for set step of motor
hayama 0:c154c65c5cc7 39 volatile unsigned char patR=0, patL=0; // index of motor pattern
hayama 0:c154c65c5cc7 40 volatile int cntR, cntL; // count of motor steps
hayama 0:c154c65c5cc7 41
hayama 0:c154c65c5cc7 42
hayama 0:c154c65c5cc7 43 volatile unsigned char timR=0, timL=0; // waiting timer for motors
hayama 0:c154c65c5cc7 44 volatile unsigned char timS; // waiting timer for sensors
hayama 0:c154c65c5cc7 45 volatile unsigned char fS=0; // flag for control of distanse from R,L walls
hayama 0:c154c65c5cc7 46 volatile unsigned char fR=0, fL=0; // flag of R, L motors, 0: low speed, 1:hight speed
hayama 0:c154c65c5cc7 47
hayama 0:c154c65c5cc7 48 union { // struct and union define for access map
hayama 0:c154c65c5cc7 49 unsigned char all; // map access by 1 byte
hayama 0:c154c65c5cc7 50 struct { unsigned char n:1; // 1 bit for north wall ￿i0:no wall, 1:exist wall￿j
hayama 0:c154c65c5cc7 51 unsigned char e:1; // 1 bit for east wall ￿i0:no wall, 1:exist wall￿j
hayama 0:c154c65c5cc7 52 unsigned char s:1; // 1 bit for south wall ￿i0:no wall, 1:exist wall￿j
hayama 0:c154c65c5cc7 53 unsigned char w:1; // 1 bit for west wall ￿i0:no wall, 1:exist wall￿j
hayama 0:c154c65c5cc7 54 unsigned char d:4; // 4bit for history
hayama 0:c154c65c5cc7 55 };
hayama 0:c154c65c5cc7 56 } mmap[16][16];
hayama 0:c154c65c5cc7 57
hayama 0:c154c65c5cc7 58 Ticker timer; // defince interval timer
hayama 0:c154c65c5cc7 59 Serial pc(USBTX, USBRX); // tx, rx
hayama 0:c154c65c5cc7 60
hayama 0:c154c65c5cc7 61 BusOut leds( LED4, LED3, LED2, LED1 ); // for LED display
hayama 0:c154c65c5cc7 62
hayama 0:c154c65c5cc7 63 BusOut motorR(p5, p6, p7, p8 ); // output for right motor
hayama 0:c154c65c5cc7 64 BusOut motorL(p11, p12, p13, p14 ); // output for left motor
hayama 0:c154c65c5cc7 65
hayama 0:c154c65c5cc7 66 AnalogIn ptFR(p15); // front right sensor, analog input
hayama 0:c154c65c5cc7 67 AnalogIn ptFL(p16); // front left sensor, analog input
hayama 0:c154c65c5cc7 68 AnalogIn ptR(p17); // right sensor, analog input
hayama 0:c154c65c5cc7 69 AnalogIn ptL(p18); // left sensor, analog input
hayama 0:c154c65c5cc7 70 AnalogIn gyro(p19); // for Gyro, analog input, reserved
hayama 0:c154c65c5cc7 71 DigitalIn setSw(p21); // set-switch, digital input
hayama 0:c154c65c5cc7 72 DigitalIn startSw(p22); // start-switch, digital input
hayama 0:c154c65c5cc7 73 DigitalOut ledFout(p9); // LED output signal for front wall detection
hayama 0:c154c65c5cc7 74 DigitalOut ledRLout(p10); // LED output signal for side wall detection
hayama 0:c154c65c5cc7 75
hayama 0:c154c65c5cc7 76 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 77 // LED display
hayama 0:c154c65c5cc7 78 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 79 void dispLED(unsigned char n)
hayama 0:c154c65c5cc7 80 {
hayama 0:c154c65c5cc7 81 leds=n;
hayama 0:c154c65c5cc7 82 }
hayama 0:c154c65c5cc7 83
hayama 0:c154c65c5cc7 84 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 85 // interrupt by timer
hayama 0:c154c65c5cc7 86 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 87 void SensAndMotor() {
hayama 0:c154c65c5cc7 88
hayama 0:c154c65c5cc7 89 // motor rotation, mode = 0: free￿C1: forward￿C2: reverse￿C3: break
hayama 0:c154c65c5cc7 90 // right motor rotation
hayama 0:c154c65c5cc7 91 if (timR>0) timR--; //count down timR￿Cwhen timR=0 do next process
hayama 0:c154c65c5cc7 92 if (timR==0) {
hayama 0:c154c65c5cc7 93 if (fR==0) timR=LSPD; else timR=HSPD;
hayama 0:c154c65c5cc7 94 if (modeR==1) {if (patR < 3) patR++; else patR = 0; }
hayama 0:c154c65c5cc7 95 if (modeR==2) {if (patR > 0) patR--; else patR = 3; }
hayama 0:c154c65c5cc7 96 cntR++; // count up right moter step
hayama 0:c154c65c5cc7 97 }
hayama 0:c154c65c5cc7 98 // left motor rotation
hayama 0:c154c65c5cc7 99 if (timL>0) timL--; //count down timL￿Cwhen timL=0 do next process
hayama 0:c154c65c5cc7 100 if (timL==0) {
hayama 0:c154c65c5cc7 101 if (fL==0) timL=LSPD; else timL=HSPD;
hayama 0:c154c65c5cc7 102 if (modeL==1) {if (patL < 3) patL++; else patL = 0; }
hayama 0:c154c65c5cc7 103 if (modeL==2) {if (patL > 0) patL--; else patL = 3; }
hayama 0:c154c65c5cc7 104 cntL++; // count up left moter step
hayama 0:c154c65c5cc7 105 }
hayama 0:c154c65c5cc7 106
hayama 0:c154c65c5cc7 107 if (modeR==0 || modeL==0) { patR=4; patL=4; } // motor free when mode=0
hayama 0:c154c65c5cc7 108 motorR= RMOTOR[patR]; // pattern output to right motor
hayama 0:c154c65c5cc7 109 motorL= LMOTOR[patL]; // pattern output to left motor
hayama 0:c154c65c5cc7 110
hayama 0:c154c65c5cc7 111 // read sensors
hayama 0:c154c65c5cc7 112 // 1st-step:measure background during LED-off, 2nd-step: measure reflecting light during LED-on. sensor value is differnce of both.
hayama 0:c154c65c5cc7 113 if (timS<20) timS++; else timS=0; // set counter timS
hayama 0:c154c65c5cc7 114 if (timS==0){
hayama 0:c154c65c5cc7 115 ptFRB=ptFR; // measure all background values
hayama 0:c154c65c5cc7 116 ptFLB=ptFL; // measure all background values
hayama 0:c154c65c5cc7 117 ledFout=1; // LED-ON
hayama 0:c154c65c5cc7 118 wait_us(100); // delay
hayama 0:c154c65c5cc7 119 sensFR=(ptFR-ptFRB)*20;
hayama 0:c154c65c5cc7 120 sensFL=(ptFL-ptFLB)*20;
hayama 0:c154c65c5cc7 121 ledFout=0; // LED-OFF
hayama 0:c154c65c5cc7 122 }
hayama 0:c154c65c5cc7 123 if (timS==10){
hayama 0:c154c65c5cc7 124 ptRB=ptR;
hayama 0:c154c65c5cc7 125 ptLB=ptL;
hayama 0:c154c65c5cc7 126 ledRLout=1;
hayama 0:c154c65c5cc7 127 wait_us(100);
hayama 0:c154c65c5cc7 128 sensR=(ptR-ptRB)*20;
hayama 0:c154c65c5cc7 129 sensL=(ptL-ptLB)*20;
hayama 0:c154c65c5cc7 130 ledRLout=0;
hayama 0:c154c65c5cc7 131 }
hayama 0:c154c65c5cc7 132
hayama 0:c154c65c5cc7 133 // set motor control flag by distance of both side walls
hayama 0:c154c65c5cc7 134 // use only right wall when right wall detected, use left wall when detected left wall only.
hayama 0:c154c65c5cc7 135 if (fS==1){ // do the following process, when flag fS=1
hayama 0:c154c65c5cc7 136 fR=fL=1; // set high speed for both motor
hayama 0:c154c65c5cc7 137 if(sensR>DISRMAX){ // when right wall exists,
hayama 0:c154c65c5cc7 138 if ((sensR-DISR)>4) fL=0; // set low speed for left moter, when close to the right wall
hayama 0:c154c65c5cc7 139 if ((sensR-DISR)<-4) fR=0; // set low speed for right moter, when close to the left wall
hayama 0:c154c65c5cc7 140 } else if(sensL>DISLMAX){ // when existing left wall only,
hayama 0:c154c65c5cc7 141 if ((sensL-DISL)>4) fR=0; // similar to the control by right wall.
hayama 0:c154c65c5cc7 142 if ((sensL-DISL)<-4) fL=0;
hayama 0:c154c65c5cc7 143 }
hayama 0:c154c65c5cc7 144 } else { fR=fL=0; } // when fS=0, set low speed for both motor
hayama 0:c154c65c5cc7 145 }
hayama 0:c154c65c5cc7 146
hayama 0:c154c65c5cc7 147 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 148 // check sensor value using serial port
hayama 0:c154c65c5cc7 149 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 150 void check_sens(){
hayama 0:c154c65c5cc7 151 while (1){
hayama 0:c154c65c5cc7 152 pc.printf("\f");
hayama 0:c154c65c5cc7 153 pc.printf("Sensor FR:%f \n",sensFR);
hayama 0:c154c65c5cc7 154 pc.printf("Sensor FL:%f \n",sensFL);
hayama 0:c154c65c5cc7 155 pc.printf("Sensor R:%f \n",sensR);
hayama 0:c154c65c5cc7 156 pc.printf("Sensor L:%f \n",sensL);
hayama 0:c154c65c5cc7 157 wait (0.5);
hayama 0:c154c65c5cc7 158 }
hayama 0:c154c65c5cc7 159 }
hayama 0:c154c65c5cc7 160
hayama 0:c154c65c5cc7 161 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 162 // break motors
hayama 0:c154c65c5cc7 163 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 164 void run_break(){
hayama 0:c154c65c5cc7 165 modeR=0; modeL=0; // mode 0 means break the motor
hayama 0:c154c65c5cc7 166 }
hayama 0:c154c65c5cc7 167
hayama 0:c154c65c5cc7 168 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 169 // adjustment by front wall
hayama 0:c154c65c5cc7 170 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 171 void adjust(){
hayama 0:c154c65c5cc7 172 fS=0; // set low speed
hayama 0:c154c65c5cc7 173 while(abs((sensFR-DISFR)-(sensFL-DISFL))>4){ // do adjustment when difference of sensor value larger than threshold(20)
hayama 0:c154c65c5cc7 174 if ((sensFR-DISFR)>(sensFL-DISFL)) {
hayama 0:c154c65c5cc7 175 modeR=2; modeL=1; // turn right
hayama 0:c154c65c5cc7 176 } else {
hayama 0:c154c65c5cc7 177 modeR=1; modeL=2; // turn left
hayama 0:c154c65c5cc7 178 }
hayama 0:c154c65c5cc7 179 }
hayama 0:c154c65c5cc7 180 run_break();
hayama 0:c154c65c5cc7 181 }
hayama 0:c154c65c5cc7 182
hayama 0:c154c65c5cc7 183 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 184 // slow start of the motors
hayama 0:c154c65c5cc7 185 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 186 void slow_start(){
hayama 0:c154c65c5cc7 187 fS=0; // set low speed
hayama 0:c154c65c5cc7 188 modeR=modeL=1; // set mode for run forward
hayama 0:c154c65c5cc7 189 cntR=0; stepR=20; // run 20 step at low speed
hayama 0:c154c65c5cc7 190 while (cntR<stepR);
hayama 0:c154c65c5cc7 191 }
hayama 0:c154c65c5cc7 192
hayama 0:c154c65c5cc7 193 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 194 // run forwad of 1 maze area
hayama 0:c154c65c5cc7 195 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 196 void run_step(){
hayama 0:c154c65c5cc7 197 slow_start();
hayama 0:c154c65c5cc7 198 fS=1; // change to high speed
hayama 0:c154c65c5cc7 199 cntR=0; stepR=STEP1-20;
hayama 0:c154c65c5cc7 200 while (cntR<stepR);
hayama 0:c154c65c5cc7 201 run_break();
hayama 0:c154c65c5cc7 202 }
hayama 0:c154c65c5cc7 203
hayama 0:c154c65c5cc7 204 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 205 // 90 degree turn right
hayama 0:c154c65c5cc7 206 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 207 void run_R90(){
hayama 0:c154c65c5cc7 208 fS=0; // set low speed
hayama 0:c154c65c5cc7 209 cntR=0; stepR=R90; // set motor step for turn 90 degree
hayama 0:c154c65c5cc7 210 modeR=2; modeL=1; // right motor: reverse, left motor: forward
hayama 0:c154c65c5cc7 211 while (cntR<stepR);
hayama 0:c154c65c5cc7 212 run_break();
hayama 0:c154c65c5cc7 213 }
hayama 0:c154c65c5cc7 214
hayama 0:c154c65c5cc7 215 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 216 // 90 degree turn left
hayama 0:c154c65c5cc7 217 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 218 void run_L90(){
hayama 0:c154c65c5cc7 219 fS=0; // set low speed
hayama 0:c154c65c5cc7 220 cntL=0; stepL=L90; // set motor step for turn 90 degree
hayama 0:c154c65c5cc7 221 modeR=1; modeL=2; // right motor: forward, left motor: reverse
hayama 0:c154c65c5cc7 222 while (cntL<stepL);
hayama 0:c154c65c5cc7 223 modeR=0; modeL=0;
hayama 0:c154c65c5cc7 224 run_break();
hayama 0:c154c65c5cc7 225 }
hayama 0:c154c65c5cc7 226
hayama 0:c154c65c5cc7 227 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 228 // u-turn
hayama 0:c154c65c5cc7 229 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 230 void run_R180(){
hayama 0:c154c65c5cc7 231 fS=0; // set low speed
hayama 0:c154c65c5cc7 232 cntR=0; stepR=R180; // set motor step for turn 180 degree
hayama 0:c154c65c5cc7 233 modeR=2; modeL=1; // right motor: reverse, left motor: forward
hayama 0:c154c65c5cc7 234 while (cntR<stepR);
hayama 0:c154c65c5cc7 235 run_break();
hayama 0:c154c65c5cc7 236 }
hayama 0:c154c65c5cc7 237
hayama 0:c154c65c5cc7 238 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 239 // run forward and u-turn when front wall detected
hayama 0:c154c65c5cc7 240 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 241 void run_Turn(unsigned char n){
hayama 0:c154c65c5cc7 242 while (1){
hayama 0:c154c65c5cc7 243 slow_start();
hayama 0:c154c65c5cc7 244 fS=1; // set high speed
hayama 0:c154c65c5cc7 245 while (sensFR<DISFR); // run forward to normal distanse from front wall
hayama 0:c154c65c5cc7 246 adjust(); // adjestment by front wall
hayama 0:c154c65c5cc7 247 if (n==0) run_R180(); else run_R90(); // u-turn or turn right by the value of n
hayama 0:c154c65c5cc7 248 }
hayama 0:c154c65c5cc7 249 }
hayama 0:c154c65c5cc7 250
hayama 0:c154c65c5cc7 251 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 252 // left hand method￿iusing direction history￿j
hayama 0:c154c65c5cc7 253 // priority is left, front and right. if all the wall exists, then u-turn.
hayama 0:c154c65c5cc7 254 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 255 void run_Hidarite(){
hayama 0:c154c65c5cc7 256 unsigned char wF, wR, wL; // flag for front right left walls
hayama 0:c154c65c5cc7 257 unsigned char wS; // flag for sensing the walls
hayama 0:c154c65c5cc7 258 unsigned char mapF, mapR, mapL; // variable to read the history
hayama 0:c154c65c5cc7 259 unsigned char mx,my; // x and y axis of mouse, start posisiton is 0,0
hayama 0:c154c65c5cc7 260 unsigned char md; // direction of the mouse￿Cnorth:1￿Ceast:2, south:4, west:8
hayama 0:c154c65c5cc7 261
hayama 0:c154c65c5cc7 262 mapF=0; mapR=0; mapL=0; // initiallize
hayama 0:c154c65c5cc7 263 mx=0; my=0; md=1; // initiallize
hayama 0:c154c65c5cc7 264 wF=0; wR=1; wL=1; // initiallize
hayama 0:c154c65c5cc7 265 mmap[0][0].d=1; // inital direction is north (1)
hayama 0:c154c65c5cc7 266
hayama 0:c154c65c5cc7 267 while (startSw==1){ // repeat durning no sw input detection (push start-sw to exit )
hayama 0:c154c65c5cc7 268
hayama 0:c154c65c5cc7 269 // reade history ￿imapF,mapR,mapL are the history of front, right, left area)
hayama 0:c154c65c5cc7 270 // no access to the out of range￿D
hayama 0:c154c65c5cc7 271 switch (md){
hayama 0:c154c65c5cc7 272 case 1: if (my<15) mapF=mmap[my+1][mx].d; // when mouse direction is north,
hayama 0:c154c65c5cc7 273 if (mx<15) mapR=mmap[my][mx+1].d;
hayama 0:c154c65c5cc7 274 if (mx>0) mapL=mmap[my][mx-1].d;
hayama 0:c154c65c5cc7 275 break;
hayama 0:c154c65c5cc7 276 case 2: if (mx<15) mapF=mmap[my][mx+1].d; // when mouse direction is east,
hayama 0:c154c65c5cc7 277 if (my>0) mapR=mmap[my-1][mx].d;
hayama 0:c154c65c5cc7 278 if (my<15) mapL=mmap[my+1][mx].d;
hayama 0:c154c65c5cc7 279 break;
hayama 0:c154c65c5cc7 280 case 4: if (my>0) mapF=mmap[my-1][mx].d; // when mouse direction is south,
hayama 0:c154c65c5cc7 281 if (mx>0) mapR=mmap[my][mx-1].d;
hayama 0:c154c65c5cc7 282 if (mx<15) mapL=mmap[my][mx+1].d;
hayama 0:c154c65c5cc7 283 break;
hayama 0:c154c65c5cc7 284 case 8: if (mx>0) mapF=mmap[my][mx-1].d; // when mouse direction is west,
hayama 0:c154c65c5cc7 285 if (my<15) mapR=mmap[my+1][mx].d;
hayama 0:c154c65c5cc7 286 if (my>0) mapL=mmap[my-1][mx].d;
hayama 0:c154c65c5cc7 287 break;
hayama 0:c154c65c5cc7 288 }
hayama 0:c154c65c5cc7 289
hayama 0:c154c65c5cc7 290 // decision by left hand rule
hayama 0:c154c65c5cc7 291 if (wL ==0 && (mapL==0 || mapL==DtoL[md])) // left turn when no left wall and no history or available history
hayama 0:c154c65c5cc7 292 {run_L90(); md=DtoL[md]; }
hayama 0:c154c65c5cc7 293 else if (wF==0 && (mapF==0 || mapF==md)){} // go forward when no front wall and no history or available history (pass the turn process)
hayama 0:c154c65c5cc7 294 else if (wR==0 && (mapR==0 || mapR==DtoR[md])) // right turn when no left wall and no history or available history
hayama 0:c154c65c5cc7 295 {run_R90(); md=DtoR[md]; }
hayama 0:c154c65c5cc7 296 else {run_R180(); md=DtoR[md]; md=DtoR[md];} //u-turn
hayama 0:c154c65c5cc7 297
hayama 0:c154c65c5cc7 298 // go forward and detect walls
hayama 0:c154c65c5cc7 299 wS=0; wF=0; wR=0; wL=0; // reset of wall flags
hayama 0:c154c65c5cc7 300 slow_start(); // slow start
hayama 0:c154c65c5cc7 301 stepR=STEP1;
hayama 0:c154c65c5cc7 302 fS=1; // change high speed
hayama 0:c154c65c5cc7 303 while (cntR<stepR){ // go forward
hayama 0:c154c65c5cc7 304 if (cntR > (STEP1*2/3) && wS==0){ // wall detection when the mouse run 2/3 step of area
hayama 0:c154c65c5cc7 305 wS=1; // set flag to detect wall at once.
hayama 0:c154c65c5cc7 306 if (sensR > DISRMAX) wR=1; else wR=0; // detection of right wall
hayama 0:c154c65c5cc7 307 if (sensL > DISLMAX) wL=1; else wL=0; // detection of left wall
hayama 0:c154c65c5cc7 308 if ((sensFR>DISFMAX || sensFL>DISFMAX)){ wF=1; break; } // detection of front wall. exit from loop when front wall detected.
hayama 0:c154c65c5cc7 309 }
hayama 0:c154c65c5cc7 310 }
hayama 0:c154c65c5cc7 311
hayama 0:c154c65c5cc7 312 // go forwrd to adjust distanse of front wall, when exit the above loop by front wall detection.
hayama 0:c154c65c5cc7 313 if (wF==1){
hayama 0:c154c65c5cc7 314 while (sensFR<DISFR); // go forward to have normal distance to front wall
hayama 0:c154c65c5cc7 315 adjust(); // adjustment by front wall
hayama 0:c154c65c5cc7 316 }
hayama 0:c154c65c5cc7 317
hayama 0:c154c65c5cc7 318 // write map and history ( opposit direction of out of the area )
hayama 0:c154c65c5cc7 319 // record map after update mouse axis
hayama 0:c154c65c5cc7 320 switch (md){
hayama 0:c154c65c5cc7 321 case 1: mmap[my][mx].d=4; my++; mmap[my][mx].n=wF; mmap[my][mx].e=wR; mmap[my][mx].w=wL; break;
hayama 0:c154c65c5cc7 322 case 2: mmap[my][mx].d=8; mx++; mmap[my][mx].e=wF; mmap[my][mx].s=wR; mmap[my][mx].n=wL; break;
hayama 0:c154c65c5cc7 323 case 4: mmap[my][mx].d=1; my--; mmap[my][mx].s=wF; mmap[my][mx].w=wR; mmap[my][mx].e=wL; break;
hayama 0:c154c65c5cc7 324 case 8: mmap[my][mx].d=2; mx--; mmap[my][mx].w=wF; mmap[my][mx].n=wR; mmap[my][mx].s=wL; break;
hayama 0:c154c65c5cc7 325 }
hayama 0:c154c65c5cc7 326 if (mx==0 && my==0) { run_break(); break; } // finish search run when mouse return start position
hayama 0:c154c65c5cc7 327 }
hayama 0:c154c65c5cc7 328 }
hayama 0:c154c65c5cc7 329
hayama 0:c154c65c5cc7 330 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 331 // fast run, find minimum route to goal and run fast
hayama 0:c154c65c5cc7 332 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 333 void run_saitan(){
hayama 0:c154c65c5cc7 334 unsigned char i,j,k,m;
hayama 0:c154c65c5cc7 335 unsigned char smap[16][16]; // map for calculate minimum route
hayama 0:c154c65c5cc7 336 unsigned char run[256]; // array for run pattern
hayama 0:c154c65c5cc7 337 unsigned char md; // direction of mouse 1:north, 2:east, 4:south, 8:west
hayama 0:c154c65c5cc7 338
hayama 0:c154c65c5cc7 339 // clear map and set walls for no histry area.
hayama 0:c154c65c5cc7 340 for(i=0;i<16;i++){
hayama 0:c154c65c5cc7 341 for(j=0;j<16;j++){
hayama 0:c154c65c5cc7 342 smap[i][j]=0;
hayama 0:c154c65c5cc7 343 if (mmap[i][j].d==0){
hayama 0:c154c65c5cc7 344 mmap[i][j].n=1; if (i<15) mmap[i+1][j].s=1;
hayama 0:c154c65c5cc7 345 mmap[i][j].e=1; if (j<15) mmap[i][j+1].w=1;
hayama 0:c154c65c5cc7 346 mmap[i][j].s=1; if (i>0) mmap[i-1][j].n=1;
hayama 0:c154c65c5cc7 347 mmap[i][j].w=1; if (j>0) mmap[i][j-1].e=1;
hayama 0:c154c65c5cc7 348 }
hayama 0:c154c65c5cc7 349 }
hayama 0:c154c65c5cc7 350 }
hayama 0:c154c65c5cc7 351
hayama 0:c154c65c5cc7 352 // write steps to smap from goal position
hayama 0:c154c65c5cc7 353 // goal position set to m=1, find same value of m in smap and put m+1 to no wall direction, increment of m,
hayama 0:c154c65c5cc7 354 // go out roop when reach to stat position.
hayama 0:c154c65c5cc7 355 smap[7][7]=1; smap[7][8]=1; smap[8][7]=1; smap[8][8]=1; // goal position set to 1
hayama 0:c154c65c5cc7 356 m=1; // set m=1
hayama 0:c154c65c5cc7 357 for(k=0;k<255;k++){ // repeat maximun 255 times
hayama 0:c154c65c5cc7 358 for(i=0;i<16;i++){
hayama 0:c154c65c5cc7 359 for(j=0;j<16;j++){ // scan all areas
hayama 0:c154c65c5cc7 360 if (smap[i][j]==m){
hayama 0:c154c65c5cc7 361 if (mmap[i][j].n==0 && i<15 && smap[i+1][j]==0) smap[i+1][j]=m+1;
hayama 0:c154c65c5cc7 362 if (mmap[i][j].e==0 && j<15 && smap[i][j+1]==0) smap[i][j+1]=m+1;
hayama 0:c154c65c5cc7 363 if (mmap[i][j].s==0 && i>0 && smap[i-1][j]==0) smap[i-1][j]=m+1;
hayama 0:c154c65c5cc7 364 if (mmap[i][j].w==0 && j>0 && smap[i][j-1]==0) smap[i][j-1]=m+1;
hayama 0:c154c65c5cc7 365 }
hayama 0:c154c65c5cc7 366 }
hayama 0:c154c65c5cc7 367 }
hayama 0:c154c65c5cc7 368 m++; // increment of m
hayama 0:c154c65c5cc7 369 if (smap[0][0]!=0) break; // go out of loop
hayama 0:c154c65c5cc7 370 }
hayama 0:c154c65c5cc7 371
hayama 0:c154c65c5cc7 372 // make run pattern to run[k] array
hayama 0:c154c65c5cc7 373 // k:number of run pattern, 1:go forward, 2:turn right, 3:turn left
hayama 0:c154c65c5cc7 374 m=smap[0][0]-1; // set m to start position
hayama 0:c154c65c5cc7 375 i=0; j=0; k=0;
hayama 0:c154c65c5cc7 376 md=1;
hayama 0:c154c65c5cc7 377 while (m>0){ // loop while reach to goal position
hayama 0:c154c65c5cc7 378 switch(md){
hayama 0:c154c65c5cc7 379 case 1: if (mmap[i][j].n==0 && smap[i+1][j]==m && i<15) {run[k]=1; i++; m--; break;}
hayama 0:c154c65c5cc7 380 if (mmap[i][j].e==0 && smap[i][j+1]==m && j<15) {run[k]=2; md=DtoR[md]; break;}
hayama 0:c154c65c5cc7 381 if (mmap[i][j].w==0 && smap[i][j-1]==m && j>0 ) {run[k]=3; md=DtoL[md]; break;}
hayama 0:c154c65c5cc7 382 case 2: if (mmap[i][j].e==0 && smap[i][j+1]==m && j<15) {run[k]=1; j++; m--; break;}
hayama 0:c154c65c5cc7 383 if (mmap[i][j].s==0 && smap[i-1][j]==m && i>0 ) {run[k]=2; md=DtoR[md]; break;}
hayama 0:c154c65c5cc7 384 if (mmap[i][j].n==0 && smap[i+1][j]==m && i<15) {run[k]=3; md=DtoL[md]; break;}
hayama 0:c154c65c5cc7 385 case 4: if (mmap[i][j].s==0 && smap[i-1][j]==m && i>0 ) {run[k]=1; i--; m--; break;}
hayama 0:c154c65c5cc7 386 if (mmap[i][j].w==0 && smap[i][j-1]==m && j>0 ) {run[k]=2; md=DtoR[md]; break;}
hayama 0:c154c65c5cc7 387 if (mmap[i][j].e==0 && smap[i][j+1]==m && j<15) {run[k]=3; md=DtoL[md]; break;}
hayama 0:c154c65c5cc7 388 case 8: if (mmap[i][j].w==0 && smap[i][j-1]==m && j>0 ) {run[k]=1; j--; m--; break;}
hayama 0:c154c65c5cc7 389 if (mmap[i][j].n==0 && smap[i+1][j]==m && i<15) {run[k]=2; md=DtoR[md]; break;}
hayama 0:c154c65c5cc7 390 if (mmap[i][j].s==0 && smap[i-1][j]==m && i>0 ) {run[k]=3; md=DtoL[md]; break;}
hayama 0:c154c65c5cc7 391 }
hayama 0:c154c65c5cc7 392 k++;
hayama 0:c154c65c5cc7 393 }
hayama 0:c154c65c5cc7 394
hayama 0:c154c65c5cc7 395 // run minimun route
hayama 0:c154c65c5cc7 396 i=0;
hayama 0:c154c65c5cc7 397 while (i<k){
hayama 0:c154c65c5cc7 398 if (run[i]==1) { run_step(); i++; }
hayama 0:c154c65c5cc7 399 if (run[i]==2) { run_R90(); i++; }
hayama 0:c154c65c5cc7 400 if (run[i]==3) { run_L90(); i++; }
hayama 0:c154c65c5cc7 401 }
hayama 0:c154c65c5cc7 402 }
hayama 0:c154c65c5cc7 403
hayama 0:c154c65c5cc7 404 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 405 // main
hayama 0:c154c65c5cc7 406 //--------------------------------------------------------------------------
hayama 0:c154c65c5cc7 407 int main(){
hayama 0:c154c65c5cc7 408 int i,j;
hayama 0:c154c65c5cc7 409
hayama 0:c154c65c5cc7 410 // initialize map
hayama 0:c154c65c5cc7 411 for (i=0; i<16;i++) for (j=0;j<16;j++) mmap[i][j].all=0; // clear map
hayama 0:c154c65c5cc7 412 for (i=0; i<16;i++){
hayama 0:c154c65c5cc7 413 mmap[i][0].w=1; mmap[i][15].e=1; // set east and west wall
hayama 0:c154c65c5cc7 414 mmap[0][i].s=1; mmap[15][i].n=1; } // set north and south wall
hayama 0:c154c65c5cc7 415 mmap[0][0].e=1;
hayama 0:c154c65c5cc7 416
hayama 0:c154c65c5cc7 417 timer.attach(&SensAndMotor, 0.001); // set timer to 1ms for timer interrupt
hayama 0:c154c65c5cc7 418
hayama 0:c154c65c5cc7 419 while (1) {
hayama 0:c154c65c5cc7 420
hayama 0:c154c65c5cc7 421 // initialize parameters
hayama 0:c154c65c5cc7 422 ledFout=ledRLout=0;
hayama 0:c154c65c5cc7 423 motorR=motorL=0;
hayama 0:c154c65c5cc7 424
hayama 0:c154c65c5cc7 425 while (startSw==1) {
hayama 0:c154c65c5cc7 426 if (setSw==0) {
hayama 0:c154c65c5cc7 427 wait(0.01);
hayama 0:c154c65c5cc7 428 while (setSw==0);
hayama 0:c154c65c5cc7 429 wait(0.01);
hayama 0:c154c65c5cc7 430 pmode++;
hayama 0:c154c65c5cc7 431 if (pmode>7) pmode=0;
hayama 0:c154c65c5cc7 432 }
hayama 0:c154c65c5cc7 433 leds=pmode;
hayama 0:c154c65c5cc7 434 }
hayama 0:c154c65c5cc7 435 leds=0;
hayama 0:c154c65c5cc7 436 wait(0.5);
hayama 0:c154c65c5cc7 437
hayama 0:c154c65c5cc7 438 // go selected functions
hayama 0:c154c65c5cc7 439 switch(pmode){
hayama 0:c154c65c5cc7 440 case 0: check_sens(); break; // check sensors
hayama 0:c154c65c5cc7 441 case 1: run_step(); break; // run 1 area step
hayama 0:c154c65c5cc7 442 case 2: run_R90(); break; // 90 deg. turn right
hayama 0:c154c65c5cc7 443 case 3: run_L90(); break; // 90 deg. turn left
hayama 0:c154c65c5cc7 444 case 4: run_R180(); break; // u-turn
hayama 0:c154c65c5cc7 445 case 5: run_Turn(0); break; // go forward and u-turn
hayama 0:c154c65c5cc7 446 case 6: run_Hidarite(); break; // left hand rule
hayama 0:c154c65c5cc7 447 case 7: run_saitan(); break; // fast run for minimum route
hayama 0:c154c65c5cc7 448 }
hayama 0:c154c65c5cc7 449 }
hayama 0:c154c65c5cc7 450 }