ti bisogna il phaserunner
Dependencies: mbed PID mbed-rtos
Peripherien/Encoder.cpp@7:15e6fc689368, 2019-05-16 (annotated)
- Committer:
- EpicG10
- Date:
- Thu May 16 20:42:39 2019 +0000
- Revision:
- 7:15e6fc689368
- Child:
- 9:56aed8c6779f
Implementation Regler (not finished)
Who changed what in which revision?
User | Revision | Line number | New 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 | 7:15e6fc689368 | 52 | // Ticker for the calculation of the frequency wiht 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 | } |