Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

IR_Turret/IR_Turret.cpp@11:5ba926692210, 2013-04-09 (annotated)

Committer:
xiaxia686
Date:
Tue Apr 09 15:32:47 2013 +0000
Revision:
11:5ba926692210
woking version (calibrated)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xiaxia686 11:5ba926692210 1 #include "IR_Turret.h"
xiaxia686 11:5ba926692210 2 #include "mbed.h"
xiaxia686 11:5ba926692210 3 #include "globals.h"
xiaxia686 11:5ba926692210 4 #include "system.h"
xiaxia686 11:5ba926692210 5 #include "geometryfuncs.h"
xiaxia686 11:5ba926692210 6
xiaxia686 11:5ba926692210 7
xiaxia686 11:5ba926692210 8 IR_TURRET::IR_TURRET(PinName step_pin,
xiaxia686 11:5ba926692210 9 PinName ir_pin)
xiaxia686 11:5ba926692210 10 : _step(step_pin),
xiaxia686 11:5ba926692210 11 _ir(ir_pin)
xiaxia686 11:5ba926692210 12 {
xiaxia686 11:5ba926692210 13
xiaxia686 11:5ba926692210 14 //setup some timer counters using p30 for the hardware counter
xiaxia686 11:5ba926692210 15 //enable PCTIM2
xiaxia686 11:5ba926692210 16 LPC_SC->PCONP|=(1<<22);
xiaxia686 11:5ba926692210 17
xiaxia686 11:5ba926692210 18 //SET P30
xiaxia686 11:5ba926692210 19 LPC_PINCON->PINSEL0|=((1<<8)|(1<<9));
xiaxia686 11:5ba926692210 20
xiaxia686 11:5ba926692210 21 //configure counter
xiaxia686 11:5ba926692210 22 LPC_TIM2->TCR =0x2;//counter disable
xiaxia686 11:5ba926692210 23 LPC_TIM2->CTCR =0x1;//counter mode,increments on rising edges
xiaxia686 11:5ba926692210 24 LPC_TIM2->PR =0x0;//set prescaler
xiaxia686 11:5ba926692210 25 LPC_TIM2->MR0 = STEPPER_DIV; // Match count for 3200 counts
xiaxia686 11:5ba926692210 26 LPC_TIM2->MCR = 2; // Reset on Match
xiaxia686 11:5ba926692210 27 LPC_TIM2->TCR =0x1; // counter enable
xiaxia686 11:5ba926692210 28
xiaxia686 11:5ba926692210 29 // initiates the stepper motor
xiaxia686 11:5ba926692210 30 _step.period(STEPPER_PERIOD);
xiaxia686 11:5ba926692210 31 _step.pulsewidth(STEPPER_PERIOD/2.0f); // 50% duty cycle always
xiaxia686 11:5ba926692210 32
xiaxia686 11:5ba926692210 33 //init callabck function
xiaxia686 11:5ba926692210 34 callbackfunc = NULL;
xiaxia686 11:5ba926692210 35 callbackobj = NULL;
xiaxia686 11:5ba926692210 36 mcallbackfunc = NULL;
xiaxia686 11:5ba926692210 37
xiaxia686 11:5ba926692210 38 //init some variables
xiaxia686 11:5ba926692210 39 IR_counter = 0;
xiaxia686 11:5ba926692210 40 IR_step_counter = 0;
xiaxia686 11:5ba926692210 41 pulsewidth_max = 0;
xiaxia686 11:5ba926692210 42 step_count_old = 0;
xiaxia686 11:5ba926692210 43
xiaxia686 11:5ba926692210 44 _ir.callbackobj = (DummyCT*)this;
xiaxia686 11:5ba926692210 45 _ir.mcallbackfunc = (void (DummyCT::*)(float pulsewidth)) &IR_TURRET::_ir_isr;
xiaxia686 11:5ba926692210 46
xiaxia686 11:5ba926692210 47
xiaxia686 11:5ba926692210 48 }
xiaxia686 11:5ba926692210 49
xiaxia686 11:5ba926692210 50 void IR_TURRET::_ir_isr(float pulsewidth)
xiaxia686 11:5ba926692210 51 {
xiaxia686 11:5ba926692210 52 static int step_count_local = 0;
xiaxia686 11:5ba926692210 53
xiaxia686 11:5ba926692210 54 step_count_local = LPC_TIM2->TC; // save current counter value to a local
xiaxia686 11:5ba926692210 55 // resets timeout if pulse is captured
xiaxia686 11:5ba926692210 56 _ir_timeout.detach();
xiaxia686 11:5ba926692210 57
xiaxia686 11:5ba926692210 58 // 360 degrees overflow protection
xiaxia686 11:5ba926692210 59 if ( step_count_local < step_count_old ) {
xiaxia686 11:5ba926692210 60 step_count_local += STEPPER_DIV;
xiaxia686 11:5ba926692210 61 }
xiaxia686 11:5ba926692210 62
xiaxia686 11:5ba926692210 63 //IR_timeout_sema.wait();
xiaxia686 11:5ba926692210 64 step_count_old = step_count_local;
xiaxia686 11:5ba926692210 65 // finds the maximum pulsewidth
xiaxia686 11:5ba926692210 66 if ( pulsewidth > pulsewidth_max) {
xiaxia686 11:5ba926692210 67 pulsewidth_max = pulsewidth;
xiaxia686 11:5ba926692210 68 }
xiaxia686 11:5ba926692210 69 IR_counter ++;
xiaxia686 11:5ba926692210 70 IR_step_counter += step_count_local;
xiaxia686 11:5ba926692210 71
xiaxia686 11:5ba926692210 72 _ir_timeout.attach(this, &IR_TURRET::_ir_timeout_isr, IR_TIMEOUT);
xiaxia686 11:5ba926692210 73 }
xiaxia686 11:5ba926692210 74
xiaxia686 11:5ba926692210 75 void IR_TURRET::_ir_timeout_isr()
xiaxia686 11:5ba926692210 76 {
xiaxia686 11:5ba926692210 77
xiaxia686 11:5ba926692210 78 static int IR_index = -1;
xiaxia686 11:5ba926692210 79
xiaxia686 11:5ba926692210 80 // detach the isr
xiaxia686 11:5ba926692210 81 _ir.mcallbackfunc = NULL;
xiaxia686 11:5ba926692210 82
xiaxia686 11:5ba926692210 83
xiaxia686 11:5ba926692210 84 if ((pulsewidth_max <= IR0_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR0_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
xiaxia686 11:5ba926692210 85 IR_index = 0;
xiaxia686 11:5ba926692210 86 } else if ((pulsewidth_max <= IR1_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR1_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
xiaxia686 11:5ba926692210 87 IR_index = 1;
xiaxia686 11:5ba926692210 88 } else if ((pulsewidth_max <= IR2_PULSEWIDTH + PULSEWIDTH_TOLERANCE) && (pulsewidth_max > IR2_PULSEWIDTH - PULSEWIDTH_TOLERANCE)) {
xiaxia686 11:5ba926692210 89 IR_index = 2;
xiaxia686 11:5ba926692210 90 }
xiaxia686 11:5ba926692210 91
xiaxia686 11:5ba926692210 92 if (IR_index != -1) {
xiaxia686 11:5ba926692210 93 _angle[IR_index] = rectifyAng(((float)IR_step_counter/IR_counter)*STEP_ANGLE);
xiaxia686 11:5ba926692210 94 _variance[IR_index] = (STEPS_PER_PULSEPERIOD*IR_counter*STEP_ANGLE);
xiaxia686 11:5ba926692210 95
xiaxia686 11:5ba926692210 96 if (callbackfunc)
xiaxia686 11:5ba926692210 97 (*callbackfunc)(IR_index, _angle[IR_index],_variance[IR_index]);
xiaxia686 11:5ba926692210 98
xiaxia686 11:5ba926692210 99 if (callbackobj && mcallbackfunc)
xiaxia686 11:5ba926692210 100 (callbackobj->*mcallbackfunc)(IR_index, _angle[IR_index], _variance[IR_index]);
xiaxia686 11:5ba926692210 101 }
xiaxia686 11:5ba926692210 102
xiaxia686 11:5ba926692210 103 IR_counter = 0;
xiaxia686 11:5ba926692210 104 IR_step_counter = 0;
xiaxia686 11:5ba926692210 105 pulsewidth_max = 0;
xiaxia686 11:5ba926692210 106 step_count_old = 0;
xiaxia686 11:5ba926692210 107 IR_index = -1;
xiaxia686 11:5ba926692210 108
xiaxia686 11:5ba926692210 109 // reattach the callback
xiaxia686 11:5ba926692210 110 _ir.mcallbackfunc = (void (DummyCT::*)(float pulsewidth)) &IR_TURRET::_ir_isr;
xiaxia686 11:5ba926692210 111 }