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 Unfinished

 **/

/*

lights.c

Controls orb1 and orb2. Can be extended for a software PWM that may be used

for servos in the future.

author: CMU Robotics Club, Colony Project

Change Log:

3/31/2009 - Martin

    Rewritten from scratch (mostly), fixes some code duplication, long ISRs,

        bugs, unnecessary synchronized code, memory waste

*/

/*

Operation:

On timer overflow:

  - switch on LEDs (where value>0, according to a pre-determined mask)

  - load the first output compare value

At compare match:

  - switch off LEDs (according to mask)

  - load the next output compare value

 */

/*

  TODO:

    - do the inversion of flags when computing then, not in the ISR

        - do the timing right

        - handle 0 values (use the pwm_init_mask)

        - Determine sorting time, possibly improve sorting algorithm

 */

/*

 * Random notes:

 *   - Current motor frequency is 32 us/30 KHz

 *   - AVR suckage: there is not timer mode with immediate OCR update and 

 *     overflow interrupt at TOP (CTC value)

 *   - AVR suckage: Set on compare match/Clear on overflow not available with

 *     non-PWM modes (especially not with immediate OCR update)

 *   - Frequency is 120 Hz (8 ms) next lower (prescaler) is 30 Hz which flickers

 *     Not that we could still use the slower prescaler and manually reload

 *     after 127. This would still cost resolution, but 128 steps should be

 *     enough.

 *   - Overflow interrupt 2.5 us (0.03%), compare interrupts are 6*10us (when

 *     using all different values) (0.75%) or 1*26 us (when using all same

 *     values)

 *   - Where to put the time base?

 *     - buzzer => doesn't work because of varying frequency

 *     - motors => possible? would trigger often (?)

 *     - lights => must put lights on 16 bit timer (no OCR left)

 */

#include "lights.h"

#include <avr/interrupt.h>

#include "dragonfly_lib.h"

// *********

// ** I/O **

// *********

#define NUM_ORBS 2   // Number or orbs

#define NUM_COLORS 3 // Number of colors per orb

// Orb port

#define ORBPORT PORTC

#define ORBDDR  DDRC

// Orb pins

#define ORB1_RED   0

#define ORB1_GREEN 1

#define ORB1_BLUE  2

#define ORB2_RED   4

#define ORB2_GREEN 5

#define ORB2_BLUE  6

// ***********

// ** Masks **

// ***********

// Some useful bit masks. All of them are are calculated from the I/O

// definitions above. The calculations should be done at compile time (even if

// they are not, they are only executed once at startup).

// Masks for the individual LEDs

#define orb1_red_mask    _BV (ORB1_RED  )

#define orb1_green_mask  _BV (ORB1_GREEN)

#define orb1_blue_mask   _BV (ORB1_BLUE )

#define orb2_red_mask    _BV (ORB2_RED  )

#define orb2_green_mask  _BV (ORB2_GREEN)

#define orb2_blue_mask   _BV (ORB2_BLUE )

// Mask for all LEDs

const uint8_t all_orbs_mask=

        orb1_red_mask | orb1_green_mask | orb1_blue_mask |

        orb2_red_mask | orb2_green_mask | orb2_blue_mask;

// Mask for the individual LEDs, organized as an array for programmatic access.

// The layout of this array is orb_mask[orb_num, color_num]

const uint8_t orb_mask[NUM_ORBS][NUM_COLORS]=

{

        { orb1_red_mask, orb1_green_mask, orb1_blue_mask },

        { orb2_red_mask, orb2_green_mask, orb2_blue_mask }

};

// ***********

// ** Types **

// ***********

struct pwm_channel_t

{

    uint8_t time;

    uint8_t mask;

};

// ***************

// ** Variables **

// ***************

#define num_pwm_channels NUM_ORBS*NUM_COLORS

struct pwm_channel_t pwm_channels[num_pwm_channels];

uint8_t orb_values[NUM_ORBS][NUM_COLORS];

uint8_t pwm_init_mask;

// TODO use everywhere, add methods to switch on/off, add init function

bool enable_orb_pwm=true;

// ****************

// ** Timer ISRs **

// ****************

volatile uint8_t current_pwm_channel=0;

SIGNAL (SIG_OVERFLOW0)

{

PORTF=2;

        // Turn all appropriate PWM channels on

        ORBPORT&=~pwm_init_mask;

        // Start at the first channel

        current_pwm_channel=0;

        // Load the first OCR

        OCR0=pwm_channels[current_pwm_channel].time;

PORTF=0;

}

SIGNAL(SIG_OUTPUT_COMPARE0)

{

PORTF=4;

        // TODO:

        //   - delayed interrupt

        //   - synchronization OK?

        // If the interrupt is executed w/o delay, TCNT0 == time+1 (and TIME=OCR0)

        // TODO improve (check overflow; maybe use return after last; fix

        // 0/1/254/255 values)

        while (TCNT0==pwm_channels[current_pwm_channel].time+1)

        {

                // Turn the current channel off

                ORBPORT|=pwm_channels[current_pwm_channel].mask;

                // Increment the channel

                current_pwm_channel++;

                // If there is a next channel, load its OCR value

                if (current_pwm_channel<=(num_pwm_channels-1))

                        if (pwm_channels[current_pwm_channel].time<255)

                                OCR0=pwm_channels[current_pwm_channel].time;

        }

PORTF=0;

}

// ************************************

// ** Internal orb setting functions **

// ************************************

// TODO: make a public version of this one, but keep a private one which does

// not sort them so you can update both sides and update only once.

static void apply_orbs (void)

{

        if (enable_orb_pwm)

        {

                // PWM mode

                // Sort the orb values.

                // 1. Write the orb values and corresponding masks to the pwm channels

                // array unsorted

                for (uint8_t orb=0; orb<2; ++orb)

                {

                        for (uint8_t color=0; color<3; ++color)

                        {

                                // TODO this should be faster w/o multiplication

                                uint8_t index=NUM_COLORS*orb+color;

                                pwm_channels[index].time=orb_values[orb][color];

                                pwm_channels[index].mask=orb_mask[orb][color];

                        }

                }

                // 2. Sort the values. Use bubble sort for now.

                for (uint8_t count=num_pwm_channels-1; count>0; --count)

                {

                        for (uint8_t i=num_pwm_channels-1; i>0; --i)

                        {

                                if (pwm_channels[i].time<pwm_channels[i-1].time)

                                {

                                        uint8_t temp;

                                        // Swap the times

                                        temp=pwm_channels[i].time;

                                        pwm_channels[i].time=pwm_channels[i-1].time;

                                        pwm_channels[i-1].time=temp;

                                        // Swap the masks

                                        temp=pwm_channels[i].mask;

                                        pwm_channels[i].mask=pwm_channels[i-1].mask;

                                        pwm_channels[i-1].mask=temp;

                                }

                        }

                }

                pwm_init_mask=all_orbs_mask; // FIXME use real values

                // Sort the values from orb_values[NUM_ORBS][NUM_COLORS].

        }

        else

        {

                // Binary mode.

                // Don't do anything, the orbs pins are set in orb_n_set.

                // It would be more consistent to set them here (because you could

                // update them independently and then apply the changes at once), but it

                // is faster this way, and being fast is the whole point of using the

                // binary orb mode anyway.

        }

}

static void orb_n_set (uint8_t num, uint8_t red, uint8_t green, uint8_t blue)

{

        if (enable_orb_pwm)

        {

                // PWM mode

                orb_values[num][0]=red;

                orb_values[num][1]=green;

                orb_values[num][2]=blue;

        }

        else

        {

                // Binary mode

                // The outputs are inverted.

                if (!red)   ORBPORT|=orb_mask[num][0]; else ORBPORT&=~orb_mask[num][0];

                if (!green) ORBPORT|=orb_mask[num][1]; else ORBPORT&=~orb_mask[num][1];

                if (!blue)  ORBPORT|=orb_mask[num][2]; else ORBPORT&=~orb_mask[num][2];

        }

}

// ************************************

// ** Frontend orb setting functions **

// ************************************

// All of these functions use orb_n_set to set the actual values, and then call

// apply_orbs() to apply the changes. orb_n_set should be used (although it

// would be faster to set the array directly) because the binary/pwm mode has

// to be handled.

/**

 * Set orb1 to the color specified. orb_init must be called before this function

 * may be used.

 *

 * @param red the red component of the color

 * @param green the green component of the color

 * @param blue the blue component of the color

 *

 * @see orb_init

 **/

void orb1_set (uint8_t red, uint8_t green, uint8_t blue)

{

        orb_n_set (0, red, green, blue);

        apply_orbs ();

}

/**

 * 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 (uint8_t red, uint8_t green, uint8_t blue)

{

        orb_n_set (1, red, green, blue);

        apply_orbs ();

}

/**

 * 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, orb1_set, orb2_set

 **/

void orb_set (uint8_t red, uint8_t green, uint8_t blue)

{

        orb_n_set (0, red, green, blue);

        orb_n_set (1, red, green, blue);

        apply_orbs ();

}

void orbs_set (

        uint8_t red1, uint8_t green1, uint8_t blue1,

        uint8_t red2, uint8_t green2, uint8_t blue2)

{

        orb_n_set (0, red1, green1, blue1);

        orb_n_set (1, red2, green2, blue2);

        apply_orbs ();

}

//////////////////////////////////////////////////////////////////////////////////////////////////////////

//#define ORB_RESET 1025

//#define ORBMASK 0x77

//

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

//

//

//// 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(uint16_t orb, uint16_t 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) {

//   uint16_t 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) {

//   uint16_t 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.

// *

// * @{

// **/

//

/**

 * 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(uint8_t col)

{

// uint16_t 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(uint8_t  col)

{

// uint16_t 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(uint8_t  col)

{

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

// ********************

// ** Initialization **

// ********************

/**

 * Initializes the PWM for Orb control. This must be called before 

 * the orbs are used for them to function.

 **/

void orb_init ()

{

        // Use 8 bit TC0. Timer mode:

        //   We cannot use CTC mode because it can only clear on OCR0 (in contrast

        //   to the 16 bit timers which can also use the ICR for that) and OCR0 is

        //   already used for generating output compare interrupts. We also need

        //   immediate (non double buffered) update of OCR0, so the only mode left

        //   is "Normal".

        //   Note that for a timer counting from 0 to 255, there are 256 states and

        //   thus 257 output possibilities (0/256...256/256)! Possible ways to deal

        //   with that:

        //     1. use a 16 bit variable for the PWM value (memory waste, overhead)

        //     2. use an additional flag for the 257th value (inconvenient)

        //     3. use 1/256...256/256 (skip 0, never complete off)

        //     4. use 0/256...256/256 (skip 256, never complete on)

        //     5. skip a value somewhere in the middle

        //   For this implementation, variant 4 was chosen.

        // Using and 8 bit timer has the added advantage that all the comparisons

        // are faster.

        // Enable the output ports and turn off the LEDs

        ORBDDR  |=  all_orbs_mask;

        ORBPORT |=  all_orbs_mask;

        // Set all orbs to "off"

        orb_set (0, 0, 0);

        // *** Set up the timer

        // Normal mode, Compare match output off, Prescaler

        TCCR0=_BV(CS02) | _BV(CS01) | _BV(CS00); // 1024, 30 Hz

        TCCR0=_BV(CS02) | _BV(CS01); // 1024, 30 Hz

        // Enable compare match and overflow interrupts

        TIMSK=_BV(OCIE0) | _BV(TOIE0);

    // Debug

    DDRF=6;

        // The output compare flag (and interrupt) is set at the next timer clock

        // cycle after compare match. So time=pwm_value-1 (for pwm_value==0: don't

        // switch on at all)

        // ORB1: red

        // ORB2: green

        // Left: greenish red, Right: greenish blue

        // For testing, set some pretty colors

        orbs_set (250, 127, 3, 3, 127, 250);

        //orbs_set (252, 129, 5, 3, 127, 250);

        //orbs_set (127, 127, 127, 127, 127, 127);

//        // Init mask: all masks where value>0

//        pwm_init_mask=

//                orb1_red_mask |

//                orb1_green_mask |

//                orb1_blue_mask |

//                orb2_green_mask |

//                orb2_blue_mask

//                ;

//

//        // Channels: value-1

//        pwm_channels[0].mask=orb2_red_mask   ; pwm_channels[0].time= 3-1;//0

//        pwm_channels[1].mask=orb1_blue_mask  ; pwm_channels[1].time= 3-1;//1

//        pwm_channels[2].mask=orb2_green_mask ; pwm_channels[2].time=127-1;//126

//        pwm_channels[3].mask=orb1_green_mask ; pwm_channels[3].time=127-1;//127

//        pwm_channels[4].mask=orb2_blue_mask  ; pwm_channels[4].time=250-1;//254

//        pwm_channels[5].mask=orb1_red_mask   ; pwm_channels[5].time=250-1;//255

}