137 lines
2.0 KiB
C++
137 lines
2.0 KiB
C++
#include "rotary_encoder_driver.h"
|
|
#include <avr/interrupt.h>
|
|
|
|
RotaryEncoderDriver* driver = nullptr;
|
|
|
|
RotaryEncoderDriver::RotaryEncoderDriver()
|
|
: cycle(0)
|
|
, count(0)
|
|
, prev_count(0)
|
|
, speed_ready(false)
|
|
, hist_idx(0)
|
|
, led(5)
|
|
, eA(2)
|
|
, eB(5)
|
|
{
|
|
for (uint8_t i = 0; i < HIST_LEN; i++) {
|
|
history[i] = 0;
|
|
}
|
|
}
|
|
|
|
|
|
void RotaryEncoderDriver::begin() {
|
|
driver = this;
|
|
|
|
led.init();
|
|
eA.init();
|
|
eB.init();
|
|
|
|
EICRA |= (1<<ISC01) | (1<<ISC00);
|
|
EIMSK |= (1<<INT0);
|
|
|
|
timer.init(4000);
|
|
}
|
|
|
|
void RotaryEncoderDriver::update() {
|
|
count += eB.is_hi() ? 1 : -1;
|
|
led.set_hi();
|
|
}
|
|
|
|
int RotaryEncoderDriver::position() {
|
|
|
|
uint8_t oldSREG = SREG;
|
|
|
|
cli();
|
|
|
|
int16_t local_count = count;
|
|
|
|
SREG = oldSREG;
|
|
|
|
led.set_lo();
|
|
|
|
return local_count;
|
|
}
|
|
|
|
void RotaryEncoderDriver::update_delta() {
|
|
|
|
int16_t oldest = history[hist_idx];
|
|
history[hist_idx] = count;
|
|
hist_idx = (hist_idx + 1) % HIST_LEN;
|
|
|
|
delta = count - oldest; // breyting yfir HIST_LEN * Ts (40 ms) glugga
|
|
|
|
prev_count = count;
|
|
|
|
speed_ready = true;
|
|
|
|
led.set_lo();
|
|
}
|
|
|
|
float RotaryEncoderDriver::count_per_sec() {
|
|
|
|
uint8_t oldSREG = SREG;
|
|
|
|
cli();
|
|
|
|
int16_t local_delta = delta;
|
|
|
|
SREG = oldSREG;
|
|
|
|
return local_delta / (HIST_LEN * 0.004f);
|
|
}
|
|
|
|
|
|
float RotaryEncoderDriver::rpm()
|
|
{
|
|
const float COUNTS_PER_REV = 1050.0f;
|
|
|
|
return count_per_sec() * 60.0f / COUNTS_PER_REV;
|
|
}
|
|
|
|
bool RotaryEncoderDriver::new_speed_available()
|
|
{
|
|
uint8_t oldSREG = SREG;
|
|
|
|
cli();
|
|
|
|
bool ready = speed_ready;
|
|
|
|
if (ready)
|
|
{
|
|
speed_ready = false;
|
|
}
|
|
|
|
SREG = oldSREG;
|
|
|
|
return ready;
|
|
}
|
|
|
|
|
|
void RotaryEncoderDriver::reset_speed_measurement()
|
|
{
|
|
uint8_t oldSREG = SREG;
|
|
|
|
cli();
|
|
|
|
cycle = 0;
|
|
prev_count = count;
|
|
delta = 0;
|
|
speed_ready = false;
|
|
|
|
for (uint8_t i = 0; i < HIST_LEN; i++) {
|
|
history[i] = count;
|
|
}
|
|
hist_idx = 0;
|
|
|
|
SREG = oldSREG;
|
|
}
|
|
|
|
ISR(INT0_vect) {
|
|
driver->update();
|
|
}
|
|
ISR(TIMER1_COMPA_vect) {
|
|
driver->update_delta();
|
|
}
|
|
|
|
|