106 lines
2.3 KiB
C++
106 lines
2.3 KiB
C++
#include "servos.h"
|
|
#include "hal.h"
|
|
#include "roverif.h"
|
|
#include <avr/io.h>
|
|
#include <avr/interrupt.h>
|
|
|
|
/* The timer runs pretty fast to get good enough resolution. The
|
|
prescaler has limited steps, meaning that a 2.0 ms pulse gets very
|
|
close to overflowing an eight bit counter and would need the first
|
|
compare to fire very shortly after the start of the cycle. Instead
|
|
split the pulse into two overflows and run two at once to get a
|
|
fast enough update rate.
|
|
|
|
So:
|
|
* Run two channels at once.
|
|
* Use OCR0A for channel 0.
|
|
* Use OCR0B for channel 1.
|
|
* On overflow, schedule the rising edge.
|
|
* On mid overflow, schedule the falling edge.
|
|
*/
|
|
|
|
const uint8_t Servos::pins_[] = {
|
|
_BV(0), _BV(1), _BV(2), _BV(3),
|
|
};
|
|
|
|
Servos::Servos()
|
|
: at_(0), state_(State::Start) {
|
|
|
|
static_assert(NumChannels % 2 == 0, "Need an even number of channels.");
|
|
|
|
for (volatile int8_t& position : position_) {
|
|
position = Mid;
|
|
}
|
|
}
|
|
|
|
void Servos::init() {
|
|
for (uint8_t pin : pins_) {
|
|
DDRC |= pin;
|
|
}
|
|
|
|
TCCR0A = 0
|
|
| (0 << COM0A0)
|
|
| (0 << COM0B0)
|
|
| (0 << WGM00)
|
|
;
|
|
|
|
static_assert(HAL::Prescaler == 64, "Prescaler disagrees.");
|
|
TCCR0B = 0
|
|
| (0 << WGM02)
|
|
| (3 << CS00)
|
|
;
|
|
|
|
TIMSK0 = _BV(OCIE0A) | _BV(OCIE0B) | _BV(TOIE0);
|
|
}
|
|
|
|
void Servos::set(uint8_t channel, int8_t position) {
|
|
position_[channel] = position;
|
|
}
|
|
|
|
inline void Servos::overflow() {
|
|
uint8_t at = at_;
|
|
|
|
if (state_ == State::Start) {
|
|
at += 2;
|
|
|
|
if (at >= NumChannels) {
|
|
at = 0;
|
|
/* Be conservitative and reset the port each cycle. */
|
|
PORTC = 0;
|
|
}
|
|
|
|
// TODO(michaelh): check that this is cheap.
|
|
OCR0A = -(uint8_t)((position_[at+0] + Bias)/2);
|
|
OCR0B = -(uint8_t)((position_[at+1] + Bias)/2);
|
|
|
|
at_ = at;
|
|
state_ = State::Centre;
|
|
} else {
|
|
OCR0A = (uint8_t)((position_[at+0] + Bias + 1)/2);
|
|
OCR0B = (uint8_t)((position_[at+1] + Bias + 1)/2);
|
|
|
|
state_ = State::Start;
|
|
}
|
|
}
|
|
|
|
inline void Servos::compare_a() {
|
|
PINC = pins_[at_+0];
|
|
}
|
|
|
|
inline void Servos::compare_b() {
|
|
PINC = pins_[at_+1];
|
|
}
|
|
|
|
ISR(TIMER0_OVF_vect) {
|
|
RoverIf::servos.overflow();
|
|
HAL::ticks++;
|
|
}
|
|
|
|
ISR(TIMER0_COMPA_vect) {
|
|
RoverIf::servos.compare_a();
|
|
}
|
|
|
|
ISR(TIMER0_COMPB_vect) {
|
|
RoverIf::servos.compare_b();
|
|
}
|