PositionSensor.cpp

00001 
00002 #include "mbed.h"
00003 #include "PositionSensor.h"
00004 #include "config.h"
00005 #include <math.h>
00006 
00007 /*
00008  * CPR: counts per revolution (4x lines per revolution)
00009  * offset: mechanical position offset in radians
00010  */
00011     
00012 PositionSensorEncoder::PositionSensorEncoder(int cpr, float offset) {
00013     _cpr = cpr;
00014     _offset = offset;
00015     
00016     __GPIOA_CLK_ENABLE();
00017     __GPIOB_CLK_ENABLE();
00018  
00019     GPIOB->MODER   |= GPIO_MODER_MODER3_1;           
00020     GPIOB->OTYPER  |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ;                 
00021     GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ;     
00022     GPIOB->AFR[0]  |= 0x00001000 ;                                          
00023     
00024     GPIOA->MODER   |= GPIO_MODER_MODER15_1;           
00025     GPIOA->OTYPER  |= GPIO_OTYPER_OT_15;                 
00026     GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15;     
00027     GPIOA->AFR[1]  |= 0x10000000 ;
00028    
00029     __TIM2_CLK_ENABLE();
00030  
00031     TIM2->CR1   = 0x0001;
00032     TIM2->SMCR  = TIM_ENCODERMODE_TI12;
00033     TIM2->CCMR1 = 0xf1f1;
00034     TIM2->CCMR2 = 0x0000;
00035     TIM2->CCER  = 0x0011;
00036     TIM2->PSC   = 0x0000;
00037     TIM2->ARR   = 0xffffffff;
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     ZTest = new DigitalOut(PC_10);
00048     ZTest->write(1);
00049     state = 0;
00050 }
00051 
00052 /*
00053  * Returns mechanical position in radians
00054  */
00055  
00056 float PositionSensorEncoder::GetMechPosition() {
00057     int raw = TIM2->CNT;
00058     if (raw < 0) raw += _cpr;
00059     if (raw >= _cpr) raw -= _cpr;
00060     float signed_elec = fmod(6.28318530718f * (raw) / (float)_cpr + _offset, 6.28318530718f);
00061     if (signed_elec < 0){
00062         return signed_elec + 6.28318530718f;
00063     }
00064     else{
00065         return signed_elec;
00066     }  
00067 }
00068 
00069 /*
00070  * Returns electrical position in radians
00071  */
00072  
00073 float PositionSensorEncoder::GetElecPosition() {
00074     int raw = TIM2->CNT;
00075     if (raw < 0) raw += _cpr;
00076     if (raw >= _cpr) raw -= _cpr;
00077     float signed_elec = fmod((POLE_PAIRS / RESOLVER_LOBES * (6.28318530718f * (raw) / (float)_cpr + _offset)), 6.28318530718f);
00078     if (signed_elec < 0) {
00079         return signed_elec + 6.28318530718f;
00080     } else {
00081         return signed_elec;
00082     }
00083 }
00084 
00085 void PositionSensorEncoder::ZeroEncoderCount(void){
00086     if (ZSense->read() == 1){
00087         if (ZSense->read() == 1){
00088             TIM2->CNT=0;
00089         }
00090     }
00091 }