Diff: Peripherien/Encoder.cpp

Revision:

7:15e6fc689368

Child:

9:56aed8c6779f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Peripherien/Encoder.cpp	Thu May 16 20:42:39 2019 +0000
@@ -0,0 +1,156 @@
+#include "Encoder.h"
+
+using namespace std;
+
+
+Encoder::Encoder(PinName& hallsensor) : HallSensor(hallsensor){        
+        
+        TIM = TIM3;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;    // manually enable port B (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER6;     // reset port A6
+        GPIOA->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port A6
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port A6
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port A6
+        GPIOA->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port A6
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER5;     // reset port B5
+        GPIOB->MODER |= GPIO_MODER_MODER5_1;    // set alternate mode of port B5
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR5;     // reset pull-up/pull-down on port B5
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR5_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~0xF0000000;           // reset alternate function of port B5
+        GPIOB->AFR[0] |= 2 << 4*5;              // set alternate funtion 2 of port B5
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST;  //reset TIM3 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;     // TIM3 clock enable
+        
+        TIM->CR1 = 0x0000;          // counter disable
+        TIM->CR2 = 0x0000;          // reset master mode selection
+        TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges   
+        TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
+        TIM->CCMR2 = 0x0000;        // reset capture mode register 2
+        TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
+        TIM->CNT = 0x0000;          // reset counter value
+        TIM->ARR = 0xBF68;          // auto reload register (49000)
+        TIM->PSC = 0x0003;          // divide count by 4
+        TIM->CR1 = TIM_CR1_CEN;     // counter enable  
+        
+        // Interrupt for Origin Position
+        HallSensor.fall(callback(this, &Encoder::ResetInterrupt));
+        this->resetOn = 0;
+        
+        // Ticker for the calculation of the frequency wiht dt = 5ms
+        this->ticker.attach(callback(this, &Encoder::calculateFrequency),dt);
+           
+}
+
+Encoder::~Encoder() {
+    ticker.detach();
+}
+
+uint8_t Encoder::reset() {
+    static int resetted=0;
+    if(this->resetOn==1){
+       TIM->CNT = 49000;
+       HallSensor.disable_irq();
+       this->resetOn = 0;
+       resetted = 1;
+    }
+    return resetted;
+}
+
+/**
+ * Reads the quadrature encoder counter value.
+ * @return the quadrature encoder counter as a signed 16-bit integer value.
+ */
+uint32_t Encoder::read() {
+    
+    return (uint16_t)49000-TIM->CNT; // Trasform Downcounter in Upcounter
+}
+/*
+ * @return the Angle as a float value
+ */
+float Encoder::readAngle() {
+    uint32_t pulses;
+    float angle;
+    
+    pulses = this->read();
+    angle = 2.0f * PI * pulses / 49000.0f; // 49000 Pulses per Rotation
+    return angle;  
+}
+
+/*
+ * @return the Frequency as a float value in rad/s
+ */
+float Encoder::readFrequency(){
+    return frequency;    
+}
+
+/*
+ * @return the Frequency as a float value in rad/s
+ */
+float Encoder::readAcceleration(){
+    return acceleration;    
+}
+
+/*
+ * @return the Frequency as a float value in rad/s
+ */
+float Encoder::readRPM(){
+    return frequency / (2.0*PI) * 60.0;    
+}
+
+/*
+ * Calculate the pedal frequency every 5ms
+ */
+void Encoder::calculateFrequency(){
+    static float angle, angleOld = 0.0f;
+    static float pedaleFreq, pedaleFreqOld = 0.0f, frequencyOld = 0.0f;
+    static float accelerationOld = 0.0f;
+    
+    // Read actual angle
+    angle = this->readAngle();
+    
+    // Diskrete Ableitung Frequenz
+    pedaleFreq = (angle - angleOld) / dt;
+   
+          
+    // Filter Nulldurchgang mit der Messung der Winkels und Frequenz Grenz [-2.5,2.5]rad/s
+    if(((pedaleFreq - pedaleFreqOld) > 2.5) || ((pedaleFreq - pedaleFreqOld) < -2.5f) ){
+    frequency = frequencyOld;
+    } 
+    else{
+    frequency = pedaleFreq;
+    }
+    
+    // Diskrete Ableitung Acceleration
+    acceleration = (frequency - frequencyOld) / dt;
+    
+    // Store old value
+    angleOld = angle;
+    pedaleFreqOld = pedaleFreq;
+    frequencyOld = frequency;
+    
+}
+
+void Encoder::ResetInterrupt(){
+    this->resetOn = 1;
+    this->reset();
+    
+}
+/*
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+Encoder::operator short() {
+    return readAngle();
+}
\ No newline at end of file