Diff: PositionSensor/PositionSensor.cpp

Revision:

0:bac9c3a3a6ca

Child:

2:eabe8feaaabb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PositionSensor/PositionSensor.cpp	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,92 @@
+
+#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((2.0f * (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;
+            state = !state;
+            ZTest->write(state);
+        }
+    }
+}
\ No newline at end of file