Commit 3a87c5ef authored by jp7g21's avatar jp7g21
Browse files

Merge remote-tracking branch 'refs/remotes/origin/master'

parents e8e21420 65bb36a2
#include "servos.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdint.h>
#define NUM_SERVOS 3
static void reorderServos();
static void initTimer1();
struct Servo
{
volatile uint8_t * const port; // the PORT register connected to the servo
volatile uint8_t * const ddr; // the DDR register connected to the servo
const uint8_t pinMask; // the pin mask for the servo
uint16_t tempValue; // stores a temporary value before it's written to the servo
uint16_t value; // current pulse width value
};
static Servo servos[NUM_SERVOS] = {
{ &PORTB, &DDRB, (1 << PB2), 0, 0 },
{ &PORTB, &DDRB, (1 << PB3), 0, 0 },
{ &PORTB, &DDRB, (1 << PB4), 0, 0 }
};
static uint16_t orderedValues[NUM_SERVOS]; // current servo values in ascending order
static uint8_t orderedValueIndex = 0; // index in orderedValues array
static bool updateFlag = false; // used to flag when the servo values have been updated
void initServos()
{
for (Servo &servo : servos)
{
*servo.ddr |= servo.pinMask;
}
initTimer1();
}
void setServoAngle(uint8_t servo, int16_t arcMin)
{
if (0 <= servo && servo < NUM_SERVOS)
servos[servo].tempValue = 2999 + (int32_t) arcMin * 1000 / (180 * 60);
}
void updateServos()
{
updateFlag = true;
}
static void reorderServos()
{
uint16_t minValue, maxValue = 0;
for (Servo &servo : servos)
servo.value = servo.tempValue;
for (uint16_t &value : orderedValues)
{
minValue = (uint16_t) -1;
for (Servo servo : servos)
{
if (servo.value < minValue && servo.value > maxValue)
minValue = servo.value;
}
maxValue = minValue;
value = maxValue;
}
}
static void initTimer1()
{
cli();
TCCR1A = 0;
TCCR1B = (1 << WGM12) // CTC mode, TOP = OCR1A
| (1 << CS11); // 8th prescaler
TCCR1C = 0;
OCR1A = 39999; // set TOP for 50 Hz
TIMSK1 = (1 << OCIE1A) | (1 << OCIE1B); // enable interrupts
sei();
}
ISR(TIMER1_COMPA_vect)
{
if (updateFlag)
{
updateFlag = false;
reorderServos();
}
for (Servo &servo : servos)
*servo.port |= (servo.pinMask); // set servo pin HIGH
orderedValueIndex = 0;
OCR1B = orderedValues[orderedValueIndex];
}
ISR(TIMER1_COMPB_vect)
{
uint16_t currValue = orderedValues[orderedValueIndex];
for (Servo &servo : servos)
{
if (servo.value == currValue)
*servo.port &= ~(servo.pinMask); // set servo pin LOW
}
orderedValueIndex++;
if (orderedValueIndex < NUM_SERVOS)
OCR1B = orderedValues[orderedValueIndex];
}
#ifndef SERVOS_H
#define SERVOS_H
#include <stdint.h>
/**
* Initialises the servos.
*/
void initServos();
/**
* Sets the angle of a servo.
*
* @param servo the index of the servo to control
* @param arcMin the angle of the servo in arc-minutes
*/
void setServoAngle(uint8_t servo, int16_t arcMin);
/**
* Writes the servo values out to the servos.
*/
void updateServos();
#endif /* SERVOS_H */
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment