Revision 0:b17919247a31, committed 2012-10-17

Comitter:

narshu

Date:

Wed Oct 17 22:27:33 2012 +0000

Commit message:

Commit before publishing

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 17 22:27:33 2012 +0000
@@ -0,0 +1,566 @@
+/*************************************************************
+     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
+*/
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Oct 17 22:27:33 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/737756e0b479
\ No newline at end of file