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

 **/

Controls orb1 and orb2. Can be extended for a software PWM that may be used for servos in the future (although maybe

using a different timer might be preferable).

3/31/2009 - Martin

    Rewritten from scratch. Fixes code duplication, long ISRs, bugs, unnecessary synchronized code, memory waste

*/

/*

 * Test cases:

 *   - The following code has to work without flickering:

 *     orb_init_pwm(); while(1) { orbs_set(1,1,1,254,254,254); }

 */

/*

 * Possible optimizations:

 *   - Use pointers instead of indicies for current_pwm_channel

 *   - Optimize output_compare()

 *   - Use a different sorting algorithm (see sort_orbs_buffer for further comments on this issue)

 *   - Use pointers in fill_orbs_buffer

 *   - Optimized orb_set (use the knowledge that there are only 3 distinct values, don't use a loop but unroll the

 *     sorting, which is no problem for 3 values)

 *   - Use a lower update frequency. The next higher prescaler value leads to a frequency of 30Hz which is too low (the

 *     orbs are flickering). So the timer would have to be reloaded manually after 127 to generate 60Hz. This would

 *     decrease the resolution from 8 to 7 bit, but 128 steps should still be enough.

 *   - On setting the orbs, combine channels with the same time. This would reduce the all-values-equal OC interrupt

 *     (30us) to the time of one regular OC interrupt (6us/10us). Also, it would reduce the total cpu usage whenever

 *     some of the values are equal.

 *

 * When code is changed, the performance measurements above should be redone.

 */

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

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

// ** Debugging **

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

//#define LIGHTS_DEBUG

#undef LIGHTS_DEBUG

#define LIGHTS_DEBUG_INIT                             DDRF=6;

#define LIGHTS_DEBUG_OVERFLOW_INTERRUPT_START         PORTF|=4;

#define LIGHTS_DEBUG_OVERFLOW_INTERRUPT_END           PORTF&=~4;

#define LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_START   PORTF|=2;

#define LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_END     PORTF&=~2;

#define LIGHTS_DEBUG_APPLY_START                      //PORTF|=2;

#define LIGHTS_DEBUG_APPLY_END                        //PORTF&=~2;

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

// ** 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. Not volatile because it's only read once per function.

bool enable_orb_pwm=true;

// The PWM channels and the buffer pointers. This data structure is triple buffered, see above for the reasons. Not

// volatile because they are not modified asynchronously (the read buffer is never written to asynchronously).

struct pwm_t pwm_buffer[3];

// The front buffer the ISR reads from. Other threads may not touch this pointer or the buffer it points to. Not

// volatile because it may only be modified by the ISR.

struct pwm_t *pwm_read_buffer =&pwm_buffer[0];

// The back buffer we can write to. The ISR may not touch this pointer or the buffer it points to. Not volatile because

// it may only be modified by the caller.

struct pwm_t *pwm_write_buffer=&pwm_buffer[1]; 

// The middle buffer to flip the write or read buffer with. Not volatile because it is only read once per function.

struct pwm_t *pwm_free_buffer =&pwm_buffer[2]; 

// Whether to perform a page flip on the beginning of the next PWM cycle. Not volatile because it is only read once

// per function.

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. Not volatile because all accesses

// are from guarded (thread safe) functions.

uint8_t orb_values[NUM_ORBS][NUM_COLORS];

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

// ** Timer ISRs **

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

// Not volatile because it is only accessed in the interrupt handler.

uint8_t current_pwm_channel=0;

static void output_compare (void) {

	// This function is called when an output compare condition may have occured.

    // If the OC interrupt is executed without delay, TCNT0==time+1 (where time==OCR0), because the interrupt flag is

	// queued at the next timer clock cycle after an output compare.

    // What may happen here is that the interrupt is delayed for more than one timer clock cycle (33 us). In that case,

	// the timer has already counted on and TCNT0 is bigger than current_channel_timer. Also, while during the ISR no

	// other interrupts will occur, the timer may still count on. Thus, we have to check the following channel as well.

	// Some optimization is possible in this function.

    while (1) {

		// The timer value at which the output compare interrupt should occur (one timer clock cycle after the output

		// compare condition is detected).

		uint8_t current_channel_time=pwm_read_buffer->channel[current_pwm_channel].time+1;

		// If the counter is not at this time yet, we don't have to do anything right now.

		if (current_channel_time>TCNT0) return;

		// We have an output compare condition for the current channel.

        // Turn the current channel off

        ORBPORT|=pwm_read_buffer->channel[current_pwm_channel].mask;

		// If this was the last channel, exit

		if (current_pwm_channel==num_pwm_channels-1) return;

        // Increment the channel index

        current_pwm_channel++;

        // There is a next channel, load its OCR value

        if (pwm_read_buffer->channel[current_pwm_channel].time<255)

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

    }

SIGNAL (SIG_OVERFLOW0) {

#ifdef LIGHTS_DEBUG

    LIGHTS_DEBUG_OVERFLOW_INTERRUPT_START

#endif

    if (pwm_page_flip) {

        // Flip the read buffer with the free buffer. We are in an ISR (and we didn't re-enable interrupts), 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. Do this directly on the orb port because at this point all orbs should

    // be off anyway.

    ORBPORT|=all_orbs_mask;

    ORBPORT&=pwm_read_buffer->init_mask;

    // Start at the first channel

    current_pwm_channel=0;

    // Load the first OCR

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

	// If this interrupt was delayed, we might already have an output compare condition.

	output_compare ();

#ifdef LIGHTS_DEBUG

    LIGHTS_DEBUG_OVERFLOW_INTERRUPT_END

#endif

SIGNAL(SIG_OUTPUT_COMPARE0) {

#ifdef LIGHTS_DEBUG

    LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_START

#endif

	// We have an output compare condition.

	output_compare ();

#ifdef LIGHTS_DEBUG

    LIGHTS_DEBUG_OUTPUT_COMPARE_INTERRUPT_END

#endif

}

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

// ** Internal orb setting functions **

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

static void sort_orbs_buffer (void) {

    // This function applies a bubble sort to sort the elements of the pwm_write_buffer->channel array by the time

    // field.

    // This implementation is heavily optimized. Note that due to the low (and constant) number of elements to be

    // sorted, the runtime complexity (O(n^2) for bubble sort) is not relevant here. In fact, a more advanced algorithm

    // like quick sort or merge sort might even be slower due to higher overhead.

    // That said, it is possible that selection sort (which is also in O(n^2)) would be faster that bubble sort because

    // it only has to do a maximum of (n-1) swapping steps (as opposed to n*(n-1)/2 for bubble sort). However, the check

    // if the elements are already in the correct order would either have to be left out (doing the full search every

    // time, even if the array is already sorted) or done explicitly, so selection sort might actually be slower than

    // bubble sort, especially if the array is already sorted or almost sorted.

    // This implementation uses macros to make the algorithm more clear because the loop is rolled out and the function

    // would become quite long without macros.

    // Macro to swap two values of any type. Requires a variable of the appropriate type called swap_temp.

    #define swap(a,b) { swap_temp=a; a=b; b=swap_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 already in correct order.

    bool done;

    // A temporary variable for swapping.

    uint8_t swap_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) {

    // We do not use a loop here because it introduces 27us overhead, which is quite much, given the total time for

    // optimized copying and sorting of 34us (elements already in correct order) to 71 us (elements in reverse order).

    #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; \

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

	/*

	 * Some timing tests: Time for apply_orbs with interrupts disabled, in us:

     *             Values in:     Correct order      Reverse order

     * Naive bubble sort:         148                217

     * Aborting bubble sort:       71                232

     * Only count to top:          73                189

     *

     * Loops rolled out:           61                120

     * Using pointers:             62                 98

     * Copy loop also rolled out:  35                 72

	 *

	 * Note that rolling out both loops and using pointers saves 52%/62% of time! 27us were spent on loop overhead,

	 * which is quite much, considering an optimized total time for copying and sorting or 35us.

	 */

#ifdef LIGHTS_DEBUG

    LIGHTS_DEBUG_APPLY_START

#endif

    if (enable_orb_pwm) {

        // PWM mode

        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;

    }

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

        // Write the new orb states to the output port. Synchronized because it is a RMW operation.

        SYNC {

            uint8_t value=ORBPORT;

            value |= all_orbs_mask; // All orbs off

            value &= ~on; // Selected orbs on

            ORBPORT=value;

        }

    }

#ifdef LIGHTS_DEBUG

    LIGHTS_DEBUG_APPLY_END

#endif

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

    // Write the values to the array, but do not sort them yet, as we might want to write the other orb values first so

    // we don't have to sort twice.

    // Any function calling this function will probably want to call apply_orbs() afterwards.

    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.

// All of these functions must be 

uint8_t orb_lock=0;

 * Sets the specified orb to the specified color. The orbs must be initialized before this function may be used.

 * Note that, when setting both orbs, using orbs_set is faster then setting the orbs individually because the values are

 * only sorted once.

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

 * @param red the red value for the specified orb

 * @param green the green value for the specified orb

 * @param blue the blue value for the specified orb

 * @see 

 */

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

	REQUIRE_LOCK_OR_RETURN(orb_lock);

    set_orb_values (num, red, green, blue);

    apply_orbs ();

	RELEASE_LOCK(orb_lock);

}

/**

 * Set orb1 to the color specified. The orbs must be initialized before this function may be used. Note that, when

 * setting both orbs, using orbs_set is faster then setting the orbs individually because the values are only sorted

 * once.

 * @param red the red component of the color

 * @param green the green component of the color

 * @param blue the blue component of the color

 *

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

	REQUIRE_LOCK_OR_RETURN(orb_lock);

    set_orb_values (0, red, green, blue);

    apply_orbs ();

	RELEASE_LOCK(orb_lock);

 * Set orb2 to the color specified. The orbs must be initialized before this function may be used. Note that, when

 * setting both orbs, using orbs_set is faster then setting the orbs individually because the values are only sorted

 * once.

void orb2_set (uint8_t red, uint8_t green, uint8_t blue) {

	REQUIRE_LOCK_OR_RETURN(orb_lock);

    set_orb_values (1, red, green, blue);

    apply_orbs ();

	RELEASE_LOCK(orb_lock);

 * Set both orbs to the color specified. The orbs must be initialized before this function may be used.

 * @see orb_init, orb1_set, orb2_set

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

	REQUIRE_LOCK_OR_RETURN(orb_lock);

    set_orb_values (0, red, green, blue);

    set_orb_values (1, red, green, blue);

    apply_orbs ();

	RELEASE_LOCK(orb_lock);

 * Set the orbs to the respective values. The orbs must be initialized before this function may be used. Note that, when

 * setting both orbs, this function is faster than calling orb1_set and orb2_set (or orb_n_set) because the values are

 * only sorted once.

 * @param red1

 * @param green1

 * @param blue1

 * @param red2

 * @param green2

 * @param blue2

 * @see orb1_set

 * @see orb2_set

 * @see orb_n_set

void orbs_set (

    uint8_t red1, uint8_t green1, uint8_t blue1,

    uint8_t red2, uint8_t green2, uint8_t blue2) {

	REQUIRE_LOCK_OR_RETURN(orb_lock);

    set_orb_values (0, red1, green1, blue1);

    set_orb_values (1, red2, green2, blue2);

    apply_orbs ();

	RELEASE_LOCK(orb_lock);

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

// ** Predefined color setting **

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

// This functions just call the corresponding orb*_set functions. If the orbs array is accessed in any other way, it

// must be synchronized on orb_lock (REQUIRE_LOCK_OR_RETURN and RELEASE_LOCK)! Note that one synchronized function

// cannot call another one with this lock implementation.

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

 **/

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 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 both orbs to the specified color. This function is intended to be used with the predefined colors.

 **/

void orb_set_color(uint8_t col) {

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

}

/**

 * Set the orbs to the respective 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 **

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

/**

 * Enables the orb timer. Note that you usually don't want to use this function directly. Instead, use orb_set_mode.

 * @see orb_set_mode

 **/

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)! However, there are only 256 values in the byte used to specify the PWM value. 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 an 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); // 256, 120 Hz

    // Enable the interrupts

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

 * Disables the orb timer. Note that you usually don't want to use this function directly. Instead, use orb_set_mode.

 * @see orb_set_mode

void orb_disable_timer (void) {

    // Disable the interrupts

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

void orb_set_mode (orb_mode_t mode) {

    // Set enable_orb_pwm to the appropriate value and disable or enable the timer.

    if (mode==orb_mode_binary) {

        orb_disable_timer ();

        enable_orb_pwm=false;

        apply_orbs ();

    }

    else { // orb_mode_pwm

        enable_orb_pwm=true;

        apply_orbs ();

        orb_enable_timer ();

    }

}

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

// ** Initialization **

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

// Orb initialization code common to all modes.

static void orb_init_common (void) {

    // Enable the output ports and turn off the LEDs

    ORBPORT |=  all_orbs_mask;

    ORBDDR  |=  all_orbs_mask;

    // Set all orbs to "off"

    orb_set (0, 0, 0);

#ifdef LIGHTS_DEBUG

    LIGHTS_DEBUG_INIT

#endif

}

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

}

/**

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

 * the orb_init_binary or orb_init_pwm function if you want one specific mode.

 *

 * @see orb_init_pwm

 * @see orb_init_binary

 **/

void orb_init () {

    orb_init_pwm ();

}