Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

PositionSensor/PositionSensor.cpp@185:5c102874b490, 2018-02-09 (annotated)

Committer:
bwang
Date:
Fri Feb 09 22:19:52 2018 +0000
Revision:
185:5c102874b490
Parent:
156:cf92f967983d
Child:
186:c18db1e31da6
moved all the hard-coded values to defaults.h

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 185:5c102874b490 4 #include "defaults.h"
bwang 185:5c102874b490 5 #include "derived.h"
bwang 185:5c102874b490 6
bwang 185:5c102874b490 7 //#include "config_motor.h"
bwang 0:bac9c3a3a6ca 8 #include <math.h>
bwang 0:bac9c3a3a6ca 9
bwang 0:bac9c3a3a6ca 10 /*
bwang 0:bac9c3a3a6ca 11 * CPR: counts per revolution (4x lines per revolution)
bwang 0:bac9c3a3a6ca 12 * offset: mechanical position offset in radians
bwang 0:bac9c3a3a6ca 13 */
bwang 124:e70ca81676fc 14
bwang 9:074575151e4b 15 PositionSensorEncoder::PositionSensorEncoder(int cpr, float offset) {
bwang 9:074575151e4b 16 _cpr = cpr;
bwang 0:bac9c3a3a6ca 17 _offset = offset;
bwang 119:ad7a6af6fba3 18 _lobes = (int) (RESOLVER_LOBES);
bwang 119:ad7a6af6fba3 19
bwang 28:ed9c1ca386fd 20 _valid = false;
bwang 119:ad7a6af6fba3 21 _rotations = 0;
bwang 0:bac9c3a3a6ca 22
bwang 0:bac9c3a3a6ca 23 __GPIOA_CLK_ENABLE();
bwang 0:bac9c3a3a6ca 24 __GPIOB_CLK_ENABLE();
bwang 0:bac9c3a3a6ca 25
bwang 0:bac9c3a3a6ca 26 GPIOB->MODER |= GPIO_MODER_MODER3_1;
bwang 0:bac9c3a3a6ca 27 GPIOB->OTYPER |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ;
bwang 0:bac9c3a3a6ca 28 GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ;
bwang 0:bac9c3a3a6ca 29 GPIOB->AFR[0] |= 0x00001000 ;
bwang 0:bac9c3a3a6ca 30
bwang 0:bac9c3a3a6ca 31 GPIOA->MODER |= GPIO_MODER_MODER15_1;
bwang 0:bac9c3a3a6ca 32 GPIOA->OTYPER |= GPIO_OTYPER_OT_15;
bwang 0:bac9c3a3a6ca 33 GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15;
bwang 0:bac9c3a3a6ca 34 GPIOA->AFR[1] |= 0x10000000 ;
bwang 0:bac9c3a3a6ca 35
bwang 0:bac9c3a3a6ca 36 __TIM2_CLK_ENABLE();
bwang 0:bac9c3a3a6ca 37
bwang 0:bac9c3a3a6ca 38 TIM2->CR1 = 0x0001;
bwang 0:bac9c3a3a6ca 39 TIM2->SMCR = TIM_ENCODERMODE_TI12;
bwang 26:955a1dfc2705 40 TIM2->CCMR1 = 0x0101;
bwang 0:bac9c3a3a6ca 41 TIM2->CCMR2 = 0x0000;
bwang 0:bac9c3a3a6ca 42 TIM2->CCER = 0x0011;
bwang 0:bac9c3a3a6ca 43 TIM2->PSC = 0x0000;
bwang 156:cf92f967983d 44 TIM2->ARR = _cpr - 1;
bwang 0:bac9c3a3a6ca 45
bwang 0:bac9c3a3a6ca 46 TIM2->CNT = 0;
bwang 0:bac9c3a3a6ca 47
bwang 0:bac9c3a3a6ca 48 ZPulse = new InterruptIn(PB_12);
bwang 0:bac9c3a3a6ca 49 ZSense = new DigitalIn(PB_12);
bwang 0:bac9c3a3a6ca 50 ZPulse->enable_irq();
bwang 0:bac9c3a3a6ca 51 ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
bwang 0:bac9c3a3a6ca 52 ZPulse->mode(PullDown);
bwang 0:bac9c3a3a6ca 53 }
bwang 0:bac9c3a3a6ca 54
bwang 0:bac9c3a3a6ca 55 /*
bwang 0:bac9c3a3a6ca 56 * Returns mechanical position in radians
bwang 0:bac9c3a3a6ca 57 */
bwang 0:bac9c3a3a6ca 58
bwang 0:bac9c3a3a6ca 59 float PositionSensorEncoder::GetMechPosition() {
bwang 121:de10418bf2c2 60 return GetUnlimitedElecPosition() / 3.0f;
bwang 0:bac9c3a3a6ca 61 }
bwang 0:bac9c3a3a6ca 62
bwang 0:bac9c3a3a6ca 63 /*
bwang 0:bac9c3a3a6ca 64 * Returns electrical position in radians
bwang 0:bac9c3a3a6ca 65 */
bwang 0:bac9c3a3a6ca 66
bwang 0:bac9c3a3a6ca 67 float PositionSensorEncoder::GetElecPosition() {
bwang 0:bac9c3a3a6ca 68 int raw = TIM2->CNT;
bwang 9:074575151e4b 69 if (raw < 0) raw += _cpr;
bwang 9:074575151e4b 70 if (raw >= _cpr) raw -= _cpr;
bwang 24:5e18a87a0e95 71 float ep = fmod((POLE_PAIRS / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset)), 2 * PI);
bwang 24:5e18a87a0e95 72 if (ep < 0) {
bwang 24:5e18a87a0e95 73 return ep + 2 * PI;
bwang 0:bac9c3a3a6ca 74 } else {
bwang 24:5e18a87a0e95 75 return ep;
bwang 0:bac9c3a3a6ca 76 }
bwang 0:bac9c3a3a6ca 77 }
bwang 0:bac9c3a3a6ca 78
bwang 119:ad7a6af6fba3 79 /*
bwang 120:57b6f3b1356b 80 * Return the electrical position in radians (no limit, INT_MAX * 2 * PI max value)
bwang 119:ad7a6af6fba3 81 */
bwang 119:ad7a6af6fba3 82
bwang 119:ad7a6af6fba3 83 float PositionSensorEncoder::GetUnlimitedElecPosition() {
bwang 119:ad7a6af6fba3 84 int raw = TIM2->CNT;
bwang 119:ad7a6af6fba3 85 float ep = POLE_PAIRS / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset);
bwang 119:ad7a6af6fba3 86 return ep + _rotations * 2 * PI;
bwang 119:ad7a6af6fba3 87 }
bwang 119:ad7a6af6fba3 88
bwang 0:bac9c3a3a6ca 89 void PositionSensorEncoder::ZeroEncoderCount(void){
bwang 0:bac9c3a3a6ca 90 if (ZSense->read() == 1){
bwang 0:bac9c3a3a6ca 91 if (ZSense->read() == 1){
bwang 24:5e18a87a0e95 92 TIM2->CNT = 0;
bwang 119:ad7a6af6fba3 93 int dir = TIM2->CR1 & (1 << 4);
bwang 119:ad7a6af6fba3 94 if (!_valid) {
bwang 119:ad7a6af6fba3 95 _valid = true;
bwang 119:ad7a6af6fba3 96 return;
bwang 119:ad7a6af6fba3 97 }
bwang 119:ad7a6af6fba3 98 if (dir == 0) {//upcounting
bwang 119:ad7a6af6fba3 99 _rotations++;
bwang 119:ad7a6af6fba3 100 } else {//downcounting
bwang 119:ad7a6af6fba3 101 _rotations--;
bwang 119:ad7a6af6fba3 102 }
bwang 0:bac9c3a3a6ca 103 }
bwang 0:bac9c3a3a6ca 104 }
bwang 28:ed9c1ca386fd 105 }
bwang 28:ed9c1ca386fd 106
bwang 28:ed9c1ca386fd 107 bool PositionSensorEncoder::IsValid() {
bwang 28:ed9c1ca386fd 108 return _valid;
bwang 0:bac9c3a3a6ca 109 }