eBike / Mbed 2 deprecated ENCODER_TEST3_peddep

ti bisogna il phaserunner

Dependencies:   mbed PID mbed-rtos

Peripherien/Encoder.cpp@11:39bd79605827, 2019-06-04 (annotated)

Committer:
beacon
Date:
Tue Jun 04 19:03:39 2019 +0000
Revision:
11:39bd79605827
Parent:
9:56aed8c6779f 
ti bisogna il phaserunner

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EpicG10 7:15e6fc689368 1 #include "Encoder.h"
EpicG10 7:15e6fc689368 2
EpicG10 7:15e6fc689368 3 using namespace std;
EpicG10 7:15e6fc689368 4
EpicG10 7:15e6fc689368 5
EpicG10 7:15e6fc689368 6 Encoder::Encoder(PinName& hallsensor) : HallSensor(hallsensor){
EpicG10 7:15e6fc689368 7
EpicG10 7:15e6fc689368 8 TIM = TIM3;
EpicG10 7:15e6fc689368 9
EpicG10 7:15e6fc689368 10 // configure reset and clock control registers
EpicG10 7:15e6fc689368 11
EpicG10 7:15e6fc689368 12 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // manually enable port B (port A enabled by mbed library)
EpicG10 7:15e6fc689368 13
EpicG10 7:15e6fc689368 14 // configure general purpose I/O registers
EpicG10 7:15e6fc689368 15
EpicG10 7:15e6fc689368 16 GPIOA->MODER &= ~GPIO_MODER_MODER6; // reset port A6
EpicG10 7:15e6fc689368 17 GPIOA->MODER |= GPIO_MODER_MODER6_1; // set alternate mode of port A6
EpicG10 7:15e6fc689368 18 GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6; // reset pull-up/pull-down on port A6
EpicG10 7:15e6fc689368 19 GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1; // set input as pull-down
EpicG10 7:15e6fc689368 20 GPIOA->AFR[0] &= ~(0xF << 4*6); // reset alternate function of port A6
EpicG10 7:15e6fc689368 21 GPIOA->AFR[0] |= 2 << 4*6; // set alternate funtion 2 of port A6
EpicG10 7:15e6fc689368 22
EpicG10 7:15e6fc689368 23 GPIOB->MODER &= ~GPIO_MODER_MODER5; // reset port B5
EpicG10 7:15e6fc689368 24 GPIOB->MODER |= GPIO_MODER_MODER5_1; // set alternate mode of port B5
EpicG10 7:15e6fc689368 25 GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR5; // reset pull-up/pull-down on port B5
EpicG10 7:15e6fc689368 26 GPIOB->PUPDR |= GPIO_PUPDR_PUPDR5_1; // set input as pull-down
EpicG10 7:15e6fc689368 27 GPIOB->AFR[0] &= ~0xF0000000; // reset alternate function of port B5
EpicG10 7:15e6fc689368 28 GPIOB->AFR[0] |= 2 << 4*5; // set alternate funtion 2 of port B5
EpicG10 7:15e6fc689368 29
EpicG10 7:15e6fc689368 30 // configure reset and clock control registers
EpicG10 7:15e6fc689368 31
EpicG10 7:15e6fc689368 32 RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST; //reset TIM3 controller
EpicG10 7:15e6fc689368 33 RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
EpicG10 7:15e6fc689368 34
EpicG10 7:15e6fc689368 35 RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // TIM3 clock enable
EpicG10 7:15e6fc689368 36
EpicG10 7:15e6fc689368 37 TIM->CR1 = 0x0000; // counter disable
EpicG10 7:15e6fc689368 38 TIM->CR2 = 0x0000; // reset master mode selection
EpicG10 7:15e6fc689368 39 TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges
EpicG10 7:15e6fc689368 40 TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
EpicG10 7:15e6fc689368 41 TIM->CCMR2 = 0x0000; // reset capture mode register 2
EpicG10 7:15e6fc689368 42 TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
EpicG10 7:15e6fc689368 43 TIM->CNT = 0x0000; // reset counter value
EpicG10 7:15e6fc689368 44 TIM->ARR = 0xBF68; // auto reload register (49000)
EpicG10 7:15e6fc689368 45 TIM->PSC = 0x0003; // divide count by 4
EpicG10 7:15e6fc689368 46 TIM->CR1 = TIM_CR1_CEN; // counter enable
EpicG10 7:15e6fc689368 47
EpicG10 7:15e6fc689368 48 // Interrupt for Origin Position
EpicG10 7:15e6fc689368 49 HallSensor.fall(callback(this, &Encoder::ResetInterrupt));
EpicG10 7:15e6fc689368 50 this->resetOn = 0;
EpicG10 7:15e6fc689368 51
EpicG10 9:56aed8c6779f 52 // Ticker for the calculation of the frequency with dt = 5ms
EpicG10 7:15e6fc689368 53 this->ticker.attach(callback(this, &Encoder::calculateFrequency),dt);
EpicG10 7:15e6fc689368 54
EpicG10 7:15e6fc689368 55 }
EpicG10 7:15e6fc689368 56
EpicG10 7:15e6fc689368 57 Encoder::~Encoder() {
EpicG10 7:15e6fc689368 58 ticker.detach();
EpicG10 7:15e6fc689368 59 }
EpicG10 7:15e6fc689368 60
EpicG10 7:15e6fc689368 61 uint8_t Encoder::reset() {
EpicG10 7:15e6fc689368 62 static int resetted=0;
EpicG10 7:15e6fc689368 63 if(this->resetOn==1){
EpicG10 7:15e6fc689368 64 TIM->CNT = 49000;
EpicG10 7:15e6fc689368 65 HallSensor.disable_irq();
EpicG10 7:15e6fc689368 66 this->resetOn = 0;
EpicG10 7:15e6fc689368 67 resetted = 1;
EpicG10 7:15e6fc689368 68 }
EpicG10 7:15e6fc689368 69 return resetted;
EpicG10 7:15e6fc689368 70 }
EpicG10 7:15e6fc689368 71
EpicG10 7:15e6fc689368 72 /**
EpicG10 7:15e6fc689368 73 * Reads the quadrature encoder counter value.
EpicG10 7:15e6fc689368 74 * @return the quadrature encoder counter as a signed 16-bit integer value.
EpicG10 7:15e6fc689368 75 */
EpicG10 7:15e6fc689368 76 uint32_t Encoder::read() {
EpicG10 7:15e6fc689368 77
EpicG10 7:15e6fc689368 78 return (uint16_t)49000-TIM->CNT; // Trasform Downcounter in Upcounter
EpicG10 7:15e6fc689368 79 }
EpicG10 7:15e6fc689368 80 /*
EpicG10 7:15e6fc689368 81 * @return the Angle as a float value
EpicG10 7:15e6fc689368 82 */
EpicG10 7:15e6fc689368 83 float Encoder::readAngle() {
EpicG10 7:15e6fc689368 84 uint32_t pulses;
EpicG10 7:15e6fc689368 85 float angle;
EpicG10 7:15e6fc689368 86
EpicG10 7:15e6fc689368 87 pulses = this->read();
EpicG10 7:15e6fc689368 88 angle = 2.0f * PI * pulses / 49000.0f; // 49000 Pulses per Rotation
EpicG10 7:15e6fc689368 89 return angle;
EpicG10 7:15e6fc689368 90 }
EpicG10 7:15e6fc689368 91
EpicG10 7:15e6fc689368 92 /*
EpicG10 7:15e6fc689368 93 * @return the Frequency as a float value in rad/s
EpicG10 7:15e6fc689368 94 */
EpicG10 7:15e6fc689368 95 float Encoder::readFrequency(){
EpicG10 7:15e6fc689368 96 return frequency;
EpicG10 7:15e6fc689368 97 }
EpicG10 7:15e6fc689368 98
EpicG10 7:15e6fc689368 99 /*
EpicG10 7:15e6fc689368 100 * @return the Frequency as a float value in rad/s
EpicG10 7:15e6fc689368 101 */
EpicG10 7:15e6fc689368 102 float Encoder::readAcceleration(){
EpicG10 7:15e6fc689368 103 return acceleration;
EpicG10 7:15e6fc689368 104 }
EpicG10 7:15e6fc689368 105
EpicG10 7:15e6fc689368 106 /*
EpicG10 7:15e6fc689368 107 * @return the Frequency as a float value in rad/s
EpicG10 7:15e6fc689368 108 */
EpicG10 7:15e6fc689368 109 float Encoder::readRPM(){
EpicG10 7:15e6fc689368 110 return frequency / (2.0*PI) * 60.0;
EpicG10 7:15e6fc689368 111 }
EpicG10 7:15e6fc689368 112
EpicG10 7:15e6fc689368 113 /*
EpicG10 7:15e6fc689368 114 * Calculate the pedal frequency every 5ms
EpicG10 7:15e6fc689368 115 */
EpicG10 7:15e6fc689368 116 void Encoder::calculateFrequency(){
EpicG10 7:15e6fc689368 117 static float angle, angleOld = 0.0f;
EpicG10 7:15e6fc689368 118 static float pedaleFreq, pedaleFreqOld = 0.0f, frequencyOld = 0.0f;
EpicG10 7:15e6fc689368 119 static float accelerationOld = 0.0f;
EpicG10 7:15e6fc689368 120
EpicG10 7:15e6fc689368 121 // Read actual angle
EpicG10 7:15e6fc689368 122 angle = this->readAngle();
EpicG10 7:15e6fc689368 123
EpicG10 7:15e6fc689368 124 // Diskrete Ableitung Frequenz
EpicG10 7:15e6fc689368 125 pedaleFreq = (angle - angleOld) / dt;
EpicG10 7:15e6fc689368 126
EpicG10 7:15e6fc689368 127
EpicG10 7:15e6fc689368 128 // Filter Nulldurchgang mit der Messung der Winkels und Frequenz Grenz [-2.5,2.5]rad/s
EpicG10 7:15e6fc689368 129 if(((pedaleFreq - pedaleFreqOld) > 2.5) || ((pedaleFreq - pedaleFreqOld) < -2.5f) ){
EpicG10 7:15e6fc689368 130 frequency = frequencyOld;
EpicG10 7:15e6fc689368 131 }
EpicG10 7:15e6fc689368 132 else{
EpicG10 7:15e6fc689368 133 frequency = pedaleFreq;
EpicG10 7:15e6fc689368 134 }
EpicG10 7:15e6fc689368 135
EpicG10 7:15e6fc689368 136 // Diskrete Ableitung Acceleration
EpicG10 7:15e6fc689368 137 acceleration = (frequency - frequencyOld) / dt;
EpicG10 7:15e6fc689368 138
EpicG10 7:15e6fc689368 139 // Store old value
EpicG10 7:15e6fc689368 140 angleOld = angle;
EpicG10 7:15e6fc689368 141 pedaleFreqOld = pedaleFreq;
EpicG10 7:15e6fc689368 142 frequencyOld = frequency;
EpicG10 7:15e6fc689368 143
EpicG10 7:15e6fc689368 144 }
EpicG10 7:15e6fc689368 145
EpicG10 7:15e6fc689368 146 void Encoder::ResetInterrupt(){
EpicG10 7:15e6fc689368 147 this->resetOn = 1;
EpicG10 7:15e6fc689368 148 this->reset();
EpicG10 7:15e6fc689368 149
EpicG10 7:15e6fc689368 150 }
EpicG10 7:15e6fc689368 151 /*
EpicG10 7:15e6fc689368 152 * The empty operator is a shorthand notation of the <code>read()</code> method.
EpicG10 7:15e6fc689368 153 */
EpicG10 7:15e6fc689368 154 Encoder::operator short() {
EpicG10 7:15e6fc689368 155 return readAngle();
EpicG10 7:15e6fc689368 156 }