Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

PositionSensor/PositionSensor.cpp@0:bac9c3a3a6ca, 2016-03-09 (annotated)

Committer:
bwang
Date:
Wed Mar 09 06:44:51 2016 +0000
Revision:
0:bac9c3a3a6ca
Child:
2:eabe8feaaabb
open loop, working; motor draws ~400mA at 30V; pre-center aligned PWM

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 0:bac9c3a3a6ca 4 #include <math.h>
bwang 0:bac9c3a3a6ca 5
bwang 0:bac9c3a3a6ca 6 /*
bwang 0:bac9c3a3a6ca 7 * CPR: counts per revolution (4x lines per revolution)
bwang 0:bac9c3a3a6ca 8 * offset: mechanical position offset in radians
bwang 0:bac9c3a3a6ca 9 */
bwang 0:bac9c3a3a6ca 10
bwang 0:bac9c3a3a6ca 11 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset) {
bwang 0:bac9c3a3a6ca 12 _CPR = CPR;
bwang 0:bac9c3a3a6ca 13 _offset = offset;
bwang 0:bac9c3a3a6ca 14
bwang 0:bac9c3a3a6ca 15 __GPIOA_CLK_ENABLE();
bwang 0:bac9c3a3a6ca 16 __GPIOB_CLK_ENABLE();
bwang 0:bac9c3a3a6ca 17
bwang 0:bac9c3a3a6ca 18 GPIOB->MODER |= GPIO_MODER_MODER3_1;
bwang 0:bac9c3a3a6ca 19 GPIOB->OTYPER |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ;
bwang 0:bac9c3a3a6ca 20 GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ;
bwang 0:bac9c3a3a6ca 21 GPIOB->AFR[0] |= 0x00001000 ;
bwang 0:bac9c3a3a6ca 22
bwang 0:bac9c3a3a6ca 23 GPIOA->MODER |= GPIO_MODER_MODER15_1;
bwang 0:bac9c3a3a6ca 24 GPIOA->OTYPER |= GPIO_OTYPER_OT_15;
bwang 0:bac9c3a3a6ca 25 GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15;
bwang 0:bac9c3a3a6ca 26 GPIOA->AFR[1] |= 0x10000000 ;
bwang 0:bac9c3a3a6ca 27
bwang 0:bac9c3a3a6ca 28 __TIM2_CLK_ENABLE();
bwang 0:bac9c3a3a6ca 29
bwang 0:bac9c3a3a6ca 30 TIM2->CR1 = 0x0001;
bwang 0:bac9c3a3a6ca 31 TIM2->SMCR = TIM_ENCODERMODE_TI12;
bwang 0:bac9c3a3a6ca 32 TIM2->CCMR1 = 0xf1f1;
bwang 0:bac9c3a3a6ca 33 TIM2->CCMR2 = 0x0000;
bwang 0:bac9c3a3a6ca 34 TIM2->CCER = 0x0011;
bwang 0:bac9c3a3a6ca 35 TIM2->PSC = 0x0000;
bwang 0:bac9c3a3a6ca 36 TIM2->ARR = 0xffffffff;
bwang 0:bac9c3a3a6ca 37
bwang 0:bac9c3a3a6ca 38 TIM2->CNT = 0;
bwang 0:bac9c3a3a6ca 39
bwang 0:bac9c3a3a6ca 40 ZPulse = new InterruptIn(PB_12);
bwang 0:bac9c3a3a6ca 41 ZSense = new DigitalIn(PB_12);
bwang 0:bac9c3a3a6ca 42 ZPulse->enable_irq();
bwang 0:bac9c3a3a6ca 43 ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
bwang 0:bac9c3a3a6ca 44 ZPulse->mode(PullDown);
bwang 0:bac9c3a3a6ca 45
bwang 0:bac9c3a3a6ca 46 ZTest = new DigitalOut(PC_10);
bwang 0:bac9c3a3a6ca 47 ZTest->write(1);
bwang 0:bac9c3a3a6ca 48 state = 0;
bwang 0:bac9c3a3a6ca 49 }
bwang 0:bac9c3a3a6ca 50
bwang 0:bac9c3a3a6ca 51 /*
bwang 0:bac9c3a3a6ca 52 * Returns mechanical position in radians
bwang 0:bac9c3a3a6ca 53 */
bwang 0:bac9c3a3a6ca 54
bwang 0:bac9c3a3a6ca 55 float PositionSensorEncoder::GetMechPosition() {
bwang 0:bac9c3a3a6ca 56 int raw = TIM2->CNT;
bwang 0:bac9c3a3a6ca 57 if (raw < 0) raw += _CPR;
bwang 0:bac9c3a3a6ca 58 if (raw >= _CPR) raw -= _CPR;
bwang 0:bac9c3a3a6ca 59 float signed_elec = fmod((1.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f);
bwang 0:bac9c3a3a6ca 60 if (signed_elec < 0){
bwang 0:bac9c3a3a6ca 61 return signed_elec + 6.28318530718f;
bwang 0:bac9c3a3a6ca 62 }
bwang 0:bac9c3a3a6ca 63 else{
bwang 0:bac9c3a3a6ca 64 return signed_elec;
bwang 0:bac9c3a3a6ca 65 }
bwang 0:bac9c3a3a6ca 66 }
bwang 0:bac9c3a3a6ca 67
bwang 0:bac9c3a3a6ca 68 /*
bwang 0:bac9c3a3a6ca 69 * Returns electrical position in radians
bwang 0:bac9c3a3a6ca 70 */
bwang 0:bac9c3a3a6ca 71
bwang 0:bac9c3a3a6ca 72 float PositionSensorEncoder::GetElecPosition() {
bwang 0:bac9c3a3a6ca 73 int raw = TIM2->CNT;
bwang 0:bac9c3a3a6ca 74 if (raw < 0) raw += _CPR;
bwang 0:bac9c3a3a6ca 75 if (raw >= _CPR) raw -= _CPR;
bwang 0:bac9c3a3a6ca 76 float signed_elec = fmod((2.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f);
bwang 0:bac9c3a3a6ca 77 if (signed_elec < 0) {
bwang 0:bac9c3a3a6ca 78 return signed_elec + 6.28318530718f;
bwang 0:bac9c3a3a6ca 79 } else {
bwang 0:bac9c3a3a6ca 80 return signed_elec;
bwang 0:bac9c3a3a6ca 81 }
bwang 0:bac9c3a3a6ca 82 }
bwang 0:bac9c3a3a6ca 83
bwang 0:bac9c3a3a6ca 84 void PositionSensorEncoder::ZeroEncoderCount(void){
bwang 0:bac9c3a3a6ca 85 if (ZSense->read() == 1){
bwang 0:bac9c3a3a6ca 86 if (ZSense->read() == 1){
bwang 0:bac9c3a3a6ca 87 TIM2->CNT=0;
bwang 0:bac9c3a3a6ca 88 state = !state;
bwang 0:bac9c3a3a6ca 89 ZTest->write(state);
bwang 0:bac9c3a3a6ca 90 }
bwang 0:bac9c3a3a6ca 91 }
bwang 0:bac9c3a3a6ca 92 }