PositionSensor.cpp

00001 #include <math.h>
00002 #include "mbed.h"
00003 #include "PositionSensor.h"
00004 
00005 #include "derived.h"
00006 #include "prefs.h"
00007 
00008 /*
00009  * CPR: counts per revolution (4x lines per revolution)
00010  * offset: mechanical position offset in radians
00011  */
00012 
00013 PositionSensorEncoder::PositionSensorEncoder(int cpr, float offset) {
00014     _cpr = cpr;
00015     _offset = offset;
00016     _lobes = (int) (_RESOLVER_LOBES);
00017     
00018     _valid = false;
00019     _rotations = 0;
00020     
00021     __GPIOA_CLK_ENABLE();                                     
00022     
00023     GPIOA->MODER   |= GPIO_MODER_MODER15_1 | GPIO_MODER_MODER1_1;           
00024     GPIOA->OTYPER  |= GPIO_OTYPER_OT_15 | GPIO_OTYPER_OT_1;                 
00025     GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15 | GPIO_OSPEEDER_OSPEEDR1;
00026     GPIOA->AFR[0]  |= 0x00000010;
00027     GPIOA->AFR[1]  |= 0x10000000 ;
00028    
00029     __TIM2_CLK_ENABLE();
00030  
00031     TIM2->CR1   = 0x0001;
00032     TIM2->SMCR  = TIM_ENCODERMODE_TI12;
00033     TIM2->CCMR1 = 0x0101;
00034     TIM2->CCMR2 = 0x0000;
00035     TIM2->CCER  = 0x0011;
00036     TIM2->PSC   = 0x0000;
00037     TIM2->ARR   = _cpr - 1;
00038   
00039     TIM2->CNT = 0;
00040     
00041     ZPulse = new InterruptIn(PB_12);
00042     ZSense = new DigitalIn(PB_12);
00043     ZPulse->enable_irq();
00044     ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
00045     ZPulse->mode(PullDown);
00046 }
00047 
00048 /*
00049  * Returns mechanical position in radians
00050  */
00051  
00052 float PositionSensorEncoder::GetMechPosition() {
00053     return GetUnlimitedElecPosition() / _POLE_PAIRS;
00054 }
00055 
00056 /*
00057  * Returns electrical position in radians
00058  */
00059  
00060 float PositionSensorEncoder::GetElecPosition() {
00061     int raw = TIM2->CNT;
00062     if (raw < 0) raw += _cpr;
00063     if (raw >= _cpr) raw -= _cpr;
00064     float ep = fmod((_POLE_PAIRS / _RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset)), 2 * PI);
00065     if (ep < 0) {
00066         return ep + 2 * PI;
00067     } else {
00068         return ep;
00069     }
00070 }
00071 
00072 /*
00073  * Return the electrical position in radians (no limit, INT_MAX * 2 * PI max value)
00074  */
00075  
00076 float PositionSensorEncoder::GetUnlimitedElecPosition() {
00077     int raw = TIM2->CNT;
00078     float ep = _POLE_PAIRS / _RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset);
00079     return ep + _rotations * 2 * PI;
00080 }
00081 
00082 void PositionSensorEncoder::ZeroEncoderCount(void){
00083     if (ZSense->read() == 1){
00084         if (ZSense->read() == 1){
00085             TIM2->CNT = 0;
00086             int dir = TIM2->CR1 & (1 << 4);
00087             if (!_valid) {
00088                 _valid = true;
00089                 return;
00090             }
00091             if (dir == 0) {//upcounting
00092                 _rotations++;
00093             } else {//downcounting
00094                 _rotations--;
00095             }
00096         }
00097     }
00098 }
00099 
00100 bool PositionSensorEncoder::IsValid() {
00101     return _valid;
00102 }