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"

#include <avr/interrupt.h>

#define ORB_RESET 1025

#define ORBPORT PORTC

#define ORBDDR DDRC

#define ORBMASK 0x77

/***** Port and Pin Definitions ****/

//Orb Ports and Registers

#define ORB_PORT        PORTC

#define ORB_DDR         DDRC

//Orb Pins

#define ORB1_RED        0x00

#define ORB1_GREEN      0x01

#define ORB1_BLUE       0x02

#define ORB2_RED        0x04

#define ORB2_GREEN      0x05

#define ORB2_BLUE       0x06

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

// an orb node

struct ORB_NODE {

   uint8_t num;

   uint16_t angle;

};

//the change in an orb

struct ORB_CHANGE {

   uint16_t port_val;

   uint16_t split_time_period;

};

// the status of an orb

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

}

/**

 * @defgroup orbs Orbs

 * @brief Functions for controlling the color of the orbs.

 *

 * Functions for controlling the color and lighting of the orbs.

 *

 * @{

 **/

/**

 * 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 to the color specified. orb_init must

 * be called before this function may be used.

 *

 * @param red_led the red component of the color

 * @param green_led the green component of the color

 * @param blue_led the blue component of the color

 *

 * @see orb_init

 **/

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);

}

/**

 * Set orb1 to the color specified. orb_init must

 * be called before this function may be used.

 *

 * @param red_led the red component of the color

 * @param green_led the green component of the color

 * @param blue_led the blue component of the color

 *

 * @see orb_init

 **/

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);

}

/**

 * Set orb2 to the color specified. orb_init must

 * be called before this function may be used.

 *

 * @param red_led the red component of the color

 * @param green_led the green component of the color

 * @param blue_led the blue component of the color

 *

 * @see orb_init

 **/

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);

}

/**

 * Set both orbs to the specified color. This function

 * is intended to be used with the predefined

 * colors. orb_init must be called before this

 * function may be used.

 *

 * @param col the color to set the orbs to

 *

 * @see orb_init

 **/

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);

}

/**

 * Set orb1 to the specified color. This function

 * is intended to be used with the predefined

 * colors. orb_init must be called before this

 * function may be used.

 *

 * @param col the color to set the orbs to

 *

 * @see orb_init

 **/

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);

}

/**

 * Set orb2 to the specified color. This function

 * is intended to be used with the predefined

 * colors. orb_init must be called before this

 * function may be used.

 *

 * @param col the color to set the orbs to

 *

 * @see orb_init

 **/

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

/**

 * Disables the orb color fading capabilities

 * by disabling the timer1 interrupt.

 *

 * @see orb_init

 **/

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

/**

 * Enables the orb's color fading capabilities.

 *

 * @see orb_init

 **/

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);

}

/** @} **/ //end group