Bayley Wang
/
foc-ed_in_the_bot_compact
robot
PositionSensor/PositionSensor.cpp
- Committer:
- bwang
- Date:
- 2018-10-27
- Revision:
- 225:81b72a4bf18b
- Parent:
- 192:3152a86cd108
File content as of revision 225:81b72a4bf18b:
#include <math.h> #include "mbed.h" #include "PositionSensor.h" #include "derived.h" #include "prefs.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; _lobes = (int) (_RESOLVER_LOBES); _valid = false; _rotations = 0; __GPIOA_CLK_ENABLE(); GPIOA->MODER |= GPIO_MODER_MODER15_1 | GPIO_MODER_MODER1_1; GPIOA->OTYPER |= GPIO_OTYPER_OT_15 | GPIO_OTYPER_OT_1; GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15 | GPIO_OSPEEDER_OSPEEDR1; GPIOA->AFR[0] |= 0x00000010; GPIOA->AFR[1] |= 0x10000000 ; __TIM2_CLK_ENABLE(); TIM2->CR1 = 0x0001; TIM2->SMCR = TIM_ENCODERMODE_TI12; TIM2->CCMR1 = 0x0101; TIM2->CCMR2 = 0x0000; TIM2->CCER = 0x0011; TIM2->PSC = 0x0000; TIM2->ARR = _cpr - 1; TIM2->CNT = 0; ZPulse = new InterruptIn(PB_12); ZSense = new DigitalIn(PB_12); ZPulse->enable_irq(); ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount); ZPulse->mode(PullDown); } /* * Returns mechanical position in radians */ float PositionSensorEncoder::GetMechPosition() { return GetUnlimitedElecPosition() / _POLE_PAIRS; } /* * Returns electrical position in radians */ float PositionSensorEncoder::GetElecPosition() { int raw = TIM2->CNT; if (raw < 0) raw += _cpr; if (raw >= _cpr) raw -= _cpr; float ep = fmod((_POLE_PAIRS / _RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset)), 2 * PI); if (ep < 0) { return ep + 2 * PI; } else { return ep; } } /* * Return the electrical position in radians (no limit, INT_MAX * 2 * PI max value) */ float PositionSensorEncoder::GetUnlimitedElecPosition() { int raw = TIM2->CNT; float ep = _POLE_PAIRS / _RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset); return ep + _rotations * 2 * PI; } void PositionSensorEncoder::ZeroEncoderCount(void){ if (ZSense->read() == 1){ if (ZSense->read() == 1){ TIM2->CNT = 0; int dir = TIM2->CR1 & (1 << 4); if (!_valid) { _valid = true; return; } if (dir == 0) {//upcounting _rotations++; } else {//downcounting _rotations--; } } } } bool PositionSensorEncoder::IsValid() { return _valid; }