File content as of revision 0:c154c65c5cc7:

//**************************************************************************
//                                                                                  
//    KNCT-MMEdu for mbed     
//    (c) Kiyoteru Hayama(Kumamoto National College of Technology)
//
//**************************************************************************
#include "mbed.h"

// run parameters
#define LSPD    5                       // timer count for low speed 
#define HSPD    3                        // timer count for high speed 
#define STEP1   665                       // number of step for 1 maze area
#define R90     265                       // number of step for 90 degree right turn 
#define L90     265                       //  number of step for 90 degree left turn
#define R180    530                      //  number of step for 180 degree u-turn 
#define DISFR   16                      // front right sensor value in normal micromouse position 
#define DISFL   16                      // front left sensor value in normal micromouse position 
#define DISR    8                       // right sensor value in normal micromouse position
#define DISL    8                       // left sensor value in normal micromouse position  
#define DISFMAX 2                       // threshold of sensor value for front wall detection
#define DISRMAX 2                        // threshold of sensor value for right  wall detection
#define DISLMAX 2                     // threshold of sensor value for left wall detection

// pattern table for stepping motor 
const unsigned char RMOTOR[]={0x03, 0x06, 0x0C, 0x09, 0x00};    // magnetization pattern for right motor 
const unsigned char LMOTOR[]={0x09, 0x0C, 0x06, 0x03, 0x00};    // magnetization pattern for left motor 

const unsigned char DtoR[]={0,2,4,0,8,0,0,0,1};   // table indicating to the right direction
const unsigned char DtoL[]={0,8,1,0,2,0,0,0,4};     // table indicating to the left direction  

unsigned char pmode=0;                            //  program mode

// Variables. It is necessary to define as a Volatile  when the variable used in interrupt.
volatile float ptFRB, ptFLB, ptRB, ptLB;          // sensor values during turn-off the LED
volatile float sensFR, sensFL, sensR, sensL;      // sensor values

volatile unsigned char modeR=0, modeL=0;           // run forward both motor  
volatile int stepR, stepL;                        // varilable for set step of motor
volatile unsigned char patR=0, patL=0;            // index of motor pattern  
volatile int cntR, cntL;                          // count of motor steps 


volatile unsigned char timR=0, timL=0;              // waiting timer for motors
volatile unsigned char timS;                      // waiting timer for sensors 
volatile unsigned char fS=0;                      // flag for control of distanse from R,L walls
volatile unsigned char fR=0, fL=0;                 // flag of R, L motors, 0: low speed, 1:hight speed

union {                                            // struct and union define for access map 
       unsigned char all;                         // map access by 1 byte 
       struct { unsigned char n:1;                // 1 bit for north wall ￿i0:no wall, 1:exist wall￿j  
                unsigned char e:1;                // 1 bit for east wall ￿i0:no wall, 1:exist wall￿j   
                unsigned char s:1;                  // 1 bit for south wall ￿i0:no wall, 1:exist wall￿j  
                unsigned char w:1;              // 1 bit for west wall ￿i0:no wall, 1:exist wall￿j    
                unsigned char d:4;                 // 4bit for history  
              };
        } mmap[16][16]; 

Ticker timer;                                  // defince interval timer 
Serial pc(USBTX, USBRX);                       // tx, rx

BusOut   leds( LED4, LED3, LED2, LED1 );    // for LED display

BusOut motorR(p5,  p6,  p7,  p8 );          // output for right motor
BusOut motorL(p11, p12, p13, p14 );         // output for left motor

AnalogIn ptFR(p15);                        // front right sensor, analog input
AnalogIn ptFL(p16);                        // front left sensor, analog input
AnalogIn ptR(p17);                        // right sensor, analog input
AnalogIn ptL(p18);                         // left sensor, analog input
AnalogIn gyro(p19);                        // for Gyro, analog input, reserved 
DigitalIn setSw(p21);                       // set-switch, digital input
DigitalIn startSw(p22);                 // start-switch, digital input
DigitalOut ledFout(p9);                 // LED output signal for front wall detection 
DigitalOut ledRLout(p10);                // LED output signal for side wall detection
     
//-------------------------------------------------------------------------- 
//  LED display                                                      
//-------------------------------------------------------------------------- 
void dispLED(unsigned char n)
{
  leds=n;
}

//-------------------------------------------------------------------------- 
// interrupt by timer                                             
//-------------------------------------------------------------------------- 
void SensAndMotor() {    

// motor rotation,  mode = 0: free￿C1: forward￿C2: reverse￿C3: break
// right motor rotation 
  if (timR>0) timR--;                        //count down timR￿Cwhen timR=0 do next process
  if (timR==0) {
    if (fR==0) timR=LSPD; else timR=HSPD;
    if (modeR==1) {if (patR < 3) patR++; else patR = 0; }
    if (modeR==2) {if (patR > 0) patR--; else patR = 3; }     
    cntR++;                                 // count up right moter step 
  }
// left motor rotation 
  if (timL>0) timL--;                          //count down timL￿Cwhen timL=0 do next process
  if (timL==0) {
    if (fL==0) timL=LSPD; else timL=HSPD;
    if (modeL==1) {if (patL < 3) patL++; else patL = 0; }
    if (modeL==2) {if (patL > 0) patL--; else patL = 3; }          
    cntL++;                                    // count up left moter step  
  }
    
    if (modeR==0 || modeL==0) { patR=4; patL=4; }   // motor free when mode=0  
    motorR= RMOTOR[patR];       // pattern output to right motor
    motorL= LMOTOR[patL];         // pattern output to left motor

    // read sensors
    // 1st-step:measure background during LED-off, 2nd-step: measure reflecting light during LED-on. sensor value is differnce of both.
    if (timS<20) timS++; else timS=0;         // set counter timS  
    if (timS==0){ 
        ptFRB=ptFR;                          // measure all background values 
        ptFLB=ptFL;                          // measure all background values 
        ledFout=1;                          // LED-ON
        wait_us(100);                       // delay 
        sensFR=(ptFR-ptFRB)*20;
        sensFL=(ptFL-ptFLB)*20;
        ledFout=0;                          // LED-OFF
    }
    if (timS==10){ 
        ptRB=ptR;                       
        ptLB=ptL;
        ledRLout=1;                        
        wait_us(100);                       
        sensR=(ptR-ptRB)*20;
        sensL=(ptL-ptLB)*20;
        ledRLout=0;                        
    }        

// set motor control flag by distance of both side walls 
// use only right wall when right wall detected,  use left wall when detected left wall only.
    if (fS==1){                                  // do the following process, when flag fS=1
        fR=fL=1;                               // set high speed for both motor
        if(sensR>DISRMAX){                       // when right wall exists,   
        if ((sensR-DISR)>4) fL=0;                // set low speed for left moter, when close to the right wall 
        if ((sensR-DISR)<-4) fR=0;               // set low speed for right moter, when close to the left wall       
    } else if(sensL>DISLMAX){                   // when existing left wall only, 
        if ((sensL-DISL)>4) fR=0;               // similar to the control by right wall.
        if ((sensL-DISL)<-4) fL=0;
    }
    } else { fR=fL=0; }                          // when fS=0, set low speed for both motor
}

//-------------------------------------------------------------------------- 
// check sensor value using serial port                          
//-------------------------------------------------------------------------- 
void check_sens(){  
  while (1){   
   pc.printf("\f");
   pc.printf("Sensor FR:%f \n",sensFR); 
   pc.printf("Sensor FL:%f \n",sensFL); 
   pc.printf("Sensor  R:%f \n",sensR); 
   pc.printf("Sensor  L:%f \n",sensL); 
   wait (0.5);
  }
}

//-------------------------------------------------------------------------- 
// break motors          
//-------------------------------------------------------------------------- 
void run_break(){
  modeR=0; modeL=0;                   // mode 0 means break the motor  
}

//-------------------------------------------------------------------------- 
// adjustment by front wall   
//-------------------------------------------------------------------------- 
void adjust(){
  fS=0;                                // set low speed 
  while(abs((sensFR-DISFR)-(sensFL-DISFL))>4){  // do adjustment when difference of sensor value larger than threshold(20)
  if ((sensFR-DISFR)>(sensFL-DISFL)) { 
      modeR=2; modeL=1;                // turn right 
    } else { 
      modeR=1; modeL=2;                  // turn left 
    } 
  } 
  run_break();
}

//-------------------------------------------------------------------------- 
// slow start of the motors             
//-------------------------------------------------------------------------- 
void slow_start(){
  fS=0;                                // set low speed 
  modeR=modeL=1;                       // set mode for run forward  
  cntR=0; stepR=20;                     // run 20 step at low speed
  while (cntR<stepR);            
}

//-------------------------------------------------------------------------- 
//    run forwad of 1 maze area      
//-------------------------------------------------------------------------- 
void run_step(){
  slow_start();
  fS=1;                                   // change to high speed 
  cntR=0; stepR=STEP1-20;             
  while (cntR<stepR);
  run_break();
}  

//-------------------------------------------------------------------------- 
// 90 degree turn right    
//-------------------------------------------------------------------------- 
void run_R90(){ 
  fS=0;                // set low speed  
  cntR=0; stepR=R90;     // set motor step for turn 90 degree 
  modeR=2; modeL=1;         // right motor: reverse, left motor: forward
  while (cntR<stepR);
  run_break();
}  

//-------------------------------------------------------------------------- 
// 90 degree turn left         
//-------------------------------------------------------------------------- 
void run_L90(){
  fS=0;                                  // set low speed  
  cntL=0; stepL=L90;                    // set motor step for turn 90 degree  
  modeR=1; modeL=2;                     // right motor: forward, left motor: reverse
  while (cntL<stepL);
  modeR=0; modeL=0;
  run_break();
}  

//-------------------------------------------------------------------------- 
// u-turn      
//-------------------------------------------------------------------------- 
void run_R180(){ 
  fS=0;                                  // set low speed 
  cntR=0; stepR=R180;                    // set motor step for turn 180 degree  
  modeR=2; modeL=1;                      // right motor: reverse, left motor: forward   
  while (cntR<stepR);
  run_break();
}  

//--------------------------------------------------------------------------
// run forward and u-turn when front wall detected
//--------------------------------------------------------------------------
void run_Turn(unsigned char n){
  while (1){
    slow_start();
    fS=1;                                // set high speed 
    while (sensFR<DISFR);             // run forward to normal distanse from front wall 
    adjust();                           // adjestment by front wall    
    if (n==0) run_R180(); else run_R90();    // u-turn or turn right by the value of n
  }
} 

//-------------------------------------------------------------------------- 
// left hand method￿iusing direction history￿j                                                 
//  priority is left, front and right. if all the wall exists, then u-turn.    
//-------------------------------------------------------------------------- 
void run_Hidarite(){
  unsigned char wF, wR, wL;               // flag for front right left walls 
  unsigned char wS;                         // flag for sensing the walls  
  unsigned char mapF, mapR, mapL;          // variable to read the history 
  unsigned char mx,my;                   // x and y axis of mouse, start posisiton is 0,0  
  unsigned char md;                      // direction of the mouse￿Cnorth:1￿Ceast:2, south:4, west:8 
  
  mapF=0; mapR=0; mapL=0;                     // initiallize 
  mx=0; my=0; md=1;                       // initiallize 
  wF=0; wR=1; wL=1;                         // initiallize 
  mmap[0][0].d=1;                         // inital direction is north (1)  

  while (startSw==1){                // repeat durning no sw input detection (push start-sw to exit )

  // reade history ￿imapF,mapR,mapL are the history of front, right, left area)
  // no access to the out of range￿D
      switch (md){
      case 1: if (my<15) mapF=mmap[my+1][mx].d; // when mouse direction is north,
              if (mx<15) mapR=mmap[my][mx+1].d; 
              if (mx>0)  mapL=mmap[my][mx-1].d; 
              break;
      case 2: if (mx<15) mapF=mmap[my][mx+1].d; // when mouse direction is east,
              if (my>0)  mapR=mmap[my-1][mx].d; 
              if (my<15) mapL=mmap[my+1][mx].d; 
              break;
      case 4: if (my>0)  mapF=mmap[my-1][mx].d; // when mouse direction is south,
              if (mx>0)  mapR=mmap[my][mx-1].d; 
              if (mx<15) mapL=mmap[my][mx+1].d; 
              break;     
      case 8: if (mx>0)  mapF=mmap[my][mx-1].d; // when mouse direction is west,
              if (my<15) mapR=mmap[my+1][mx].d; 
              if (my>0)  mapL=mmap[my-1][mx].d; 
              break;
    }
  
// decision by left hand rule 
    if (wL ==0 && (mapL==0 || mapL==DtoL[md]))     // left turn when no left wall and no history or available history
      {run_L90(); md=DtoL[md]; }               
    else if (wF==0 && (mapF==0 || mapF==md)){}     // go forward when no front wall and no history or available history (pass the turn process)
    else if (wR==0 && (mapR==0 || mapR==DtoR[md]))   // right turn when no left wall and no history or available history
      {run_R90(); md=DtoR[md]; }                   
    else {run_R180(); md=DtoR[md]; md=DtoR[md];}    //u-turn  

// go forward and detect walls 
    wS=0; wF=0; wR=0; wL=0;                          // reset of wall flags 
    slow_start();                                     // slow start
    stepR=STEP1;
    fS=1;                                            // change high speed 
    while (cntR<stepR){                            // go forward
      if (cntR > (STEP1*2/3) && wS==0){            // wall detection when the mouse run 2/3 step of area 
        wS=1;                                      // set flag to detect wall at once.         
        if (sensR > DISRMAX) wR=1; else wR=0;       // detection of right wall 
        if (sensL > DISLMAX) wL=1; else wL=0;       // detection of left wall 
        if ((sensFR>DISFMAX || sensFL>DISFMAX)){ wF=1; break; }  // detection of front wall. exit from loop when front wall detected.   
      }
    } 
    
      // go forwrd to adjust distanse of front wall, when exit the above loop by front wall detection. 
      if (wF==1){                                     
      while (sensFR<DISFR);                // go forward to have normal distance to front wall 
      adjust();                              // adjustment by front wall 
    } 

// write map and history ( opposit direction of out of the area ) 
// record map after update mouse axis 
    switch (md){  
      case 1: mmap[my][mx].d=4; my++; mmap[my][mx].n=wF; mmap[my][mx].e=wR; mmap[my][mx].w=wL; break; 
      case 2: mmap[my][mx].d=8; mx++; mmap[my][mx].e=wF; mmap[my][mx].s=wR; mmap[my][mx].n=wL; break;
      case 4: mmap[my][mx].d=1; my--; mmap[my][mx].s=wF; mmap[my][mx].w=wR; mmap[my][mx].e=wL; break;
      case 8: mmap[my][mx].d=2; mx--; mmap[my][mx].w=wF; mmap[my][mx].n=wR; mmap[my][mx].s=wL; break;
    } 
    if (mx==0 && my==0) { run_break(); break; }      // finish search run when mouse return start position  
  }
}

//-------------------------------------------------------------------------- 
// fast run, find minimum route to goal and run fast  
//-------------------------------------------------------------------------- 
void run_saitan(){
  unsigned char i,j,k,m;                  
  unsigned char smap[16][16];            // map for calculate minimum route   
  unsigned char run[256];                 // array for run pattern  
  unsigned char md;                       // direction of mouse 1:north, 2:east, 4:south, 8:west 
    
// clear map and set walls for no histry area. 
  for(i=0;i<16;i++){
    for(j=0;j<16;j++){         
      smap[i][j]=0;
      if (mmap[i][j].d==0){ 
        mmap[i][j].n=1; if (i<15) mmap[i+1][j].s=1;
        mmap[i][j].e=1; if (j<15) mmap[i][j+1].w=1;
        mmap[i][j].s=1; if (i>0)  mmap[i-1][j].n=1;
        mmap[i][j].w=1; if (j>0)  mmap[i][j-1].e=1;
      }
    }
  }

// write steps to smap from goal position 
// goal position set to m=1, find same value of m in smap and put m+1 to no wall direction, increment of m,
// go out roop when reach to stat position. 
  smap[7][7]=1; smap[7][8]=1; smap[8][7]=1; smap[8][8]=1; // goal position set to 1 
  m=1;                                                     // set m=1  
  for(k=0;k<255;k++){                                     // repeat maximun 255 times 
    for(i=0;i<16;i++){
      for(j=0;j<16;j++){                                   // scan all areas  
        if (smap[i][j]==m){                                 
          if (mmap[i][j].n==0 && i<15 && smap[i+1][j]==0) smap[i+1][j]=m+1;
          if (mmap[i][j].e==0 && j<15 && smap[i][j+1]==0) smap[i][j+1]=m+1;
          if (mmap[i][j].s==0 && i>0  && smap[i-1][j]==0) smap[i-1][j]=m+1;
          if (mmap[i][j].w==0 && j>0  && smap[i][j-1]==0) smap[i][j-1]=m+1;
        }
      }
    }
    m++;                                        // increment of m 
    if (smap[0][0]!=0) break;                  // go out of loop 
  }
        
// make run pattern to run[k]  array
// k:number of run pattern, 1:go forward, 2:turn right, 3:turn left 
  m=smap[0][0]-1;                             // set m to start position 
  i=0; j=0; k=0;
  md=1;
  while (m>0){                                   // loop while reach to goal position
    switch(md){
      case 1: if (mmap[i][j].n==0 && smap[i+1][j]==m && i<15) {run[k]=1; i++; m--;    break;}
              if (mmap[i][j].e==0 && smap[i][j+1]==m && j<15) {run[k]=2; md=DtoR[md]; break;}
              if (mmap[i][j].w==0 && smap[i][j-1]==m && j>0 ) {run[k]=3; md=DtoL[md]; break;}
      case 2: if (mmap[i][j].e==0 && smap[i][j+1]==m && j<15) {run[k]=1; j++; m--;    break;}
              if (mmap[i][j].s==0 && smap[i-1][j]==m && i>0 ) {run[k]=2; md=DtoR[md]; break;}
              if (mmap[i][j].n==0 && smap[i+1][j]==m && i<15) {run[k]=3; md=DtoL[md]; break;}
      case 4: if (mmap[i][j].s==0 && smap[i-1][j]==m && i>0 ) {run[k]=1; i--; m--;    break;}
              if (mmap[i][j].w==0 && smap[i][j-1]==m && j>0 ) {run[k]=2; md=DtoR[md]; break;}
              if (mmap[i][j].e==0 && smap[i][j+1]==m && j<15) {run[k]=3; md=DtoL[md]; break;}
      case 8: if (mmap[i][j].w==0 && smap[i][j-1]==m && j>0 ) {run[k]=1; j--; m--;    break;}
              if (mmap[i][j].n==0 && smap[i+1][j]==m && i<15) {run[k]=2; md=DtoR[md]; break;}
              if (mmap[i][j].s==0 && smap[i-1][j]==m && i>0 ) {run[k]=3; md=DtoL[md]; break;}
    }
    k++;
  }
    
// run minimun route
  i=0;
  while (i<k){
    if (run[i]==1) { run_step(); i++; }
    if (run[i]==2) { run_R90();  i++; }
    if (run[i]==3) { run_L90();  i++; }
  }
}

//--------------------------------------------------------------------------
//    main                                                      
//--------------------------------------------------------------------------
int main(){
    int i,j;

    // initialize map
    for (i=0; i<16;i++) for (j=0;j<16;j++) mmap[i][j].all=0; // clear map
    for (i=0; i<16;i++){
        mmap[i][0].w=1; mmap[i][15].e=1;                        // set east and west wall 
        mmap[0][i].s=1; mmap[15][i].n=1; }                          // set north and south wall 
    mmap[0][0].e=1;    
 
    timer.attach(&SensAndMotor, 0.001);   // set timer to 1ms for timer interrupt 

    while (1) {

        // initialize parameters
        ledFout=ledRLout=0;
        motorR=motorL=0;
        
        while (startSw==1) {
            if (setSw==0) {               
                wait(0.01); 
                while (setSw==0);
                wait(0.01); 
                pmode++;
                if (pmode>7) pmode=0;
            }
            leds=pmode;
        }
        leds=0;
        wait(0.5);                     
        
        // go selected functions
        switch(pmode){
            case  0: check_sens();   break;     // check sensors  
            case  1: run_step(); break;         // run 1 area step 
            case  2: run_R90();  break;          // 90 deg. turn right 
            case  3: run_L90();  break;         // 90 deg. turn left     
            case  4: run_R180(); break;          // u-turn 
            case  5: run_Turn(0);    break;      // go forward and u-turn
            case  6: run_Hidarite(); break;        // left hand rule
            case  7: run_saitan();   break;      // fast run for minimum route 
        }
    }
}