Cooper Liu
/
Eurobot2013_Co-Processor
working version with calibration done
Fork of Eurobot2013 by
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?
User | Revision | Line number | New 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 | } |