Shuto Naruse
Eurobot2012_IRturret
Diff: main.cpp
- Revision:
- 0:b17919247a31
--- /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
+*/
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+