Colony

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file ligths.c

 * @brief Orbs

 *

 * Implemenation for the orbs (tri-colored LEDs)

 *

 * @author Colony Project, CMU Robotics Club

 * @bug Colors are incorrect, seems to not work with wireless library

 **/

/*

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

}

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;

}

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

}

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

}

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

}

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

}

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

}

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

}