Diff: main.cpp

Revision:

0:da22b0b4395a

Child:

1:200aad416161

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 19 10:00:41 2013 +0000
@@ -0,0 +1,281 @@
+//**************************************************************************
+//                                                                                  
+//    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 sensor value using serial port                          
+//-------------------------------------------------------------------------- 
+void check_sens(){  
+  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);
+  }
+}
+
+//-------------------------------------------------------------------------- 
+// 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(); break;          // check sensors  
+            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; 
+        }
+    }
+}
\ No newline at end of file