N K
/
foc-ed_in_the_bot_compact
last working
Fork of foc-ed_in_the_bot_compact by
PositionSensor/PositionSensor.cpp
- Committer:
- bwang
- Date:
- 2016-03-09
- Revision:
- 0:bac9c3a3a6ca
- Child:
- 2:eabe8feaaabb
File content as of revision 0:bac9c3a3a6ca:
#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); } } }