Bayley Wang
/
foc-ed_in_the_bot_compact
robot
PositionSensor/PositionSensor.cpp
- Committer:
- bwang
- Date:
- 2017-04-25
- Revision:
- 120:57b6f3b1356b
- Parent:
- 119:ad7a6af6fba3
- Child:
- 121:de10418bf2c2
File content as of revision 120:57b6f3b1356b:
#include "mbed.h" #include "PositionSensor.h" #include "config_motor.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; _lobes = (int) (RESOLVER_LOBES); _valid = false; _rotations = 0; __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 = 0x0101; 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); } /* * Returns mechanical position in radians */ float PositionSensorEncoder::GetMechPosition() { int raw = TIM2->CNT; if (raw < 0) raw += _cpr; if (raw >= _cpr) raw -= _cpr; float mp = fmod(1 / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset), 2 * PI); if (mp < 0){ return mp + 2 * PI; } else { return mp; } } /* * 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; } float PositionSensorEncoder::GetUnlimitedMechPosition() { return GetUnlimitedElecPosition() / 3.0f; } 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; }