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@1:4f623bfc5fdd, 2013-11-13 (annotated)

Committer:
hayama
Date:
Wed Nov 13 09:48:01 2013 +0000
Revision:
1:4f623bfc5fdd
Parent:
0:c154c65c5cc7 
This micromouse is for educational use in our College. The hardware and software is very simple.

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