Colony

#include "servo.h"

#define SERVO_COUNT 16        //please dont change this, or bad things might happen

struct SERVO_NODE {

   uint8_t num;

   uint16_t angle;

};

struct SERVO_CHANGE {

   uint16_t port_val;

   uint16_t split_time_period;

};

struct SERVO_STATUS_STRUCT {

   struct SERVO_NODE servos[SERVO_COUNT];

   uint16_t servo_angles[SERVO_COUNT];

   struct SERVO_CHANGE changes[SERVO_COUNT+1];

   uint8_t change_count;

   uint16_t ms;

   uint8_t new_angles;

   uint8_t in_pulse;

   uint8_t current_servo;

} SERVO_STATUS;

void servo_sort(void);

void servo_setup_pulse(void);

void servo_set_next_interrupt(uint16_t ticks, struct SERVO_CHANGE *sn);

SIGNAL (SIG_OUTPUT_COMPARE3B){

   if (SERVO_STATUS.in_pulse) {

                //pull the correct ones down

      SERVOPORT1 &= (~SERVOMASK1)|SERVO_STATUS.changes[SERVO_STATUS.current_servo].port_val;

      SERVOPORT2 &= (~SERVOMASK2)|(SERVO_STATUS.changes[SERVO_STATUS.current_servo].port_val >> 8);

      ++SERVO_STATUS.current_servo; //now look at next servo

      if (SERVO_STATUS.current_servo < SERVO_STATUS.change_count) { //if it isnt the end...

                        //setup timer for next pull down

         OCR3B = SERVO_STATUS.changes[SERVO_STATUS.current_servo].split_time_period;

                 TCNT3 = 0;

     }

      else { //we are done with these pulses

//         SERVO_STATUS.in_pulse = 0;

//         SERVO_STATUS.ms = 0;

//         OCR3B += SERVO_MS; //NOTE--this might mean the negative width is a function of max pos width

//                 TCNT3 = 0;

                servo_setup_pulse();

      }

   }

   else { //we are between pulses

      ++SERVO_STATUS.ms;  //increment ms counter

      if (SERVO_STATUS.ms >= SERVO_PERIOD) { //see how many ms have passed

         servo_setup_pulse(); //Seems like we could do this while not on the clock, but who knows

      }

      else {

         OCR3B = SERVO_MS;  //go another ms

                 TCNT3 = 0;

      }

   }

}

void servo_init(void) {

   int i;

   uint8_t mysreg;

   SERVODDR1 |= SERVOMASK1;        //all outputs

   SERVODDR2 |= SERVOMASK2;

        mysreg=SREG;

        cli(); //turn off interrupts for now

        //init everything

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

      SERVO_STATUS.servos[i].num = i;

      SERVO_STATUS.servos[i].angle = 127;

      SERVO_STATUS.servo_angles[i] = 127;

   }

   SERVO_STATUS.ms = 0;

   SERVO_STATUS.new_angles = 1;

   SERVO_STATUS.in_pulse = 0;

   SERVO_STATUS.change_count = 0;

        //init timer3

        TCCR3A = 0;

        TCCR3B = _BV(CS31); //prescale = 8

        TCCR3C=0;

        TCNT3=0;

        ETIMSK |= _BV(OCIE3B);

        OCR3B = SERVO_MS;

        SREG=mysreg;

}

void servo_set_angle(int servo, int angle) {

        uint8_t mysreg;

        if(servo>=8){ //2nd port flipped (on avrxbee at least)

                servo= 23-servo;

        }

   if (SERVO_STATUS.servo_angles[servo] != angle) {

          mysreg=SREG;

          cli();

      SERVO_STATUS.servo_angles[servo] = angle;

      SERVO_STATUS.new_angles = 1;

          SREG=mysreg;

   }

}

void servo_sort(void) {

   int done = 0, i;

   while (! done) {

      done = 1;

      for (i = 0; i < SERVO_COUNT - 1; ++i) {  //loop through all

                        //if they are out of order, swap them

         if (SERVO_STATUS.servos[i].angle > SERVO_STATUS.servos[i+1].angle) {

            SERVO_STATUS.servos[i].angle ^= SERVO_STATUS.servos[i+1].angle;

            SERVO_STATUS.servos[i+1].angle ^= SERVO_STATUS.servos[i].angle;

            SERVO_STATUS.servos[i].angle ^= SERVO_STATUS.servos[i+1].angle;

            SERVO_STATUS.servos[i].num ^= SERVO_STATUS.servos[i+1].num;

            SERVO_STATUS.servos[i+1].num ^= SERVO_STATUS.servos[i].num;

            SERVO_STATUS.servos[i].num ^= SERVO_STATUS.servos[i+1].num;

            done = 0;

         }

      }

   }

}

void servo_setup_pulse(void) {

   int i;

   uint16_t my_port;

   uint16_t sum = 0;

   uint16_t split_time;

   my_port = 0xffff; //all on

   if (SERVO_STATUS.new_angles) {

      SERVO_STATUS.change_count = 0;

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

         SERVO_STATUS.servos[i].angle = SERVO_STATUS.servo_angles[SERVO_STATUS.servos[i].num];

      }

      servo_sort();

      SERVO_STATUS.new_angles = 0;

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

         split_time = /*SERVO_MIN + ((((((long int)(SERVO_MAX-SERVO_MIN)) >> 3) ) *

                                  SERVO_STATUS.servos[i].angle) >> 5) */SERVO_STATUS.servos[i].angle - sum;

         my_port &= ~_BV(SERVO_STATUS.servos[i].num);

         for (; i < SERVO_COUNT - 1 && SERVO_STATUS.servos[i].angle == SERVO_STATUS.servos[i+1].angle; ++i) {

            my_port &= ~_BV(SERVO_STATUS.servos[i+1].num);

         }

         SERVO_STATUS.changes[SERVO_STATUS.change_count].port_val = my_port;

         servo_set_next_interrupt(split_time, &SERVO_STATUS.changes[SERVO_STATUS.change_count]);

                 ++SERVO_STATUS.change_count;

         sum += split_time;

      }

      SERVO_STATUS.changes[SERVO_STATUS.change_count].port_val = my_port;

      servo_set_next_interrupt(SERVO_RESET - split_time, &SERVO_STATUS.changes[SERVO_STATUS.change_count]);

      ++SERVO_STATUS.change_count;

   }

   SERVO_STATUS.current_servo = 0;

   SERVO_STATUS.in_pulse = 1;

   SERVOPORT1 |= SERVOMASK1;

   SERVOPORT2 |= SERVOMASK2;

        OCR3B = SERVO_STATUS.changes[0].split_time_period;

        TCNT3=0;

}

void servo_set_next_interrupt(uint16_t ticks, struct SERVO_CHANGE *sn) {

        //this function is a holdover from when we used a worse clock.

      (*sn).split_time_period = ticks;

}