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/signal.h>

#include "servo.h"

#include "dio.h"

#include "lights.h"

#include "lcd.h"

//16000 c/ms

//i dunno must be 2^k

//9C40 ~ 20ms

//#A000 ~ 20.48 but prolly wont work

#define SERVO_PERIOD 0xA000

//needs be 1ms

//is 1.024ms

#define SERVO_CONSTANT 0x800

/* 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;

        }

}        

//currently the light must be initted b4

void init_servo()

{

        //1010 1001

        TCCR3A = 0;  // COM## set to noninvert, WGM 1:0 set to 1

        //0000 1100

        TCCR3B = 0x02;  // High bits to 0, WGM 3:2 set to 1, prescaler to 8

        lcd_init();

        ETIMSK |= 0x08;

        servo_flag = 1;

        OCR3B = 0x80;

        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, int value)

{

        enable_servo(servo);

        servo_vals[servo] = value;

}