133 lines
1.9 KiB
C++
133 lines
1.9 KiB
C++
#include "rotary_encoder_driver.h"
|
|
#include <avr/interrupt.h>
|
|
|
|
static RotaryEncoderDriver* driver = nullptr;
|
|
|
|
RotaryEncoderDriver::RotaryEncoderDriver()
|
|
: count(0)
|
|
, delta(0)
|
|
, speed_ready(false)
|
|
, hist_idx(0)
|
|
, eA(2)
|
|
, eB(5)
|
|
{
|
|
for (uint8_t i = 0; i < HIST_LEN; i++)
|
|
{
|
|
history[i] = 0;
|
|
}
|
|
}
|
|
|
|
|
|
void RotaryEncoderDriver::begin()
|
|
{
|
|
driver = this;
|
|
|
|
eA.init();
|
|
eB.init();
|
|
|
|
EICRA |= (1<<ISC01) | (1<<ISC00);
|
|
EIMSK |= (1<<INT0);
|
|
|
|
timer.init(SAMPLE_PERIOD_US);
|
|
}
|
|
|
|
void RotaryEncoderDriver::update()
|
|
{
|
|
count += eB.is_hi() ? 1 : -1;
|
|
}
|
|
|
|
int32_t RotaryEncoderDriver::position()
|
|
{
|
|
|
|
uint8_t oldSREG = SREG;
|
|
|
|
cli();
|
|
|
|
int32_t local_count = count;
|
|
|
|
SREG = oldSREG;
|
|
|
|
return local_count;
|
|
}
|
|
|
|
void RotaryEncoderDriver::update_delta()
|
|
{
|
|
|
|
int32_t oldest = history[hist_idx];
|
|
|
|
history[hist_idx] = count;
|
|
|
|
hist_idx = (hist_idx + 1) % HIST_LEN;
|
|
|
|
delta = (int16_t)(count - oldest); // breyting yfir HIST_LEN * Ts (16 ms) glugga
|
|
|
|
speed_ready = true;
|
|
}
|
|
|
|
float RotaryEncoderDriver::count_per_sec()
|
|
{
|
|
|
|
uint8_t oldSREG = SREG;
|
|
|
|
cli();
|
|
|
|
int16_t local_delta = delta;
|
|
|
|
SREG = oldSREG;
|
|
|
|
return local_delta / (HIST_LEN * SAMPLE_PERIOD_US * 1e-6f);
|
|
}
|
|
|
|
|
|
float RotaryEncoderDriver::rpm()
|
|
{
|
|
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();
|
|
|
|
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();
|
|
}
|