Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

PositionSensor/PositionSensor.cpp@252:38644631ed97, 2018-11-13 (annotated)

Committer:
bwang
Date:
Tue Nov 13 17:46:23 2018 +0000
Revision:
252:38644631ed97
Parent:
225:81b72a4bf18b 
11/13/2018 12:45 - hitting <return> on empty line prints "\r>", so that hitting enter after intially connecting to the controller generates a prompt

Who changed what in which revision?

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