Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

PositionSensor/PositionSensor.cpp@92:a9dac72d8cac, 2017-04-06 (annotated)

Committer:
bwang
Date:
Thu Apr 06 17:33:47 2017 +0000
Revision:
92:a9dac72d8cac
Parent:
28:ed9c1ca386fd
Child:
119:ad7a6af6fba3
--PwmIn now checks lower bounds for sanity and fall without rise; --switched to edge aligned pwm to work around prius module propagation delays

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 0:bac9c3a3a6ca 1
bwang 0:bac9c3a3a6ca 2 #include "mbed.h"
bwang 0:bac9c3a3a6ca 3 #include "PositionSensor.h"
bwang 15:b583cd30b063 4 #include "config_motor.h"
bwang 0:bac9c3a3a6ca 5 #include <math.h>
bwang 0:bac9c3a3a6ca 6
bwang 0:bac9c3a3a6ca 7 /*
bwang 0:bac9c3a3a6ca 8 * CPR: counts per revolution (4x lines per revolution)
bwang 0:bac9c3a3a6ca 9 * offset: mechanical position offset in radians
bwang 0:bac9c3a3a6ca 10 */
bwang 0:bac9c3a3a6ca 11
bwang 9:074575151e4b 12 PositionSensorEncoder::PositionSensorEncoder(int cpr, float offset) {
bwang 9:074575151e4b 13 _cpr = cpr;
bwang 0:bac9c3a3a6ca 14 _offset = offset;
bwang 28:ed9c1ca386fd 15 _valid = false;
bwang 0:bac9c3a3a6ca 16
bwang 0:bac9c3a3a6ca 17 __GPIOA_CLK_ENABLE();
bwang 0:bac9c3a3a6ca 18 __GPIOB_CLK_ENABLE();
bwang 0:bac9c3a3a6ca 19
bwang 0:bac9c3a3a6ca 20 GPIOB->MODER |= GPIO_MODER_MODER3_1;
bwang 0:bac9c3a3a6ca 21 GPIOB->OTYPER |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ;
bwang 0:bac9c3a3a6ca 22 GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ;
bwang 0:bac9c3a3a6ca 23 GPIOB->AFR[0] |= 0x00001000 ;
bwang 0:bac9c3a3a6ca 24
bwang 0:bac9c3a3a6ca 25 GPIOA->MODER |= GPIO_MODER_MODER15_1;
bwang 0:bac9c3a3a6ca 26 GPIOA->OTYPER |= GPIO_OTYPER_OT_15;
bwang 0:bac9c3a3a6ca 27 GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15;
bwang 0:bac9c3a3a6ca 28 GPIOA->AFR[1] |= 0x10000000 ;
bwang 0:bac9c3a3a6ca 29
bwang 0:bac9c3a3a6ca 30 __TIM2_CLK_ENABLE();
bwang 0:bac9c3a3a6ca 31
bwang 0:bac9c3a3a6ca 32 TIM2->CR1 = 0x0001;
bwang 0:bac9c3a3a6ca 33 TIM2->SMCR = TIM_ENCODERMODE_TI12;
bwang 26:955a1dfc2705 34 TIM2->CCMR1 = 0x0101;
bwang 0:bac9c3a3a6ca 35 TIM2->CCMR2 = 0x0000;
bwang 0:bac9c3a3a6ca 36 TIM2->CCER = 0x0011;
bwang 0:bac9c3a3a6ca 37 TIM2->PSC = 0x0000;
bwang 0:bac9c3a3a6ca 38 TIM2->ARR = 0xffffffff;
bwang 0:bac9c3a3a6ca 39
bwang 0:bac9c3a3a6ca 40 TIM2->CNT = 0;
bwang 0:bac9c3a3a6ca 41
bwang 0:bac9c3a3a6ca 42 ZPulse = new InterruptIn(PB_12);
bwang 0:bac9c3a3a6ca 43 ZSense = new DigitalIn(PB_12);
bwang 0:bac9c3a3a6ca 44 ZPulse->enable_irq();
bwang 0:bac9c3a3a6ca 45 ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
bwang 0:bac9c3a3a6ca 46 ZPulse->mode(PullDown);
bwang 0:bac9c3a3a6ca 47 }
bwang 0:bac9c3a3a6ca 48
bwang 0:bac9c3a3a6ca 49 /*
bwang 0:bac9c3a3a6ca 50 * Returns mechanical position in radians
bwang 0:bac9c3a3a6ca 51 */
bwang 0:bac9c3a3a6ca 52
bwang 0:bac9c3a3a6ca 53 float PositionSensorEncoder::GetMechPosition() {
bwang 0:bac9c3a3a6ca 54 int raw = TIM2->CNT;
bwang 9:074575151e4b 55 if (raw < 0) raw += _cpr;
bwang 9:074575151e4b 56 if (raw >= _cpr) raw -= _cpr;
bwang 24:5e18a87a0e95 57 float mp = fmod(1 / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset), 2 * PI);
bwang 24:5e18a87a0e95 58 if (mp < 0){
bwang 24:5e18a87a0e95 59 return mp + 2 * PI;
bwang 15:b583cd30b063 60 } else {
bwang 24:5e18a87a0e95 61 return mp;
bwang 0:bac9c3a3a6ca 62 }
bwang 0:bac9c3a3a6ca 63 }
bwang 0:bac9c3a3a6ca 64
bwang 0:bac9c3a3a6ca 65 /*
bwang 0:bac9c3a3a6ca 66 * Returns electrical position in radians
bwang 0:bac9c3a3a6ca 67 */
bwang 0:bac9c3a3a6ca 68
bwang 0:bac9c3a3a6ca 69 float PositionSensorEncoder::GetElecPosition() {
bwang 0:bac9c3a3a6ca 70 int raw = TIM2->CNT;
bwang 9:074575151e4b 71 if (raw < 0) raw += _cpr;
bwang 9:074575151e4b 72 if (raw >= _cpr) raw -= _cpr;
bwang 24:5e18a87a0e95 73 float ep = fmod((POLE_PAIRS / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset)), 2 * PI);
bwang 24:5e18a87a0e95 74 if (ep < 0) {
bwang 24:5e18a87a0e95 75 return ep + 2 * PI;
bwang 0:bac9c3a3a6ca 76 } else {
bwang 24:5e18a87a0e95 77 return ep;
bwang 0:bac9c3a3a6ca 78 }
bwang 0:bac9c3a3a6ca 79 }
bwang 0:bac9c3a3a6ca 80
bwang 0:bac9c3a3a6ca 81 void PositionSensorEncoder::ZeroEncoderCount(void){
bwang 0:bac9c3a3a6ca 82 if (ZSense->read() == 1){
bwang 0:bac9c3a3a6ca 83 if (ZSense->read() == 1){
bwang 24:5e18a87a0e95 84 TIM2->CNT = 0;
bwang 28:ed9c1ca386fd 85 _valid = true;
bwang 0:bac9c3a3a6ca 86 }
bwang 0:bac9c3a3a6ca 87 }
bwang 28:ed9c1ca386fd 88 }
bwang 28:ed9c1ca386fd 89
bwang 28:ed9c1ca386fd 90 bool PositionSensorEncoder::IsValid() {
bwang 28:ed9c1ca386fd 91 return _valid;
bwang 0:bac9c3a3a6ca 92 }