![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
working version with calibration done
Fork of Eurobot2013 by
Diff: IR_Turret/IR_Turret.cpp
- Revision:
- 11:5ba926692210
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IR_Turret/IR_Turret.cpp Tue Apr 09 15:32:47 2013 +0000 @@ -0,0 +1,111 @@ +#include "IR_Turret.h" +#include "mbed.h" +#include "globals.h" +#include "system.h" +#include "geometryfuncs.h" + + +IR_TURRET::IR_TURRET(PinName step_pin, + PinName ir_pin) + : _step(step_pin), + _ir(ir_pin) +{ + + //setup some timer counters using p30 for the hardware counter + //enable PCTIM2 + LPC_SC->PCONP|=(1<<22); + + //SET P30 + LPC_PINCON->PINSEL0|=((1<<8)|(1<<9)); + + //configure counter + LPC_TIM2->TCR =0x2;//counter disable + LPC_TIM2->CTCR =0x1;//counter mode,increments on rising edges + LPC_TIM2->PR =0x0;//set prescaler + LPC_TIM2->MR0 = STEPPER_DIV; // Match count for 3200 counts + LPC_TIM2->MCR = 2; // Reset on Match + LPC_TIM2->TCR =0x1; // counter enable + + // initiates the stepper motor + _step.period(STEPPER_PERIOD); + _step.pulsewidth(STEPPER_PERIOD/2.0f); // 50% duty cycle always + + //init callabck function + callbackfunc = NULL; + callbackobj = NULL; + mcallbackfunc = NULL; + + //init some variables + IR_counter = 0; + IR_step_counter = 0; + pulsewidth_max = 0; + step_count_old = 0; + + _ir.callbackobj = (DummyCT*)this; + _ir.mcallbackfunc = (void (DummyCT::*)(float pulsewidth)) &IR_TURRET::_ir_isr; + + +} + +void IR_TURRET::_ir_isr(float pulsewidth) +{ + static int step_count_local = 0; + + step_count_local = LPC_TIM2->TC; // save current counter value to a local + // resets timeout if pulse is captured + _ir_timeout.detach(); + + // 360 degrees overflow protection + if ( step_count_local < step_count_old ) { + step_count_local += STEPPER_DIV; + } + + //IR_timeout_sema.wait(); + step_count_old = step_count_local; + // finds the maximum pulsewidth + if ( pulsewidth > pulsewidth_max) { + pulsewidth_max = pulsewidth; + } + IR_counter ++; + IR_step_counter += step_count_local; + + _ir_timeout.attach(this, &IR_TURRET::_ir_timeout_isr, IR_TIMEOUT); +} + +void IR_TURRET::_ir_timeout_isr() +{ + + static int IR_index = -1; + + // detach the isr + _ir.mcallbackfunc = NULL; + + + if ((pulsewidth_max <= IR0_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR0_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) { + IR_index = 0; + } else if ((pulsewidth_max <= IR1_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR1_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) { + IR_index = 1; + } else if ((pulsewidth_max <= IR2_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR2_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) { + IR_index = 2; + } + + if (IR_index != -1) { + _angle[IR_index] = rectifyAng(((float)IR_step_counter/IR_counter)*STEP_ANGLE); + _variance[IR_index] = (STEPS_PER_PULSEPERIOD*IR_counter*STEP_ANGLE); + + if (callbackfunc) + (*callbackfunc)(IR_index, _angle[IR_index],_variance[IR_index]); + + if (callbackobj && mcallbackfunc) + (callbackobj->*mcallbackfunc)(IR_index, _angle[IR_index], _variance[IR_index]); + } + + IR_counter = 0; + IR_step_counter = 0; + pulsewidth_max = 0; + step_count_old = 0; + IR_index = -1; + + // reattach the callback + _ir.mcallbackfunc = (void (DummyCT::*)(float pulsewidth)) &IR_TURRET::_ir_isr; +}