Encoder.cpp

00001 #include "Encoder.h"
00002 
00003 using namespace std;
00004 
00005 
00006 Encoder::Encoder(PinName& hallsensor) : HallSensor(hallsensor){        
00007         
00008         TIM = TIM3;
00009         
00010         // configure reset and clock control registers
00011         
00012         RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;    // manually enable port B (port A enabled by mbed library)
00013         
00014         // configure general purpose I/O registers
00015         
00016         GPIOA->MODER &= ~GPIO_MODER_MODER6;     // reset port A6
00017         GPIOA->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port A6
00018         GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port A6
00019         GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
00020         GPIOA->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port A6
00021         GPIOA->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port A6
00022         
00023         GPIOB->MODER &= ~GPIO_MODER_MODER5;     // reset port B5
00024         GPIOB->MODER |= GPIO_MODER_MODER5_1;    // set alternate mode of port B5
00025         GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR5;     // reset pull-up/pull-down on port B5
00026         GPIOB->PUPDR |= GPIO_PUPDR_PUPDR5_1;    // set input as pull-down
00027         GPIOB->AFR[0] &= ~0xF0000000;           // reset alternate function of port B5
00028         GPIOB->AFR[0] |= 2 << 4*5;              // set alternate funtion 2 of port B5
00029         
00030         // configure reset and clock control registers
00031         
00032         RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST;  //reset TIM3 controller
00033         RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
00034         
00035         RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;     // TIM3 clock enable
00036         
00037         TIM->CR1 = 0x0000;          // counter disable
00038         TIM->CR2 = 0x0000;          // reset master mode selection
00039         TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges   
00040         TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
00041         TIM->CCMR2 = 0x0000;        // reset capture mode register 2
00042         TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
00043         TIM->CNT = 0x0000;          // reset counter value
00044         TIM->ARR = 0xBF68;          // auto reload register (49000)
00045         TIM->PSC = 0x0003;          // divide count by 4
00046         TIM->CR1 = TIM_CR1_CEN;     // counter enable  
00047         
00048         // Interrupt for Origin Position
00049         HallSensor.fall(callback(this, &Encoder::ResetInterrupt));
00050         this->resetOn = 0;
00051         
00052         // Ticker for the calculation of the frequency with dt = 5ms
00053         this->ticker.attach(callback(this, &Encoder::calculateFrequency),dt);
00054            
00055 }
00056 
00057 Encoder::~Encoder() {
00058     ticker.detach();
00059 }
00060 
00061 uint8_t Encoder::reset() {
00062     static int resetted=0;
00063     if(this->resetOn==1){
00064        TIM->CNT = 49000;
00065        HallSensor.disable_irq();
00066        this->resetOn = 0;
00067        resetted = 1;
00068     }
00069     return resetted;
00070 }
00071 
00072 /**
00073  * Reads the quadrature encoder counter value.
00074  * @return the quadrature encoder counter as a signed 16-bit integer value.
00075  */
00076 uint32_t Encoder::read() {
00077     
00078     return (uint16_t)49000-TIM->CNT; // Trasform Downcounter in Upcounter
00079 }
00080 /*
00081  * @return the Angle as a float value
00082  */
00083 float Encoder::readAngle() {
00084     uint32_t pulses;
00085     float angle;
00086     
00087     pulses = this->read();
00088     angle = 2.0f * PI * pulses / 49000.0f; // 49000 Pulses per Rotation
00089     return angle;  
00090 }
00091 
00092 /*
00093  * @return the Frequency as a float value in rad/s
00094  */
00095 float Encoder::readFrequency(){
00096     return frequency;    
00097 }
00098 
00099 /*
00100  * @return the Frequency as a float value in rad/s
00101  */
00102 float Encoder::readAcceleration(){
00103     return acceleration;    
00104 }
00105 
00106 /*
00107  * @return the Frequency as a float value in rad/s
00108  */
00109 float Encoder::readRPM(){
00110     return frequency / (2.0*PI) * 60.0;    
00111 }
00112 
00113 /*
00114  * Calculate the pedal frequency every 5ms
00115  */
00116 void Encoder::calculateFrequency(){
00117     static float angle, angleOld = 0.0f;
00118     static float pedaleFreq, pedaleFreqOld = 0.0f, frequencyOld = 0.0f;
00119     static float accelerationOld = 0.0f;
00120     
00121     // Read actual angle
00122     angle = this->readAngle();
00123     
00124     // Diskrete Ableitung Frequenz
00125     pedaleFreq = (angle - angleOld) / dt;
00126    
00127           
00128     // Filter Nulldurchgang mit der Messung der Winkels und Frequenz Grenz [-2.5,2.5]rad/s
00129     if(((pedaleFreq - pedaleFreqOld) > 2.5) || ((pedaleFreq - pedaleFreqOld) < -2.5f) ){
00130     frequency = frequencyOld;
00131     } 
00132     else{
00133     frequency = pedaleFreq;
00134     }
00135     
00136     // Diskrete Ableitung Acceleration
00137     acceleration = (frequency - frequencyOld) / dt;
00138     
00139     // Store old value
00140     angleOld = angle;
00141     pedaleFreqOld = pedaleFreq;
00142     frequencyOld = frequency;
00143     
00144 }
00145 
00146 void Encoder::ResetInterrupt(){
00147     this->resetOn = 1;
00148     this->reset();
00149     
00150 }
00151 /*
00152  * The empty operator is a shorthand notation of the <code>read()</code> method.
00153  */
00154 Encoder::operator short() {
00155     return readAngle();
00156 }