IR_Turret.cpp

00001 #include "IR_Turret.h"
00002 #include "mbed.h"
00003 #include "globals.h"
00004 #include "system.h"
00005 #include "geometryfuncs.h"
00006 
00007 
00008 IR_TURRET::IR_TURRET(PinName step_pin,
00009                      PinName ir_pin)
00010     : _step(step_pin),
00011       _ir(ir_pin)
00012 {
00013 
00014     //setup some timer counters using p30 for the hardware counter
00015     //enable PCTIM2
00016     LPC_SC->PCONP|=(1<<22);
00017 
00018     //SET P30
00019     LPC_PINCON->PINSEL0|=((1<<8)|(1<<9));
00020 
00021     //configure counter
00022     LPC_TIM2->TCR   =0x2;//counter disable
00023     LPC_TIM2->CTCR  =0x1;//counter mode,increments on rising edges
00024     LPC_TIM2->PR    =0x0;//set prescaler
00025     LPC_TIM2->MR0   = STEPPER_DIV;        // Match count for 3200 counts
00026     LPC_TIM2->MCR   = 2;           // Reset on Match
00027     LPC_TIM2->TCR   =0x1;           // counter enable
00028 
00029     // initiates the stepper motor
00030     _step.period(STEPPER_PERIOD);
00031     _step.pulsewidth(STEPPER_PERIOD/2.0f); // 50% duty cycle always
00032 
00033     //init callabck function
00034     callbackfunc = NULL;
00035     callbackobj = NULL;
00036     mcallbackfunc = NULL;
00037 
00038     //init some variables
00039     IR_counter = 0;
00040     IR_step_counter = 0;
00041     pulsewidth_max = 0;
00042     step_count_old = 0;
00043 
00044     _ir.callbackobj = (DummyCT*)this;
00045     _ir.mcallbackfunc = (void (DummyCT::*)(float pulsewidth)) &IR_TURRET::_ir_isr;
00046 
00047 
00048 }
00049 
00050 void IR_TURRET::_ir_isr(float pulsewidth)
00051 {
00052     static int step_count_local = 0;
00053 
00054     step_count_local = LPC_TIM2->TC;  // save current counter value to a local
00055     // resets timeout if pulse is captured
00056     _ir_timeout.detach();
00057 
00058     // 360 degrees overflow protection
00059     if ( step_count_local < step_count_old ) {
00060         step_count_local += STEPPER_DIV;
00061     }
00062 
00063     //IR_timeout_sema.wait();
00064     step_count_old = step_count_local;
00065     // finds the maximum pulsewidth
00066     if ( pulsewidth > pulsewidth_max) {
00067         pulsewidth_max = pulsewidth;
00068     }
00069     IR_counter ++;
00070     IR_step_counter += step_count_local;
00071 
00072     _ir_timeout.attach(this, &IR_TURRET::_ir_timeout_isr, IR_TIMEOUT);
00073 }
00074 
00075 void IR_TURRET::_ir_timeout_isr()
00076 {
00077 
00078     static int IR_index = -1;
00079 
00080     // detach the isr
00081     _ir.mcallbackfunc = NULL;
00082     
00083     
00084     if ((pulsewidth_max <= IR0_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR0_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
00085         IR_index = 0;
00086     } else if ((pulsewidth_max <= IR1_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR1_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
00087         IR_index = 1;
00088     } else if ((pulsewidth_max <= IR2_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR2_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
00089         IR_index = 2;
00090     }
00091 
00092     if (IR_index != -1) {
00093         _angle[IR_index] = rectifyAng(((float)IR_step_counter/IR_counter)*STEP_ANGLE);
00094         _variance[IR_index] = (STEPS_PER_PULSEPERIOD*IR_counter*STEP_ANGLE);
00095 
00096         if (callbackfunc)
00097             (*callbackfunc)(IR_index, _angle[IR_index],_variance[IR_index]);
00098 
00099         if (callbackobj && mcallbackfunc)
00100             (callbackobj->*mcallbackfunc)(IR_index, _angle[IR_index], _variance[IR_index]);
00101     }
00102 
00103     IR_counter = 0;
00104     IR_step_counter = 0;
00105     pulsewidth_max = 0;
00106     step_count_old = 0;
00107     IR_index = -1;
00108 
00109     // reattach the callback
00110     _ir.mcallbackfunc = (void (DummyCT::*)(float pulsewidth)) &IR_TURRET::_ir_isr;
00111 }