File content as of revision 92:a9dac72d8cac:

#include "mbed.h"
#include "PositionSensor.h"
#include "config_motor.h"
#include <math.h>

/*
 * CPR: counts per revolution (4x lines per revolution)
 * offset: mechanical position offset in radians
 */
    
PositionSensorEncoder::PositionSensorEncoder(int cpr, float offset) {
    _cpr = cpr;
    _offset = offset;
    _valid = false;
    
    __GPIOA_CLK_ENABLE();
    __GPIOB_CLK_ENABLE();
 
    GPIOB->MODER   |= GPIO_MODER_MODER3_1;           
    GPIOB->OTYPER  |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ;                 
    GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ;     
    GPIOB->AFR[0]  |= 0x00001000 ;                                          
    
    GPIOA->MODER   |= GPIO_MODER_MODER15_1;           
    GPIOA->OTYPER  |= GPIO_OTYPER_OT_15;                 
    GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15;     
    GPIOA->AFR[1]  |= 0x10000000 ;
   
    __TIM2_CLK_ENABLE();
 
    TIM2->CR1   = 0x0001;
    TIM2->SMCR  = TIM_ENCODERMODE_TI12;
    TIM2->CCMR1 = 0x0101;
    TIM2->CCMR2 = 0x0000;
    TIM2->CCER  = 0x0011;
    TIM2->PSC   = 0x0000;
    TIM2->ARR   = 0xffffffff;
  
    TIM2->CNT = 0;
    
    ZPulse = new InterruptIn(PB_12);
    ZSense = new DigitalIn(PB_12);
    ZPulse->enable_irq();
    ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
    ZPulse->mode(PullDown);
}

/*
 * Returns mechanical position in radians
 */
 
float PositionSensorEncoder::GetMechPosition() {
    int raw = TIM2->CNT;
    if (raw < 0) raw += _cpr;
    if (raw >= _cpr) raw -= _cpr;
    float mp = fmod(1 / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset), 2 * PI);
    if (mp < 0){
        return mp + 2 * PI;
    } else {
        return mp;
    }  
}

/*
 * Returns electrical position in radians
 */
 
float PositionSensorEncoder::GetElecPosition() {
    int raw = TIM2->CNT;
    if (raw < 0) raw += _cpr;
    if (raw >= _cpr) raw -= _cpr;
    float ep = fmod((POLE_PAIRS / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset)), 2 * PI);
    if (ep < 0) {
        return ep + 2 * PI;
    } else {
        return ep;
    }
}

void PositionSensorEncoder::ZeroEncoderCount(void){
    if (ZSense->read() == 1){
        if (ZSense->read() == 1){
            TIM2->CNT = 0;
            _valid = true;
        }
    }
}

bool PositionSensorEncoder::IsValid() {
    return _valid;
}