Colony

/*

        servo.c - Contains functions and interrupts necessary to activate servos

        author:  Tom Lauwers

        3/1/06 Iain

                Man, I don't know what a servo is.

*/

#include <avr/io.h>

#include <avr/interrupt.h>

#include <servo.h>

#include <dio.h>

#include <lights.h>

#include <lcd.h>

//1677.7216 c/ms

//i dunno must be 2^k

//250

//9C40 ~ 20ms

//#A000 ~ 20.48 but prolly wont work

//div by 256: 6.55078125 c/ms 80 ~ 19.5ms

#define SERVO_PERIOD 0x80

//needs be 1ms

//is 1.22ms

#define SERVO_CONSTANT 0x8

/* dirty dirty

unsigned int servo_vals[4] = {0,0,0,0}; // Stores the set servo values

int current_servo = 0;  // Stores which servo is current in the interrupt routine

*/

unsigned int servo_vals[8] = {0,0,0,0,0,0,0,0}; // Stores the set servo values

int phase_time=0;

int phase_base=0;

int servos_on = 0;      // Stores which servos are enabled

int servos_enabled=0; //which are high

int servo_flag = 0;     // Stores whether or not servos are enabled

/* Timer 1 output compare B interrupt.  Does the following:

        Checks to see if the current servo is 4, in which case just need

        to set low servo 3's signal and return.

        Else, check to see if the current servo is enabled, and set it's signal high

        Then, set the previous servo's signal low

        Now, set the next output compare to occur for some time from now

        specified by the current servo's value

*/

SIGNAL (SIG_OUTPUT_COMPARE3B)

{

        lcd_putchar('i');

        if(servo_flag == 0)

                return;

        int i;

        if(phase_time == 0){

                lcd_putchar('p');

                for(i=0;i<8;i++){

                        if(servos_on & _BV(i))

                                digital_output((_PORT_E << 3) +i,1);

                }

                servos_enabled = servos_on;

                phase_base = OCR3B;

        }

        unsigned int min=~0;

        for(i=0;i<8;i++){

                if(_BV(i) & servos_enabled){

                        if(SERVO_CONSTANT*servo_vals[i] <= phase_time){

                                digital_output((_PORT_E << 3) + i,0);

                                servos_enabled &= ~_BV(i);

                        }

                        else if(SERVO_CONSTANT*servo_vals[i] < min) {

                                min = servo_vals[i];

                        }

                }

        }

        if(!servos_enabled){

                OCR3B = phase_base + SERVO_PERIOD;

                phase_time = 0;

        }

        else {

                OCR3B = phase_base + min;

                phase_time = min;

        }

}

void init_servo()

{

        //1010 1001

        TCCR3A = 0;

        //0000 0100

        TCCR3B = 0x4;  //prescaler to 256

        TCCR3C=0;

        lcd_init();

        ETIMSK |= 0x08;

        servo_flag = 1;

        OCR3B = 0x0;

        lcd_putchar('i');

}

// Enable a servo specified by config

void enable_servo(int config)

{

        servos_on |= _BV(config);

}

// Disable a servo specified by config

void disable_servo(int config)

{

        servos_on &= (0xFF-_BV(config));

}

// Disables the timer1 interrupt, disabling the servos

void disable_servos()

{

        // Disables interrupts

        ETIMSK &= ~0x08;

        servo_flag=0;

}

// Enables the timer1 interrupt, enabling the servos

void enable_servos()

{

        // Enable interrupts on output compare B

        ETIMSK |= 0x08;

        servo_flag=1;

}

// Set a servo to a value.  Automatically enables the servo.

void set_servo(int servo, unsigned int value)

{

        enable_servo(servo);

        servo_vals[servo] = value;

}