Colony

/*

  lights.c

  Controls orb1 and orb2. Also contains the framework for a software PWM that may be used for servos in the future.

  author: CMU Robotics Club, Colony Project

  Change Log:

  2.4.07 - Aaron

        Revamped orb code so it works.  Need to check interaction with rtc, and tweak some colors.

  2.1.07 - James

        Modified sort_buffer() to prune for repeats.  PWM now uses orb_buf_size for the number of orb values in orb_time_arr[].

  Changed sorting algorithm used in sort_buffer() to selection sort (faster). And it works now.

  1.25.07 - KWoo

        Deleted old FF+ code to make it cleaner. Commented code. This all works. Note however that if you ever plan to use the

  software PWM (which is this) you will need to change the implementation of orb_enable() and orb_disable() to not

  shutdown the PWM.

*/

#include "lights.h"

#include "dragonfly_lib.h"

#define ORB_COUNT 8        //please dont change this, or bad things might happen

struct ORB_NODE {

  uint8_t num;

  uint16_t angle;

};

struct ORB_CHANGE {

  uint16_t port_val;

  uint16_t split_time_period;

};

struct ORB_STATUS_STRUCT {

  struct ORB_NODE orbs[ORB_COUNT];

  uint16_t orb_angles[ORB_COUNT];

  struct ORB_CHANGE changes[ORB_COUNT+1];

  uint8_t change_count;

  uint8_t new_angles;

  uint8_t current_orb;

} ORB_STATUS;

void orb_sort(void);

void orb_setup_pulse(void);

SIGNAL (SIG_OUTPUT_COMPARE3C){

  //pull the correct ones down

  ORBPORT &= (~ORBMASK)|(ORB_STATUS.changes[ORB_STATUS.current_orb].port_val);

  ++ORB_STATUS.current_orb; //now look at next orb transition

  if (ORB_STATUS.current_orb < ORB_STATUS.change_count) { //if it isnt the end...

    //setup timer for next pull down

    OCR3C = TCNT3+ORB_STATUS.changes[ORB_STATUS.current_orb].split_time_period;

  }

  else { //we are done with these pulses

                orb_setup_pulse();

  }

}

//sets a channel to a value

void orb_set_angle(int orb, int angle) {

        uint8_t mysreg;

        orb=orb&0x07; //only have 8

        angle=angle&0xff; //only accept 0-255

        angle=255-angle; //inverse intensity

        angle=angle<<2; //scale up so that we dont run it too often

        angle+=3; //0 values dont really work

  if (ORB_STATUS.orb_angles[orb] != angle) { //if the angle has changed

          mysreg=SREG; 

          cli(); //disable interrupts

    ORB_STATUS.orb_angles[orb] = angle; //update angle

    ORB_STATUS.new_angles = 1;

          SREG=mysreg; //put interrupt status back

  }

}

void orb_sort(void) {

  int done = 0, i;

  while (! done) {

    done = 1;

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

                        //if they are out of order, swap them

      if (ORB_STATUS.orbs[i].angle > ORB_STATUS.orbs[i+1].angle) {

        ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;

        ORB_STATUS.orbs[i+1].angle ^= ORB_STATUS.orbs[i].angle;

        ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;

        ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;

        ORB_STATUS.orbs[i+1].num ^= ORB_STATUS.orbs[i].num;

        ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;

        done = 0;

      }

    }

  }

}

//calculate the split times

void orb_setup_pulse(void) {

  int i;

  uint16_t my_port;

  uint16_t sum = 0;

  uint16_t split_time;

  my_port = 0xff; //all on

  if (ORB_STATUS.new_angles) {

    ORB_STATUS.change_count = 0;

          for (i = 0; i < ORB_COUNT; ++i) { //get the new values

      ORB_STATUS.orbs[i].angle = ORB_STATUS.orb_angles[ORB_STATUS.orbs[i].num];

    }

    orb_sort(); //sort them

    ORB_STATUS.new_angles = 0;

    for (i = 0; i < ORB_COUNT; ++i) { //calculate split times

      split_time = ORB_STATUS.orbs[i].angle - sum;

      my_port &= ~_BV(ORB_STATUS.orbs[i].num);

      for (; i < ORB_COUNT - 1 && ORB_STATUS.orbs[i].angle == ORB_STATUS.orbs[i+1].angle; ++i) {

        my_port &= ~_BV(ORB_STATUS.orbs[i+1].num); //look for doups

      }

      ORB_STATUS.changes[ORB_STATUS.change_count].port_val = my_port; //which pins are low

      ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = split_time;

      ++ORB_STATUS.change_count;

      sum += split_time;

    }

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

    ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = ORB_RESET - sum; //get a constant period

    ++ORB_STATUS.change_count;

  }

  ORB_STATUS.current_orb = 0;

  ORBPORT |= ORBMASK; //start with all high

        OCR3C = TCNT3 + ORB_STATUS.changes[0].split_time_period; //wait for first split

}

//Initializes the PWM for Orb control. This must be called before the Orbs are used for them to function.

void orb_init() 

{        

  int i;

  uint8_t mysreg;

  ORBDDR |= ORBMASK;        //all outputs

        mysreg=SREG;

        cli(); //turn off interrupts for now

        //init everything

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

    ORB_STATUS.orbs[i].num = i;

    ORB_STATUS.orbs[i].angle = 1023;        //127 is a pretty stupid start angle, but oh well

    ORB_STATUS.orb_angles[i] = 1023;

  }

  ORB_STATUS.new_angles = 1;

  ORB_STATUS.change_count = 0;

        //init timer3

        TCCR3A = 0; 

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

        TCCR3C = 0;

        ETIMSK |= _BV(OCIE3C); //turn on oc3c interrupt

        OCR3C = TCNT3+ORB_RESET;

        SREG=mysreg;

}

//Set both Orbs (1 and 2) to the same color as specified

void orb_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {

        orb1_set(red_led,green_led,blue_led);

        orb2_set(red_led,green_led,blue_led);

}

//Sets Orb1 to the specified color

void orb1_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {

        orb_set_angle(0,red_led);

        orb_set_angle(1,green_led);

        orb_set_angle(2,blue_led);

}

//Sets Orb2 to the specified color

void orb2_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {        

        orb_set_angle(4,red_led);

        orb_set_angle(5,green_led);

        orb_set_angle(6,blue_led);

}

//Sets both orbs to the same color using the color defines

void orb_set_color(int col)

{

  int red, green, blue;

  red = ((col & 0xE0) >> 5) * 36;

  green = ((col & 0x1C) >> 2) * 36;

  blue = (col & 0x03) * 85;

  orb_set(red, green, blue);

}

//Sets both orbs to the same color using the color defines

void orb1_set_color(int col)

{

  int red, green, blue;

  red = ((col & 0xE0) >> 5) * 36;

  green = ((col & 0x1C) >> 2) * 36;

  blue = (col & 0x03) * 85;

  orb1_set(red, green, blue);

}

//Sets both orbs to the same color using the color defines

void orb2_set_color(int col)

{

  int red, green, blue;

  red = ((col & 0xE0) >> 5) * 36;

  green = ((col & 0x1C) >> 2) * 36;

  blue = (col & 0x03) * 85;

  orb2_set(red, green, blue);

}

//DOES THIS WORK?

// Disables the timer1 interrupt, disabling the Orb's color fading capabilities

// You can still turn the red, green, and blue leds on and off with set_orb_dio

/* If we use the PWM for anything else besides the ORB, this implementation needs to be done better */

void orb_disable()

{

        TCCR3B &= 0;          //Turn off everything

        ORB_PORT |= _BV(ORB1_RED);

        ORB_PORT |= _BV(ORB1_GREEN);

        ORB_PORT |= _BV(ORB1_BLUE);

        ORB_PORT |= _BV(ORB2_RED);

        ORB_PORT |= _BV(ORB2_GREEN);

        ORB_PORT |= _BV(ORB2_BLUE);

}

//DOES THIS WORK?

// Enables the timer1 interrupt, enabling the Orb's color fading capabilities

void orb_enable()

{

//        TCCR0 |= _BV(COM01) | _BV(COM00)  | _BV(WGM00) | _BV(CS01);        //Toggle OC Pin on match, FAST PWM Mode, clock/8

        TCCR3B =_BV(CS31);

}

void orb_tester()

{

        orb_init();

        lcd_init();

        sei();

        while(1)

        {

                orb_set_color(RED);

                //orb_set1(128,128,128);

                //orb_set2(1,56,255);

                delay_ms(100);

                for (int i = 0; i < ORB_COUNT; i++) {

                        lcd_putint(ORB_STATUS.changes[ORB_STATUS.current_orb].split_time_period>>3);

                        lcd_putchar(' ');

                }

                delay_ms(2000);

                lcd_clear_screen();

                orb_set_color(GREEN);

                //orb_set1(128,128,128);

                //orb_set2(128,128,128);

                delay_ms(100);

                for (int i = 0; i < ORB_COUNT; i++) {

                        lcd_putint(ORB_STATUS.changes[ORB_STATUS.current_orb].split_time_period>>3);

                        lcd_putchar(' ');

                }

                delay_ms(2000);

                lcd_clear_screen();

        }

}