File content as of revision 8:70122bad5f90:

#include "mbed.h"
#include "PositionSensor.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;
    
    __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 = 0xf1f1;
    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);
    
    ZTest = new DigitalOut(PC_10);
    ZTest->write(1);
    state = 0;
}

/*
 * Returns mechanical position in radians
 */
 
float PositionSensorEncoder::GetMechPosition() {
    int raw = TIM2->CNT;
    if (raw < 0) raw += _CPR;
    if (raw >= _CPR) raw -= _CPR;
    float signed_elec = fmod((1.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f);
    if (signed_elec < 0){
        return signed_elec + 6.28318530718f;
    }
    else{
        return signed_elec;
    }  
}

/*
 * Returns electrical position in radians
 */
 
float PositionSensorEncoder::GetElecPosition() {
    int raw = TIM2->CNT;
    if (raw < 0) raw += _CPR;
    if (raw >= _CPR) raw -= _CPR;
    float signed_elec = fmod((10.0f * (6.28318530718f * (raw) / (float)_CPR + _offset)), 6.28318530718f);
    if (signed_elec < 0) {
        return signed_elec + 6.28318530718f;
    } else {
        return signed_elec;
    }
}

int PositionSensorEncoder::GetRawPosition() {
    return TIM2->CNT;
}

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