Diff: main.cpp

Revision:

0:8626b3f15c02

Child:

1:3f0aea9f0622

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jan 30 12:39:11 2016 +0000
@@ -0,0 +1,380 @@
+//**********************************
+//
+// mbed Robotracer using RTOS
+//
+// Rion Yamada(National Institute of Technology)
+//
+//**********************************
+#include "mbed.h"
+#include "rtos.h"
+
+Serial pc(USBTX, USBRX);               // serial port
+
+// run parameters
+#define STH    0.2                    // threshold value for digital photo sensor
+#define SGTH   0.2                    // threshold value for start/goal marker
+#define CTH    0.1                    // 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 sens1, sens2, sens3, sens4, sens5, sens6, sensSG, sensC;       // sensor values
+volatile int   sensD,sensDout=0;
+
+unsigned char frun, fmarker;
+volatile int markerSG, markerC, markerX;
+
+int cntGoal=0;    // couter for run after goal
+
+
+volatile unsigned char modeR=0, modeL=0;
+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 int waitR,waitL;                             //count of thread wait
+
+volatile int bp=0;                          // couter for beep
+volatile int f;
+volatile int r,l;
+
+
+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
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+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
+
+//--------------------------------------------------------------------------
+// RTOS Timer
+//--------------------------------------------------------------------------
+void Sensor(void const *argument)
+{
+
+    ledSGout=1;                          // LED-ON
+    sens1=pt12;                          // measure all sensor values
+    sens3=pt34;
+    sens5=pt56;
+    sensSG=ptSG;
+    ledSGout=0;                          // LED-OFF
+
+    ledCout=1;                           // LED-ON
+    sens2=pt12;                          // measure all sensor values
+    sens4=pt34;
+    sens6=pt56;
+    sensC=ptC;
+    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];
+
+    if (sensSG>STH) markerSG++;
+    else if (markerSG>0) markerSG--;
+    if (sensC>CTH ) 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) {
+        bp--;
+        if (buzzer==1) buzzer=0;
+        else buzzer=1;    // alternate ON-OFF
+    }
+}
+
+//--------------------------------------------------------------------------
+// Motor speed control
+//--------------------------------------------------------------------------
+int Motor_Control()
+{
+
+    switch(sensDout) {
+        case  -5:
+            r=3;
+            l=20;
+            break;
+        case  -4:
+            r=3;
+            l=15;
+            break;
+        case  -3:
+            r=3;
+            l=15;
+            break;
+        case  -2:
+            r=5;
+            l=10;
+            break;
+        case  -1:
+            r=5;
+            l=10;
+            break;
+        case  0:
+            r=5;
+            l=5;
+            break;
+        case  1:
+            r=10;
+            l=5;
+            break;
+        case  2:
+            r=10;
+            l=5;
+            break;
+        case  3:
+            r=15;
+            l=3;
+            break;
+        case  4:
+            r=15;
+            l=3;
+            break;
+        case  5:
+            r=20;
+            l=3;
+            break;
+    }
+    if(f==1) return(r);
+    else return(l);
+}
+
+//--------------------------------------------------------------------------
+// thread of right motor rotation
+// motor rotation,  mode = 0: free，1: forward，2: reverse，3: break
+//--------------------------------------------------------------------------
+void motorR_thread(void const *argsument)
+{
+
+    Thread::signal_wait(0x1);
+
+    while (true) {
+        f=1;
+        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
+        if (modeR==0) patR=4;   // motor free when mode=0
+        motorR= RMOTOR[patR];
+        waitR=Motor_Control();
+        Thread::wait(waitR);
+    }
+}
+
+//--------------------------------------------------------------------------
+// thread of right motor rotation
+// motor rotation,  mode = 0: free，1: forward，2: reverse，3: break
+//--------------------------------------------------------------------------
+void motorL_thread(void const *argument)
+{
+
+    Thread::signal_wait(0x1);
+
+    while (true) {
+        f=0;          //runの戻り値が左モータ専用
+        if(modeL==1) {
+            if (patL < 3) patL++;
+            else patL = 0;
+        }
+        if(modeL==2) {
+            if (patL > 0) patL--;
+            else patL = 3;
+        }
+        if(modeL==3) patL=3;
+        cntL++;                                  // count up left moter step
+        if (modeL==0) patL=4;   // motor free when mode=0
+        motorL= LMOTOR[patL];
+        waitL=Motor_Control();
+        Thread::wait(waitL);
+    }
+}
+
+// -----------------------------------------------
+// beep
+// -----------------------------------------------
+void beep(int n)
+{
+    bp=n;     // set beep couter
+}
+
+//--------------------------------------------------------------------------
+// check sensor value using serial port
+//--------------------------------------------------------------------------
+void check_sens()
+{
+    while (1) {
+        pc.printf("\f");
+        pc.printf("Sensor C:%f \n",sensC);
+        pc.printf("Sensor SG:%f \n",sensSG);
+        pc.printf("Sensor  1:%f \n",sens1);
+        pc.printf("Sensor  2:%f \n",sens2);
+        pc.printf("Sensor  3:%f \n",sens3);
+        pc.printf("Sensor  4:%f \n",sens4);
+        pc.printf("Sensor  5:%f \n",sens5);
+        pc.printf("Sensor  6:%f \n",sens6);
+        wait (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
+// nstep: number of step
+//--------------------------------------------------------------------------
+void runTurn(int mR,int mL , int nstep )
+{
+    modeR=mR;
+    modeL=mL;
+    cntR=0;
+    stepR=nstep;
+    while (cntR<stepR);
+    run_break();
+}
+
+//-----------------------
+// run
+//----------------------
+void run()
+{
+
+    markerSG=0;
+    markerC=0;
+    markerX=0;
+    fmarker=0;
+    frun=0;
+    modeR=modeL=1;
+
+    while(startSw==1 ) {
+
+        // 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(100);
+        }
+
+        // start/goal marker check
+        if (frun==0 && sensSG>SGTH) {
+            frun=1;    // start marker detect
+            beep(100);
+        }
+        if (frun==1 && markerSG==0) frun=2;                 // start marker fix
+        if (frun==2 && sensSG>SGTH) frun=3;                 // goal marker detect
+        if (frun==3 && markerX==1) {
+            frun=2;    // ignor cross line
+            led3=1;
+        }
+        if (frun==3 && markerSG==0) {                       // goal marker fix
+            frun=4;
+            beep(50);
+            break;
+        }
+    }
+    run_break();
+}
+
+int main()
+{
+
+    Thread thread1(motorR_thread , NULL ,  osPriorityHigh);       // thread of right motor control
+    Thread thread2(motorL_thread , NULL ,  osPriorityHigh);       // thread of left motor control
+
+    RtosTimer sensor_timer(Sensor, osTimerPeriodic, (void *)0);   // set RTOS timer for sensor
+    sensor_timer.start(5);                                        // start RTOS timer for sensor
+
+    while (1) {
+        while (startSw==1) {            // program mode selection
+            if (setSw==0) {
+                wait(0.1);
+                beep(50);
+                while (setSw==0);
+                wait(0.1);
+                pmode++;
+                if (pmode>3) pmode=0;
+            }
+            leds=pmode;
+        }
+        leds=0;
+        beep(50);
+        wait(0.5);
+
+        // run selected functions
+        switch(pmode) {
+            case  0:
+                check_sens();
+                break;     // check sensors
+            case  1:
+                thread1.signal_set(0x1);   // run forward
+                thread2.signal_set(0x1);
+                runTurn(1,1,500);
+                break;
+            case  2:
+                thread1.signal_set(0x1);   // turn right
+                thread2.signal_set(0x1);
+                runTurn(2,1,500);
+                break;
+            case  3:
+                thread1.signal_set(0x1);   // trace run
+                thread2.signal_set(0x1);
+                run();
+                break;
+        }
+    }
+}
+
+