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

    - enable_orb_pwm use everywhere, add methods to switch on/off, add init

          function

        - test old code: continuously setting the orbs

        - fix sync/volatile

 */

/*

 * 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

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;

};

struct pwm_t

{

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

// TODO using pointers might be faster (or might be slower because pointers are

// 16 bit long).

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

// TODO: random note: what happens if the main process is sorting and a different

// interrupt calls set_orb?

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

// ** Timer ISRs **

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

// Not volatile - only accessed in the interrupt handler

uint8_t current_pwm_channel=0;

uint8_t lastim=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

        //   - 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, maybe use

        // pointers instead of indicies)

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

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

#define SYNC_START uint8_t tmp_sreg; do { tmp_sreg=SREG; cli (); } while (false)

#define SYNC_RESTART                 do { tmp_sreg=SREG; cli (); } while (false)

#define SYNC_END                     do { SREG=tmp_sreg; } while (false)

static void apply_orbs (void)

{

        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

                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;

                                uint8_t time=orb_values[orb][color];

                                uint8_t mask=orb_mask[orb][color];

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

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

                                // If the channel is not off (0), it has to be turned on

                                // (low) at the beginning of the PWM cycle.

                                if (time!=0)

                                        pwm_write_buffer->init_mask &= ~mask;

                        }

                }

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

                // Considering the low number of data points, more

                // sophisticated algorithms are unlikely to be faster.

                bool done;

                // For 6 channels, we count i=0..4 and compare i with i+1

                uint8_t top=num_pwm_channels-1;

                do

                {

                        done=true; // We are done unless we do some swapping

                        for (uint8_t i=0; i<top; ++i)

                        {

                                #define channel_a pwm_write_buffer->channel[i]

                                #define channel_b pwm_write_buffer->channel[i+1]

                                if (channel_a.time>channel_b.time)

                                {

                                        uint8_t temp;

                                        // Swap the times

                                        temp           = channel_a.time;

                                        channel_a.time = channel_b.time;

                                        channel_b.time = temp;

                                        // Swap the masks

                                        temp           = channel_a.mask;

                                        channel_a.mask = channel_b.mask;

                                        channel_b.mask = temp;

                                        done=false; // No, we're not done yet.

                                }

                                #undef channel_a

                                #undef channel_b

                        }

                        // On the next iteration, we need one less comparison

                        top--;

                } while (!done);

                // 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, do the page flip.

                pwm_page_flip=true;

PORTF&=~2;

        }

        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.

        }

}

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

}

/**

 * 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

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

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

//        }

}

// Pure sorting time/us (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%)