N K
/
foc-ed_in_the_bot_compact
last working
Fork of foc-ed_in_the_bot_compact by
Diff: PositionSensor/PositionSensor.cpp
- Revision:
- 0:bac9c3a3a6ca
- Child:
- 2:eabe8feaaabb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PositionSensor/PositionSensor.cpp Wed Mar 09 06:44:51 2016 +0000 @@ -0,0 +1,92 @@ + +#include "mbed.h" +#include "PositionSensor.h" +#include <math.h> + +/* + * CPR: counts per revolution (4x lines per revolution) + * offset: mechanical position offset in radians + */ + +PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset) { + _CPR = CPR; + _offset = offset; + + __GPIOA_CLK_ENABLE(); + __GPIOB_CLK_ENABLE(); + + GPIOB->MODER |= GPIO_MODER_MODER3_1; + GPIOB->OTYPER |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ; + GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ; + GPIOB->AFR[0] |= 0x00001000 ; + + GPIOA->MODER |= GPIO_MODER_MODER15_1; + GPIOA->OTYPER |= GPIO_OTYPER_OT_15; + GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15; + GPIOA->AFR[1] |= 0x10000000 ; + + __TIM2_CLK_ENABLE(); + + TIM2->CR1 = 0x0001; + TIM2->SMCR = TIM_ENCODERMODE_TI12; + TIM2->CCMR1 = 0xf1f1; + TIM2->CCMR2 = 0x0000; + TIM2->CCER = 0x0011; + TIM2->PSC = 0x0000; + TIM2->ARR = 0xffffffff; + + TIM2->CNT = 0; + + ZPulse = new InterruptIn(PB_12); + ZSense = new DigitalIn(PB_12); + ZPulse->enable_irq(); + ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount); + ZPulse->mode(PullDown); + + ZTest = new DigitalOut(PC_10); + ZTest->write(1); + state = 0; +} + +/* + * Returns mechanical position in radians + */ + +float PositionSensorEncoder::GetMechPosition() { + int raw = TIM2->CNT; + if (raw < 0) raw += _CPR; + if (raw >= _CPR) raw -= _CPR; + float signed_elec = fmod((1.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f); + if (signed_elec < 0){ + return signed_elec + 6.28318530718f; + } + else{ + return signed_elec; + } +} + +/* + * Returns electrical position in radians + */ + +float PositionSensorEncoder::GetElecPosition() { + int raw = TIM2->CNT; + if (raw < 0) raw += _CPR; + if (raw >= _CPR) raw -= _CPR; + float signed_elec = fmod((2.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f); + if (signed_elec < 0) { + return signed_elec + 6.28318530718f; + } else { + return signed_elec; + } +} + +void PositionSensorEncoder::ZeroEncoderCount(void){ + if (ZSense->read() == 1){ + if (ZSense->read() == 1){ + TIM2->CNT=0; + state = !state; + ZTest->write(state); + } + } +} \ No newline at end of file