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

Ad triple buffering:

  - The buffer the ISR is reading from may only be changed at timer overflow,

    before the next PWM sequence is started, because otherwise, the next OCR

        value may be set to a value smaller than the current timer value, resulting

        in the remaining channels not being turned off in that PWM period (flash to

        on).

  - When using two buffers, the copying (or switching) would have to wait until

    the next timer overflow. During this time, neither of the buffers could be

        modified because one is used by the ISR and the other may be copied/switched

        at any time. Thus, the main thread would possibly be delayed by up to one

        full PWM period (8ms in the current implementation, but 20ms-50ms would be a

        reasonable value to expect here.

  - Triple buffering For Teh Win!

 */

/*

  TODO:

        - Find out the interrupt time

        - Optimize the OC interrupt

        - test old code: continuously setting the orbs

        - fix sync/volatile

        - make functions static

 */

/*

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

 *   - Syncronization test case: set orb A to 1,1,1 (not 0 because they will

 *     not be turned on) and orb B to 254,254,254. Do this in a loop, with

 *     some delay d between.

 *      * d=1ms => occasional flickering

 *      * d=400us => frequent flickering

 *      * d=0 => no usable orb output

 *     Without syncronization, both LEDs flicker (because the wrong values are

 *     in the channels array while sorting). When the sorting code ist

 *     synchronized, only orb A flickers, because the timing is disrupted by the

 *     large synchronized block.

 */

#include "lights.h"

#include <avr/interrupt.h>

#include "dragonfly_lib.h"

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

// ** Constants **

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

#define NUM_ORBS 2   // Number or orbs

#define NUM_COLORS 3 // Number of colors per orb

#define num_pwm_channels NUM_ORBS*NUM_COLORS

// *********

// ** I/O **

// *********

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

#define 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 // 2 bytes

{

    uint8_t time;

    uint8_t mask;

};

struct pwm_t // 13 bytes

{

        uint8_t init_mask;

        struct pwm_channel_t channel[num_pwm_channels];

};

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

// ** Variables **

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

// Whether to use PWM (true) or binary (false) orb mode

bool enable_orb_pwm=true;

// The PWM channels and the buffer pointers. This data structure is triple

// buffered, see above for the reasons.

struct pwm_t pwm_buffer[3];

struct pwm_t *pwm_read_buffer =&pwm_buffer[0]; // The front buffer the ISR reads from. Other thread may not touch this pointer or the buffer it points to.

struct pwm_t *pwm_write_buffer=&pwm_buffer[1]; // The back buffer we can write to. The ISR may not touch this pointer or the buffer it points to.

struct pwm_t *pwm_free_buffer =&pwm_buffer[2]; // The back buffer to flip with.

bool pwm_page_flip=false; // Whether to do a page flip on the next overflow

// The orb value array. Orb values are written here to be sorted into

// pwm_channels.

uint8_t orb_values[NUM_ORBS][NUM_COLORS];

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

// ** Timer ISRs **

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

// Not volatile - only accessed in the interrupt handler

uint8_t current_pwm_channel=0;

SIGNAL (SIG_OVERFLOW0)

{

PORTF|=4;

        if (pwm_page_flip)

        {

                // Flip the read buffer with the free buffer

                // We are in an ISR, so we don't have to synchronize explicitly.

                struct pwm_t *temp = pwm_read_buffer;

                pwm_read_buffer    = pwm_free_buffer;

                pwm_free_buffer    = temp;

                pwm_page_flip=false;

        }

        // Turn only the appropriate PWM channels on

        // TODO: why do we have to turn them off? It should be done in the oc isr.

        // Test code: set (0); set (255); delay (100ms); set (0) => blu/green is on

        // Does it also happen with 254 instead of 255? But isn't it turned off with

        // 255? Better turn them off anyway.

        ORBPORT|=all_orbs_mask;

        ORBPORT&=pwm_read_buffer->init_mask;

        // Start at the first channel (TODO faster w/ pointers?)

        current_pwm_channel=0;

        // Load the first OCR

        OCR0=pwm_read_buffer->channel[current_pwm_channel].time;

PORTF&=~4;

}

SIGNAL(SIG_OUTPUT_COMPARE0)

{

PORTF|=4;

        // TODO:

        //   - delayed interrupt

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

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

        // pointers instead of indicies)

        // (but find out interrupt time before to measure improvement)

        while (TCNT0==pwm_read_buffer->channel[current_pwm_channel].time+1)

        {

                // Turn the current channel off

                ORBPORT|=pwm_read_buffer->channel[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_read_buffer->channel[current_pwm_channel].time<255)

                                OCR0=pwm_read_buffer->channel[current_pwm_channel].time;

        }

PORTF&=~4;

}

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

// ** Internal orb setting functions **

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

static void sort_orbs_buffer (void)

{

        // This function applies an optimized bubble sort. TODO document, and other

        // methods would probably not be faster.

                // Considering the low number of data points, more

                // sophisticated algorithms are unlikely to be faster,

                // especially as this function is fairly optimized.

        // Macro to swap two values of any type. Requires a temp variable of the

        // appropriate type. 

        #define swap(a,b) { temp=a; a=b; b=temp; }

        // Macro to do one bubble sorting step (compare & swap)

        #define bubble \

                if(a->time > b->time) \

                { \

                        swap (a->time, b->time); \

                        swap (a->mask, b->mask); \

                        done=false; \

                }

        // Macro to move to the next bubble sort pair

        #define next { a++; b++; }

        // Whether no change was made during the last run, which means that all

        // values are in correct order.

        bool done;

        // A temporary variable for swapping.

        uint8_t temp;

        // Precompute the first PWM channel (tested faster).

        struct pwm_channel_t *first=&(pwm_write_buffer->channel[0]);

        // Pointers to the two PWM channels under inspection

        struct pwm_channel_t *a, *b;

        // The actual sorting

        a=first; b=a+1; done=true;

        bubble next bubble next bubble next bubble next bubble

        if (done) return;

        a=first; b=a+1; done=true;

        bubble next bubble next bubble next bubble

        if (done) return;

        a=first; b=a+1; done=true;

        bubble next bubble next bubble

        if (done) return;

        a=first; b=a+1; done=true;

        bubble next bubble

        if (done) return;

        a=first; b=a+1; done=true;

        bubble

        if (done) return;

        // Undefine the macros so they do not disturb some other function.

        #undef next

        #undef bubble

        #undef swap

}

static void fill_orbs_buffer (void)

{

        #define copy_value(orb, color) \

                index=NUM_COLORS*orb+color; \

                time=orb_values[orb][color]; \

                mask=orb_mask[orb][color]; \

                \

                pwm_write_buffer->channel[index].time=time-1; \

                pwm_write_buffer->channel[index].mask=mask; \

                \

                if (time!=0) \

                        pwm_write_buffer->init_mask &= ~mask; \

        // TODO try using pointers. Might make it even faster.

        uint8_t index, time, mask;

        copy_value(0,0); copy_value(0,1); copy_value(0,2);

        copy_value(1,0); copy_value(1,1); copy_value(1,2);

        #undef copy_value

}

static void apply_orbs (void)

{

// Time for apply_orbs (enable_orb_pwm block only), with interrupts disabled

// (difference to naive bs):

//                            Correct order      Reverse order

// Naive bubble sort:         147                216

// Aborting bubble sort:       70 (-52%)         231 (+7%)

// Aborting w/ top:            72 (-51%)         188 (-13%)

// Rolled out with aborting:   60 (-59%)         119 (-45%)

// +pointers:                  61 (-59%)          97 (-55%)

// Also unrolled copy loop:    34 (-77%)          71 (-67%) (turns out 27us were spent on loop overhead)

// Improvement of rolling out + pointers: -53%/-62%

        if (enable_orb_pwm)

        {

                // PWM mode

                // Sort the orb values.

PORTF|=2;

                pwm_write_buffer->init_mask=~0;

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

                // array unsorted.

                fill_orbs_buffer ();

                // 2. sort the buffer.

                sort_orbs_buffer ();

                // Flip the write buffer with the free buffer.

                SYNC

                {

                        struct pwm_t *temp = pwm_write_buffer;

                        pwm_write_buffer   = pwm_free_buffer;

                        pwm_free_buffer    = temp;

                }

                // On the next overflow, flip the read buffer with the free buffer.

                pwm_page_flip=true;

PORTF&=~2;

        }

        else

        {

                // Binary mode.

                // The outputs are inverted.

                uint8_t on=0;

                if (orb_values[0][0]) on |= orb_mask[0][0];

                if (orb_values[0][1]) on |= orb_mask[0][1];

                if (orb_values[0][2]) on |= orb_mask[0][2];

                if (orb_values[1][0]) on |= orb_mask[1][0];

                if (orb_values[1][1]) on |= orb_mask[1][1];

                if (orb_values[1][2]) on |= orb_mask[1][2];

                ORBPORT |= all_orbs_mask; // All orbs off

                ORBPORT &= ~on; // Selected orbs on

        }

}

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

{

        // PWM mode

        orb_values[num][0]=red;

        orb_values[num][1]=green;

        orb_values[num][2]=blue;

}

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

// ** RGB color setting **

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

// All of these functions use set_orb_values to set the actual values, and then call apply_orbs() to apply the changes.

// set_orb_values should be used (even though it would be faster to set the array directly) because the binary/pwm mode

// has to be handled.

/**

 * @param num the number of the orb to set (0 or 1)

 */

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

{

        set_orb_values (num, red, green, blue);

        apply_orbs ();

}

/**

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

{

        set_orb_values (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)

{

        set_orb_values (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)

{

        set_orb_values (0, red, green, blue);

        set_orb_values (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)

{

        set_orb_values (0, red1, green1, blue1);

        set_orb_values (1, red2, green2, blue2);

        apply_orbs ();

}

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

// ** Predefined color setting **

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

// Macros for extracting a color.

#define C_RED(col)   (((col & 0xE0) >> 5) * 36)

#define C_GREEN(col) (((col & 0x1C) >> 2) * 36)

#define C_BLUE(col)  (((col & 0x03)     ) * 85)

/**

 * Set both orbs to the specified color. This function is intended to be used with the predefined colors.

 *

 * @param col the color to set the orbs to

 **/

void orb_set_color(uint8_t col)

{

        orb_set (C_RED(col), C_GREEN(col), C_BLUE(col));

}

/**

 * Set orb1 to the specified color. This function is intended to be used with the predefined colors.

 *

 * @param col the color to set the orbs to

 **/

void orb1_set_color(uint8_t col)

{

        orb1_set (C_RED(col), C_GREEN(col), C_BLUE(col));

}

/**

 * Set orb2 to the specified color. This function is intended to be used with the predefined colors.

 *

 * @param col the color to set the orbs to

 **/

void orb2_set_color(uint8_t col)

{

        orb2_set(C_RED(col), C_GREEN(col), C_BLUE(col));

}

/**

 * Set the specified orb to the specified color. This function is intended to be used with the predefined colors.

 *

 * @param num the number of the orb to set (0 or 1)

 * @param col the color to set the orbs to

 **/

void orb_n_set_color(uint8_t num, uint8_t col)

{

        orb_n_set(num, C_RED(col), C_GREEN(col), C_BLUE(col));

}

/**

 * Set the orbs to the respecitve color. This function is intended to be used with the predefined colors.

 *

 * @param col1 the color to set orb 1 to

 * @param col2 the color to set orb 2 to

 **/

void orbs_set_color(uint8_t col1, uint8_t col2)

{

        orbs_set (C_RED(col1), C_GREEN(col1), C_BLUE(col1), C_RED(col2), C_GREEN(col2), C_BLUE(col2));

}

#undef C_BLUE

#undef C_GREEN

#undef C_RED2

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

// ** Mode setting **

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

void orb_enable_timer (void)

{

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

        //     6. reload the timer after 254

        //   For this implementation, variant 4 was chosen.

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

        // are faster.

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

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

}

void orb_disable_timer (void)

{

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

}

void orb_set_mode (orb_mode_t mode)

{

        if (mode==orb_mode_binary)

        {

                enable_orb_pwm=false;

                orb_disable_timer ();

        }

        else // orb_mode_pwm

        {

                enable_orb_pwm=true;

                orb_enable_timer ();

        }

        apply_orbs ();

}

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

// ** Initialization **

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

// Orb initialization code common to all modes.

static void orb_init_common (void)

{

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

    // Debug

    DDRF=6; // TODO remove

}

/**

 * Initializes the orbs in PWM mode. One of the orb_init* functions must be called before the orbs can be used.

 * 

 * @see orb_init_pwm

 **/

void orb_init_binary (void)

{

        orb_init_common ();

        orb_set_mode (orb_mode_binary);

}

/**

 * Initializes the orbs in PWM mode. One of the orb_init* functions must be called before the orbs can be used.

 * 

 * @see orb_init_binary

 **/

void orb_init_pwm (void)

{

        orb_init_common ();

        orb_set_mode (orb_mode_pwm);

}

/**

 * A synonym for orb_init_pwm

 *

 * @see orb_init_pwm

 **/

void orb_init ()

{

        orb_init_pwm ();

}

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

// ** Debugging **

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

void orb_test (void)

{

        // 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); // Pretty colors

        //orbs_set (255, 127, 0, 0, 127, 255); // Pretty colors with extreme values

        //orbs_set (0, 1, 2, 253, 254, 255); // Timing tests

//        orbs_set (255, 255, 255, 0, 0, 0);

//        delay_ms (1000);

        //while (1)

        //{

        //        orbs_set (250, 127, 3, 3, 127, 250); // Pretty colors

        //        //orbs_set (255, 255, 255, 1, 1, 1);

        //        //_delay_us(400);

        //}

        // Test the time of the sorting routine

//        while (1)

//        {

//                orbs_set (10, 20, 30, 40, 50, 60);        // Correct order

//                //orbs_set (60, 50, 40, 30, 20, 10); // Reverse order

//                delay_ms (10);

//        }

}