![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
derp
PositionSensor/PositionSensor.cpp
- Committer:
- bwang
- Date:
- 2016-04-19
- Revision:
- 21:3bfff547c5e2
- Parent:
- 8:314074b56470
File content as of revision 21:3bfff547c5e2:
#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((10.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f); if (signed_elec < 0) { return signed_elec + 6.28318530718f; } else { return signed_elec; } } int PositionSensorEncoder::GetRawPosition() { return TIM2->CNT; } void PositionSensorEncoder::ZeroEncoderCount(void){ if (ZSense->read() == 1){ if (ZSense->read() == 1){ TIM2->CNT=0; } } }