Jared DiCarlo / Mbed 2 deprecated foc-ed_in_the_bot_compact

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FastPWM3 mbed

Fork of foc-ed_in_the_bot_compact by Bayley Wang

PositionSensor/PositionSensor.cpp

Committer:
dicarloj
Date:
2016-10-30
Revision:
13:41d102a53caf
Parent:
9:074575151e4b

File content as of revision 13:41d102a53caf:


#include "mbed.h"
#include "PositionSensor.h"
#include "config.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(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((POLE_PAIRS / RESOLVER_LOBES * (6.28318530718f * (raw) / (float)_cpr + _offset)), 6.28318530718f);
    if (signed_elec < 0) {
        return signed_elec + 6.28318530718f;
    } else {
        return signed_elec;
    }
}

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