servos.cpp 2.82 KB
Newer Older
1
#include "servos.hpp"
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
2
3
4
5
6
7

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdint.h>

#define NUM_SERVOS 3
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
8
9
#define MIN_PULSE 999
#define MAX_PULSE 4999
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

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)
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
46
        servos[servo].tempValue = MIN_PULSE + (int32_t) (arcMin + 180 * 60)  * (MAX_PULSE - MIN_PULSE) / (360 * 60);
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
}

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];
}