N K
/
foc-ed_in_the_bot_compact
last working
Fork of foc-ed_in_the_bot_compact by
PositionSensor/PositionSensor.cpp@8:70122bad5f90, 2016-04-23 (annotated)
- Committer:
- bwang
- Date:
- Sat Apr 23 21:29:32 2016 +0000
- Revision:
- 8:70122bad5f90
- Parent:
- 7:caebf421f288
foc'ed in the bot
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 0:bac9c3a3a6ca | 1 | |
bwang | 0:bac9c3a3a6ca | 2 | #include "mbed.h" |
bwang | 0:bac9c3a3a6ca | 3 | #include "PositionSensor.h" |
bwang | 0:bac9c3a3a6ca | 4 | #include <math.h> |
bwang | 0:bac9c3a3a6ca | 5 | |
bwang | 0:bac9c3a3a6ca | 6 | /* |
bwang | 0:bac9c3a3a6ca | 7 | * CPR: counts per revolution (4x lines per revolution) |
bwang | 0:bac9c3a3a6ca | 8 | * offset: mechanical position offset in radians |
bwang | 0:bac9c3a3a6ca | 9 | */ |
bwang | 0:bac9c3a3a6ca | 10 | |
bwang | 0:bac9c3a3a6ca | 11 | PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset) { |
bwang | 0:bac9c3a3a6ca | 12 | _CPR = CPR; |
bwang | 0:bac9c3a3a6ca | 13 | _offset = offset; |
bwang | 0:bac9c3a3a6ca | 14 | |
bwang | 0:bac9c3a3a6ca | 15 | __GPIOA_CLK_ENABLE(); |
bwang | 0:bac9c3a3a6ca | 16 | __GPIOB_CLK_ENABLE(); |
bwang | 0:bac9c3a3a6ca | 17 | |
bwang | 0:bac9c3a3a6ca | 18 | GPIOB->MODER |= GPIO_MODER_MODER3_1; |
bwang | 0:bac9c3a3a6ca | 19 | GPIOB->OTYPER |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ; |
bwang | 0:bac9c3a3a6ca | 20 | GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ; |
bwang | 0:bac9c3a3a6ca | 21 | GPIOB->AFR[0] |= 0x00001000 ; |
bwang | 0:bac9c3a3a6ca | 22 | |
bwang | 0:bac9c3a3a6ca | 23 | GPIOA->MODER |= GPIO_MODER_MODER15_1; |
bwang | 0:bac9c3a3a6ca | 24 | GPIOA->OTYPER |= GPIO_OTYPER_OT_15; |
bwang | 0:bac9c3a3a6ca | 25 | GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15; |
bwang | 0:bac9c3a3a6ca | 26 | GPIOA->AFR[1] |= 0x10000000 ; |
bwang | 0:bac9c3a3a6ca | 27 | |
bwang | 0:bac9c3a3a6ca | 28 | __TIM2_CLK_ENABLE(); |
bwang | 0:bac9c3a3a6ca | 29 | |
bwang | 0:bac9c3a3a6ca | 30 | TIM2->CR1 = 0x0001; |
bwang | 0:bac9c3a3a6ca | 31 | TIM2->SMCR = TIM_ENCODERMODE_TI12; |
bwang | 0:bac9c3a3a6ca | 32 | TIM2->CCMR1 = 0xf1f1; |
bwang | 0:bac9c3a3a6ca | 33 | TIM2->CCMR2 = 0x0000; |
bwang | 0:bac9c3a3a6ca | 34 | TIM2->CCER = 0x0011; |
bwang | 0:bac9c3a3a6ca | 35 | TIM2->PSC = 0x0000; |
bwang | 0:bac9c3a3a6ca | 36 | TIM2->ARR = 0xffffffff; |
bwang | 0:bac9c3a3a6ca | 37 | |
bwang | 0:bac9c3a3a6ca | 38 | TIM2->CNT = 0; |
bwang | 0:bac9c3a3a6ca | 39 | |
bwang | 0:bac9c3a3a6ca | 40 | ZPulse = new InterruptIn(PB_12); |
bwang | 0:bac9c3a3a6ca | 41 | ZSense = new DigitalIn(PB_12); |
bwang | 0:bac9c3a3a6ca | 42 | ZPulse->enable_irq(); |
bwang | 0:bac9c3a3a6ca | 43 | ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount); |
bwang | 0:bac9c3a3a6ca | 44 | ZPulse->mode(PullDown); |
bwang | 0:bac9c3a3a6ca | 45 | |
bwang | 0:bac9c3a3a6ca | 46 | ZTest = new DigitalOut(PC_10); |
bwang | 0:bac9c3a3a6ca | 47 | ZTest->write(1); |
bwang | 0:bac9c3a3a6ca | 48 | state = 0; |
bwang | 0:bac9c3a3a6ca | 49 | } |
bwang | 0:bac9c3a3a6ca | 50 | |
bwang | 0:bac9c3a3a6ca | 51 | /* |
bwang | 0:bac9c3a3a6ca | 52 | * Returns mechanical position in radians |
bwang | 0:bac9c3a3a6ca | 53 | */ |
bwang | 0:bac9c3a3a6ca | 54 | |
bwang | 0:bac9c3a3a6ca | 55 | float PositionSensorEncoder::GetMechPosition() { |
bwang | 0:bac9c3a3a6ca | 56 | int raw = TIM2->CNT; |
bwang | 0:bac9c3a3a6ca | 57 | if (raw < 0) raw += _CPR; |
bwang | 0:bac9c3a3a6ca | 58 | if (raw >= _CPR) raw -= _CPR; |
bwang | 0:bac9c3a3a6ca | 59 | float signed_elec = fmod((1.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f); |
bwang | 0:bac9c3a3a6ca | 60 | if (signed_elec < 0){ |
bwang | 0:bac9c3a3a6ca | 61 | return signed_elec + 6.28318530718f; |
bwang | 0:bac9c3a3a6ca | 62 | } |
bwang | 0:bac9c3a3a6ca | 63 | else{ |
bwang | 0:bac9c3a3a6ca | 64 | return signed_elec; |
bwang | 0:bac9c3a3a6ca | 65 | } |
bwang | 0:bac9c3a3a6ca | 66 | } |
bwang | 0:bac9c3a3a6ca | 67 | |
bwang | 0:bac9c3a3a6ca | 68 | /* |
bwang | 0:bac9c3a3a6ca | 69 | * Returns electrical position in radians |
bwang | 0:bac9c3a3a6ca | 70 | */ |
bwang | 0:bac9c3a3a6ca | 71 | |
bwang | 0:bac9c3a3a6ca | 72 | float PositionSensorEncoder::GetElecPosition() { |
bwang | 0:bac9c3a3a6ca | 73 | int raw = TIM2->CNT; |
bwang | 0:bac9c3a3a6ca | 74 | if (raw < 0) raw += _CPR; |
bwang | 0:bac9c3a3a6ca | 75 | if (raw >= _CPR) raw -= _CPR; |
bwang | 7:caebf421f288 | 76 | float signed_elec = fmod((10.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f); |
bwang | 0:bac9c3a3a6ca | 77 | if (signed_elec < 0) { |
bwang | 0:bac9c3a3a6ca | 78 | return signed_elec + 6.28318530718f; |
bwang | 0:bac9c3a3a6ca | 79 | } else { |
bwang | 0:bac9c3a3a6ca | 80 | return signed_elec; |
bwang | 0:bac9c3a3a6ca | 81 | } |
bwang | 0:bac9c3a3a6ca | 82 | } |
bwang | 0:bac9c3a3a6ca | 83 | |
bwang | 8:70122bad5f90 | 84 | int PositionSensorEncoder::GetRawPosition() { |
bwang | 8:70122bad5f90 | 85 | return TIM2->CNT; |
bwang | 8:70122bad5f90 | 86 | } |
bwang | 8:70122bad5f90 | 87 | |
bwang | 0:bac9c3a3a6ca | 88 | void PositionSensorEncoder::ZeroEncoderCount(void){ |
bwang | 0:bac9c3a3a6ca | 89 | if (ZSense->read() == 1){ |
bwang | 0:bac9c3a3a6ca | 90 | if (ZSense->read() == 1){ |
bwang | 0:bac9c3a3a6ca | 91 | TIM2->CNT=0; |
bwang | 0:bac9c3a3a6ca | 92 | } |
bwang | 0:bac9c3a3a6ca | 93 | } |
bwang | 0:bac9c3a3a6ca | 94 | } |