File content as of revision 1:200aad416161:

//**************************************************************************
//                                                                                  
//    mbed Robotracer for education     
//    (c) Kiyoteru Hayama(Kumamoto National College of Technology)
//
//**************************************************************************
#include "mbed.h"

Serial pc(USBTX, USBRX); 

// run parameters
#define STH    0.5               // threshold value for digital photo sensor
#define SGTH   10                    // threshold value for start/goal marker
#define CTH    10                    // threshold value for corner marker

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

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};

unsigned char pmode=0;                 //  program mode

volatile float pt1B, pt2B, pt3B, pt4B, pt5B, pt6B, ptSGB, ptCB;          // sensor values during turn-off the LED
volatile float sens1, sens2, sens3, sens4, sens5, sens6, sensSG, sensC;      // sensor values
volatile int   sensD,sensDout=0;

volatile int markerSG, markerC, markerX;
unsigned char frun, fmarker;

volatile int spdR,spdL;
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;              // timer for motors
volatile unsigned char timS=0;                      // timer for sensors 
volatile int bp=0;                          // couter for beep

Ticker timerS;            // defince interval timer 
Ticker timerM;               // defince interval timer 

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 pt12(p19);                        // front right sensor, analog input
AnalogIn pt34(p18);                        // front left sensor, analog input
AnalogIn pt56(p17);                        // right sensor, analog input
AnalogIn ptC(p20);                         // left sensor, analog input
AnalogIn ptSG(p16);                         // left sensor, analog input
AnalogIn gyro(p15);                        // for Gyro, analog input, reserved 

DigitalIn setSw(p21);                       // set-switch, digital input
DigitalIn startSw(p22);                 // start-switch, digital input
DigitalOut ledCout(p9);                 // LED output signal for corner marker 
DigitalOut ledSGout(p10);                // LED output signal for start/goal marker
DigitalOut buzzer(p23);                     // buzzer out

//-------------------------------------------------------------------------- 
// interrupt by us timerS                                             
//-------------------------------------------------------------------------- 
void Sensor() { 
    // read sensors
    // 1st-step:measure background during LED-off, 2nd-step: measure reflecting light during LED-on. sensor value is differnce of both.
    timS = !timS; 
    if (timS==0){ 
        pt1B=pt12;                          // measure all background values 
        pt3B=pt34;     
        pt5B=pt56; 
        ptSGB=ptSG;    
        ledSGout=1;                          // LED-ON
        wait_us(50);                       // delay 
        sens1=abs(pt12-pt1B);                  //"sabunn"
        sens3=abs(pt34-pt3B);
        sens5=abs(pt56-pt5B);
        sensSG=abs(ptSG-ptSGB);
        ledSGout=0;                          // LED-OFF
    } else{ 
        pt2B=pt12;                          // measure all background values 
        pt4B=pt34;     
        pt6B=pt56; 
        ptCB=ptC;    
        ledCout=1;                          // LED-ON
        wait_us(50);                        // delay 
        sens2=abs(pt12-pt2B);
        sens4=abs(pt34-pt4B);
        sens6=abs(pt56-pt6B);
        sensC=abs(ptC-ptCB);
        ledCout=0;                          // LED-OFF                        
    }   

    sensD=0;
    if (sens1>STH ) sensD |= 0x20; else sensD &= ~(0x20);   
    if (sens2>STH ) sensD |= 0x10; else sensD &= ~(0x10);   
    if (sens3>STH ) sensD |= 0x08; else sensD &= ~(0x08);  
    if (sens4>STH ) sensD |= 0x04; else sensD &= ~(0x04);  
    if (sens5>STH ) sensD |= 0x02; else sensD &= ~(0x02);  
    if (sens6>STH ) sensD |= 0x01; else sensD &= ~(0x01);  
    sensDout=sensArray[sensD];        

    // corner and start/goal marker detection 
    if (sensSG>STH) markerSG++; else if (markerSG>0) markerSG--;
    if (sensC>STH ) markerC++;  else if (markerC>0)  markerC--; 
    // cross line detection 
    if (markerSG>1 && markerC>1) markerX=1;           // both marker                
    if (markerX==1 && markerSG==0 && markerC==0) markerX=0; // ignore cross line      
    
    // buzzer 
    if (bp>0){             // high beep
        bp--;
        if (buzzer==1) buzzer=0; else buzzer=1;    // alternate ON-OFF      
    }
}

//-------------------------------------------------------------------------- 
// interrupt by us timerR/L   
// motor rotation,  mode = 0: free，1: forward，2: reverse，3: break
// right motor rotation                                           
//-------------------------------------------------------------------------- 
void stepMotor(){

    if (timR>0) timR--;                        //count down timR，when timR=0 do next process
    if (timR==0) {
        timR=spdR;
        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，when timL=0 do next process
    if (timL==0) {
        timL=spdL;
        //modeL==1;
        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
}

// -----------------------------------------------
// beep
// -----------------------------------------------
void beep(int n){
    bp=n;     // set beep couter
}

//-------------------------------------------------------------------------- 
// check sensors                        
//-------------------------------------------------------------------------- 
void check_sens(){                                 // serial output of sensor level
  while (1){   
    pc.printf("sensC :"); pc.printf("%f\n",sensC); // 
    pc.printf("sensSG :"); pc.printf("%f\n",sensSG); // 
    pc.printf("sens1 :"); pc.printf("%f\n",sens1); // 
    pc.printf("sens2 :"); pc.printf("%f\n",sens2); // 
    pc.printf("sens3 :"); pc.printf("%f\n",sens3); // 
    pc.printf("sens4 :"); pc.printf("%f\n",sens4); // 
    pc.printf("sens5 :"); pc.printf("%f\n",sens5); // 
    pc.printf("sens6 :"); pc.printf("%f\n",sens6); // 
    pc.printf("sensD :"); pc.printf("%d\n",sensD);
    pc.printf("sensDout :"); pc.printf("%d\n",sensDout);
    wait (0.5);
  }
}

void check_sens_level(){                           // leds output of sensor level
  int i=0;
  while (1){   
    if (setSw==0) {               
      wait(0.01); 
      beep(50);
      while (setSw==0);
      wait(0.01); 
      i++; if (i>7) i=0;
    }
    if (i==0){leds=(int)(sens1*10); pc.printf("sens1 x 10="); pc.printf("%d\n",(int)(sens1*10)); }
    if (i==1){leds=(int)(sens2*10); pc.printf("sens2 x 10="); pc.printf("%d\n",(int)(sens2*10)); }
    if (i==2){leds=(int)(sens3*10); pc.printf("sens3 x 10="); pc.printf("%d\n",(int)(sens3*10)); }
    if (i==3){leds=(int)(sens4*10); pc.printf("sens4 x 10="); pc.printf("%d\n",(int)(sens4*10)); }
    if (i==4){leds=(int)(sens5*10); pc.printf("sens5 x 10="); pc.printf("%d\n",(int)(sens5*10)); }
    if (i==5){leds=(int)(sens6*10); pc.printf("sens6 x 10="); pc.printf("%d\n",(int)(sens6*10)); }
    if (i==6){leds=(int)(sensC*10); pc.printf("sensC x 10="); pc.printf("%d\n",(int)(sensC*10)); }
    if (i==7){leds=(int)(sensSG*10); pc.printf("sensSG x 10="); pc.printf("%d\n",(int)(sensSG*10)); }
    wait (0.1);
  }
}
        
void check_pos(){                                   // leds output of line position
  while (1){   
    leds=sensDout;
    wait (0.1);
  }
}
        
//-------------------------------------------------------------------------- 
// break and release motors          
//-------------------------------------------------------------------------- 
void run_release(){
    modeR=0; modeL=0;              // motor release
}
void run_break(){
  modeR=3; modeL=3;                // mode 0 means break the motor  
  wait(0.5);
  run_release();
}

//-------------------------------------------------------------------------- 
// run and turn
// (mR,mL)=(1,1):forward, (2,1): turn right, (1,2): turn left, (2,2): Reverse  
// spd: speed, 
// nstep: number of step
//-------------------------------------------------------------------------- 
void runTurn(int mR,int mL, int spd, int nstep ){ 
    modeR=mR;modeL=mL; 
    spdR=spdL=spd;
    cntR=0; stepR=nstep;             
    while (cntR<stepR);
}

//-----------------------
// run
// n: run speed, m: factor of reduce speed 
//----------------------
 void run(int n, int m){
    int cntGoal=0;    // couter for run after goal

    markerSG=0; markerC=0; markerX=0; fmarker=0; 
    frun=0;
    runTurn(1,1,n*2,50);    // slow start
    
    while(startSw==1 ){

        spdR=spdL=n;
        if (sensDout>0){
            spdR+=sensDout*m;
        } else {
            spdL-=sensDout*m; 
        }  
    
        // corner marker check
        if (markerX==1) fmarker=0;   
        if (markerX==0 && fmarker==0 && markerC>5) fmarker=1;  
        if (markerX==0 && fmarker==1 && markerC==0){           
            fmarker=0; beep(50); 
            
            
                 
        }

        // start/goal marker check
        if (frun==0 && markerSG>SGTH) frun=1;  // start marker detect
        if (frun==1 && markerSG==0){         // start marker fix
            frun=2; beep(100);              
        }
        if (frun==2 && markerSG>SGTH) frun=3;  // goal marker detect
        if (frun==3 && markerX==1)  frun=2;  // ignor cross line  
        if (frun==3 && markerSG==0){         // goal marker fix     
            frun=4; beep(100);
            cntGoal=cntR;
        }   
        if (frun==4 && cntR>(cntGoal+500)) break;       
        wait(0.005);                         // wait 5ms for control loop 
    }
    run_break();
}

//--------------------------------------------------------------------------
//    main                                                      
//--------------------------------------------------------------------------
int main(){

    timerS.attach_us(&Sensor, 2000);    // set timer for sensor
    timerM.attach_us(&stepMotor, 400);   // set timer for motor
    
    while (1) {
    
        // initialize motor
        run_release();
        
        while (startSw==1) {            // program mode selection
            if (setSw==0) {               
                wait(0.01); 
                beep(50);
                while (setSw==0);
                wait(0.01); 
                pmode++;
                if (pmode>7) pmode=0;
            }
            leds=pmode;
        }
        leds=0; beep(50);
        wait(0.5);                     
        
        // go selected program
        switch(pmode){
            case  0: check_sens_level(); break;    // check sensors by leds 
          //case  0: check_sens(); break;          // check sensors by serial output 
          //case  0: check_pos(); break;           // check line position  
            case  1: runTurn(1,1,15,500); break;   // run forward 
            case  2: runTurn(2,1,15,500); break;   // turn right       
            case  3: run(10,30);  break;           // trace run 
            case  4: run(8,24);  break;          
            case  5: run(6,18);  break;    
            case  6: run(5,15);  break; 
            case  7: run(4,12);  break; 
        }
    }
}