File content as of revision 0:b17919247a31:

/*************************************************************
     Eurobot 2012
     IR Localization Beacon Turret Control Code

     author: Shuto
*************************************************************/

/*
NOTES

IR identification is based on beacon period => may need to change this
+ ON time is coded
+ OFF time used to confirm read,

ticker based stepper control could be unstable

may need to slow down stepper, microstep less



iterative average predictions


*/


#include "mbed.h"

// Pin Definitions ============================================================

// IR sensor input
InterruptIn    irt_pin_sensor     (p30);        // pull up to supply

// stepper control pins
DigitalOut    irt_pin_step    (p11);
DigitalOut    irt_pin_dir     (p12);
DigitalOut    StepperDisable  (p23);// Connects to Stepper controller !EN pin


// data output
Serial         mainMbed (p13,p14);

// start interrupt
DigitalIn      startTrig (p21);

// start location signal
//DigitalIn      startOnRed (p22);

// debug
Serial         pc        (USBTX, USBRX); // tx, rx
DigitalOut     rx0     (LED1);
DigitalOut     rx1     (LED2);
DigitalOut     rx2     (LED3);
DigitalOut     serialTx (LED4);


// IR Beacon Settings =========================================================

// IR signal coding: ON time in us
// TODO need to adjust
#define    IR0_ON        1000
#define    IR1_ON        750
#define    IR2_ON        500

#define    IR0_OFF       500
#define    IR1_OFF       750
#define    IR2_OFF       1000

#define    IR_TOR       100         // torrerence

#define    IR_BUF_SIZE  100         // buffer size for sensor readings 
#define    IR_INVALID   100000      // invalid angle 

#define    IR_ANGLE_TOR 10

#define    MIN_SC         5         // minimum sample count for valid recieve
#define    NO_SIG_WINDOW  32        // need to adjust!  (NO_SIG_WINDOW * 1.8/16)

// Stepper Motor Settings =====================================================

#define STEPPER_DIV   3200    // number of steps per cycle = default * microstep
#define STEP_PULSE    0.00005    // step pulse duration


// Variables ==================================================================

// stepper
Ticker    irt_stepTicker;            // ticker triggering stepper motor step

float    irt_STEPANGLE    = (float)360 / STEPPER_DIV;        // step angle of stepper

float    irt_rps         = 5;
float    irt_angle        = 0;    // current direction which the stepper is facing

bool    irt_spin_dir = true;

// IR sensor
Timer    irt_timer_on, irt_timer_off;        // timer used to measure signal duration

bool        irt_rx0_ON = false;
bool        irt_rx1_ON = false;
bool        irt_rx2_ON = false;

//data buffer
int   data0[IR_BUF_SIZE];
int   data1[IR_BUF_SIZE];
int   data2[IR_BUF_SIZE];
int     sc0 = 0;
int     sc1 = 0;
int     sc2 = 0;
int     acc0 = 0;
int     acc1 = 0;
int     acc2 = 0;

int     nosig0 = 0;
int     nosig1 = 0;
int     nosig2 = 0;

// game end
Timeout gameEnd;
bool    inGame = false;

// functions ==================================================================

// prototypes
void irt_stepperStep ();
void irt_sensorSetup ();
void irt_sigStart ();
void irt_sigEnd ();
void sendSerial (int ID, float center, float var);
void stopRobot ();
float step2rad (int step);
float radFormat (float in);

//--- debug main --------------------------------
int main () {
    mainMbed.baud(115200);
    mainMbed.format(8,Serial::Odd,1);

    StepperDisable = 0; //Enable stepper

    irt_sensorSetup();
    
    /*
    // change spin direction for different start locations
    if (startOnRed){
        irt_spin_dir = true;
    }else{
        irt_spin_dir = false;
    }
    */
    irt_pin_dir = irt_spin_dir;
    

    // reset buffer
    sc0 = 0;
    sc1 = 0;
    sc2 = 0;
    for (int i = 0; i < IR_BUF_SIZE; i++) {
        data0[i] = IR_INVALID;       // fill with invalid angle values
        data1[i] = IR_INVALID;
        data2[i] = IR_INVALID;
    }
    
    // calibration 5 sec
    for (int cv = 0; cv < (STEPPER_DIV * irt_rps)*5 ; cv++) {
        irt_stepperStep();
        wait(1/(STEPPER_DIV * irt_rps));
        rx0 = false;
        rx1 = false;
        rx2 = false;
    }
    
    // wait for start signal
    while(!startTrig){}
    
    // start off 90sec kill timer
    gameEnd.attach(&stopRobot, 87.0);
    inGame = true;
    
    // spin loop
    while (inGame) {
        irt_stepperStep();
        wait(1/(STEPPER_DIV * irt_rps));
        rx0 = false;
        rx1 = false;
        rx2 = false;
    }

}


//--- Stepper Control Functions -----------------
void irt_stepperStep () {
// function to handle stepper stepping
// this function is repeatedly called by the ticker

    // generate step pulse
    irt_pin_step = true;
    wait(STEP_PULSE);
    irt_pin_step = false;

    // increment
    irt_angle ++;
    if (irt_angle >= STEPPER_DIV) {
        irt_angle = 0;
    }

    // check for signal end
    if (sc0) {
        nosig0++;

        // if signal not recieved for long enough
        if (nosig0 > NO_SIG_WINDOW) {

            // if enough samples obtained, valid signal
            if (sc0 > MIN_SC) {
                float range = step2rad(data0[sc0-1] - data0[0]);

                // if robot facing a beacon signal will be recieved accross 0
                if (range < 0) {
                    acc0 = 0;
                    for (int i = 0; i < sc0; i++) {
                        if (data0[i] > STEPPER_DIV/2) {
                            acc0 += data0[i] - STEPPER_DIV;
                        } else {
                            acc0 += data0[i];
                        }
                    }
                }

                // send data packet
                sendSerial (0, step2rad((float)acc0/sc0), range*range);
            }
            // if not, probably noise

            // reset
            nosig0 = 0;
            sc0 = 0;
            acc0 = 0;
        }
    }

    if (sc1) {
        nosig1++;

        // if signal not recieved for long enough
        if (nosig1 > NO_SIG_WINDOW) {

            // if enough samples obtained, valid signal
            if (sc1 > MIN_SC) {
                float range = step2rad(data1[sc1-1] - data1[0]);

                // if robot facing a beacon signal will be recieved accross 0
                if (range < 0) {
                    acc1 = 0;
                    for (int i = 0; i < sc1; i++) {
                        if (data1[i] > STEPPER_DIV/2) {
                            acc1 += data1[i] - STEPPER_DIV;
                        } else {
                            acc1 += data1[i];
                        }
                    }
                }

                // send data packet
                sendSerial (1, step2rad((float)acc1/sc1), range*range);
            }
            // if not, probably noise

            // reset
            nosig1 = 0;
            sc1 = 0;
            acc1 = 0;
        }
    }

    if (sc2) {
        nosig2++;

        // if signal not recieved for long enough
        if (nosig2 > NO_SIG_WINDOW) {

            // if enough samples obtained, valid signal
            if (sc2 > MIN_SC) {
                float range = step2rad(data2[sc2-1] - data2[0]);

                // if robot facing a beacon signal will be recieved accross 0
                if (range < 0) {
                    acc2 = 0;
                    for (int i = 0; i < sc2; i++) {
                        if (data2[i] > STEPPER_DIV/2) {
                            acc2 += data2[i] - STEPPER_DIV;
                        } else {
                            acc2 += data2[i];
                        }
                    }
                }

                // send data packet
                sendSerial (2, step2rad((float)acc2/sc2), range*range);
            }
            // if not, probably noise

            // reset
            nosig2 = 0;
            sc2 = 0;
            acc2 = 0;
        }
    }


    return;
}


//--- IR Signal Decode Functions ----------------
void irt_sensorSetup () {
// function to setup sensor ISRs

    // attach ISRs to sensor input
    irt_pin_sensor.fall(&irt_sigStart);        // ISR for sensor active (low value)
    irt_pin_sensor.rise(&irt_sigEnd);        // ISR for sensor inactive (high)

    // reset timer
    irt_timer_on.reset();
    irt_timer_off.reset();

    

    return;
}


void irt_sigStart () {
// high to low transition

    // start ON timer
    irt_timer_on.reset();
    irt_timer_on.start();

    // stop OFF timer
    irt_timer_off.stop();

    // read timer
    int off_time = irt_timer_off.read_us();

    // check if signal recieved

    if (irt_angle != 0) {
        if (abs(off_time - IR0_OFF) < IR_TOR) {
            rx0 = irt_rx0_ON;
            if (irt_rx0_ON) {
                data0[sc0] = irt_angle;
                acc0 += irt_angle;
                sc0++;
                nosig0 = 0;
                if (sc0 >= IR_BUF_SIZE) {
                    //pc.printf("buffer overflow \n");
                    acc0 = 0;
                    sc0 = 0;
                }
            }
        }

        if (abs(off_time - IR1_OFF) < IR_TOR) {
            rx1 = irt_rx1_ON;
            if (irt_rx1_ON) {
                data1[sc1] = irt_angle;
                acc1 += irt_angle;
                sc1++;
                nosig1 = 0;
                if (sc1 >= IR_BUF_SIZE) {
                    //pc.printf("buffer overflow \n");
                    acc1 = 0;
                    sc1 = 0;
                }
            }
        }

        if (abs(off_time - IR2_OFF) < IR_TOR) {
            rx2 = irt_rx2_ON;
            if (irt_rx2_ON) {
                data2[sc2] = irt_angle;
                acc2 += irt_angle;
                sc2++;
                nosig2 = 0;
                if (sc2 >= IR_BUF_SIZE) {
                    //pc.printf("buffer overflow \n");
                    acc2 = 0;
                    sc2 = 0;
                }
            }
        }
    }



    return;
}

void irt_sigEnd () {
// low to high transition

    // start OFF timer
    irt_timer_off.reset();
    irt_timer_off.start();

    // stop ON timer
    irt_timer_on.stop();

    // read timer
    int on_time = irt_timer_on.read_us();

    // check
    if (abs(on_time - IR0_ON) < IR_TOR) {
        irt_rx0_ON = true;
    } else {
        irt_rx0_ON = false;
    }

    if (abs(on_time - IR1_ON) < IR_TOR) {
        irt_rx1_ON = true;
    } else {
        irt_rx1_ON = false;
    }

    if (abs(on_time - IR2_ON) < IR_TOR) {
        irt_rx2_ON = true;
    } else {
        irt_rx2_ON = false;
    }

    return;
}


// send serial ----------------------------------------------------------------
void sendSerial (int ID, float center, float var) {

    // bytes packing for IR turret serial comm
    union IRValue_t {
        int     IR_ints[4];
        float   IR_floats[4];
        unsigned char IR_chars[16];
    } IRValues;

    IRValues.IR_chars[0] = 0xFF;
    IRValues.IR_chars[1] = 0xFE;
    IRValues.IR_chars[2] = 0xFD;
    IRValues.IR_chars[3] = 0xFC;

    IRValues.IR_ints[1]     = ID;       // beacon ID
    IRValues.IR_floats[2]   = center;   // center
    IRValues.IR_floats[3]   = var;      // (max - min)^2

    // output sample to main board
    for (int i = 0; i < 16; i++) {
        mainMbed.putc(IRValues.IR_chars[i]);
    }
    
    // debug 
    //pc.printf("%d; %f; %f; \n",ID,center,var);
}

//--- end game ----------------------------------------------------------------
void stopRobot (){
    inGame = false;
    StepperDisable = 1; //Disable Stepper Disable pin set High
}


//--- util --------------------------------------------------------------------
float step2rad (int step) {
// convert moter step count to rad format with correct range (defined in radFormat)
    return radFormat(step * irt_STEPANGLE * 3.14 / 180);
}

float radFormat (float in) {
    return in;
// format angle range => currently -PI to PI
/*
    if (in > 3.14)  {
        return in - 6.28;
    }
    if (in < -3.14) {
        return in + 6.28;
    }
    return in;
 */
}






// notes about the stepper controller =========================================
/*
*** timing info **********************************************
    STEP input pulse   min widths               1us
    setup and hold for MS inputs and RESET/DIR  0.2us

    FULL Step config        !MS1 !MS2 !MS3
    STEP number (for full step config) ==> 200

*** stepper lead colors **************************************
    -> steps clockwise for DIR true
        1A          BLUE
        1B          RED
        2A          BLACK
        2B          GREEN

*** stepper driver pin connection ****************************
        GND         ground
        5V          float
        Vdd         jump to 3.3v
        3V3         (jump to Vdd)
        GND         ground
        REF         floating
        !EN         GND
        MS1/2/3     floating (have pull down resistors)
        !RST        pull up to 3.3v (10k resistor)
        !SLP        connect to !RST (pull up to 3.3v)
        STEP        STEP data pin
        DIR         DIR data pin

        VMOT        connect to 12v supply
        GND         ground
        2B          motor lead - GREEN
        2A          motor lead - BLACK
        1A          motor lead - BLUE
        1B          motor lead - RED            (clock wise direction when DIR true)
        Vdd         float
        GND         ground

*** current limit setting ************************************
    coil resistance = 3.3ohm

    check voltage output to motor  ==> set it to ~1v
        this will limit current to 0.333A
        (total current 0.66A, since two phase)

    higher currents needed for higher rotation speeds
        2v for 5 rps
*/