Skip to content
Snippets Groups Projects
Select Git revision
  • e13ded23dd9faa4062e20d4297ab9a9f196384e0
  • master default protected
2 results

servos.cpp

  • servos.cpp 3.19 KiB
    #include "servos.hpp"
    
    #include <avr/io.h>
    #include <avr/interrupt.h>
    #include <stdint.h>
    
    #define MIN_PULSE 999
    #define MAX_PULSE 4999
    
    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
        int16_t angle;                  // the angle in arc-minutes
    };
    
    static Servo servos[NUM_SERVOS] = {
        { &PORTC, &DDRC, (1 << PB0), 0, 0 },
        { &PORTC, &DDRC, (1 << PB1), 0, 0 },
        { &PORTC, &DDRC, (1 << PB2), 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)
        {
            if (arcMin < -180 * 60)
                arcMin = -180 * 60;
            else if (arcMin > 180 * 60)
                arcMin = 180 * 60;
            
            servos[servo].tempValue = MIN_PULSE + (int32_t) (arcMin + 180 * 60)  * (MAX_PULSE - MIN_PULSE) / (360 * 60);
            servos[servo].angle = arcMin;
        }
    }
    
    int16_t getServoAngle(uint8_t servo)
    {
        if (0 <= servo && servo < NUM_SERVOS)
            return servos[servo].angle;
        else
            return 0;
    }
    
    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];
    }