File content as of revision 0:8626b3f15c02:

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