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