Diff: IR_Turret/IR_Turret.cpp

Revision:

11:5ba926692210

diff -r 2bd9f4e02b74 -r 5ba926692210 IR_Turret/IR_Turret.cpp
--- /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;
+}