servos.cpp 2.74 KB
Newer Older
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
1
2
3
4
5
6
7
8
9
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
46
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
#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];
}