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

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

Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
7
8
#define MIN_PULSE 999
#define MAX_PULSE 4999
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
9
10
11
12
13
14
15
16
17
18
19

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
20
    int16_t angle;                  // the angle in arc-minutes
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
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 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)
46
47
48
49
50
51
    {
        if (arcMin < -180 * 60)
            arcMin = -180 * 60;
        else if (arcMin > 180 * 60)
            arcMin = 180 * 60;
        
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
52
        servos[servo].tempValue = MIN_PULSE + (int32_t) (arcMin + 180 * 60)  * (MAX_PULSE - MIN_PULSE) / (360 * 60);
53
54
55
56
57
58
59
60
61
62
        servos[servo].angle = arcMin;
    }
}

int16_t getServoAngle(uint8_t servo)
{
    if (0 <= servo && servo < NUM_SERVOS)
        return servos[servo].angle;
    else
        return 0;
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
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
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
}

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