Psi Swarm Robot
Practical Robotics Modular Robot Library
led.cpp
- Committer:
- jah128
- Date:
- 2017-01-13
- Revision:
- 6:732aa91eb555
- Parent:
- 5:6da8daaeb9f7
File content as of revision 6:732aa91eb555:
#include "robot.h"
// The LED driver is quite powerful in terms of how it can be used (LEDs can be grouped, have individual and group PWM control, can be blinked etc).
//
// To keep things relatively simple, in this version of the code, we will not use the groups or blinking functions, and will have all LEDs enabled.
//
// Their state is controlled solely by each LEDs PWM control - which is a 1 byte value between 0 (fully off) and 255 (fully on)
char red_led_states [8];
char green_led_states [8];
Ticker led_ticker;
int animation_on = 0;
int animation_mode = 0;
int animation_state = 0;
int animation_substate = 0;
char increment = 1;
void Led::restore_states ()
{
char data[17];
data[0]=130; //2 + 128 (AutoInc for brightness)
for(int i=0; i<8; i++) {
data[(i+i)+1] = red_led_states[i];
data[(i+i)+2] = green_led_states[i];
}
if(animation_on == 1) stop_animation();
else {
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,17);
}
i2c_lock = 0;
}
}
void Led::set_all (char red_brightness, char green_brightness)
{
char data[17];
data[0]=130; //2 + 128 (AutoInc for brightness)
for(int i=0; i<8; i++) {
red_led_states[i] = red_brightness;
green_led_states[i] = green_brightness;
data[(i+i)+1] = red_brightness;
data[(i+i)+2] = green_brightness;
}
if(animation_on == 1) stop_animation();
else {
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,17);
}
i2c_lock = 0;
}
}
void Led::set_leds (char * red_brightness, char * green_brightness)
{
char data[17];
data[0]=130; //2 + 128 (AutoInc for brightness)
for(int i=0; i<8; i++) {
red_led_states[i] = red_brightness[i];
green_led_states[i] = green_brightness[i];
data[(i+i)+1] = red_brightness[i];
data[(i+i)+2] = green_brightness[i];
}
if(animation_on == 1) stop_animation();
else {
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,17);
}
i2c_lock = 0;
}
}
void Led::set_leds (char red1, char red2, char red3, char red4, char red5, char red6, char red7, char red8, char grn1, char grn2, char grn3, char grn4, char grn5, char grn6, char grn7, char grn8)
{
char data[17];
data[0]=130; //2 + 128 (AutoInc for brightness)
red_led_states[0] = red1;
red_led_states[1] = red2;
red_led_states[2] = red3;
red_led_states[3] = red4;
red_led_states[4] = red5;
red_led_states[5] = red6;
red_led_states[6] = red7;
red_led_states[7] = red8;
green_led_states[0] = grn1;
green_led_states[1] = grn2;
green_led_states[2] = grn3;
green_led_states[3] = grn4;
green_led_states[4] = grn5;
green_led_states[5] = grn6;
green_led_states[6] = grn7;
green_led_states[7] = grn8;
for(int i=0; i<8; i++) {
data[(i+i)+1] = red_led_states[i];
data[(i+i)+2] = green_led_states[i];
}
if(animation_on == 1) stop_animation();
else {
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,17);
}
i2c_lock = 0;
}
}
void Led::_set_leds (char red1, char red2, char red3, char red4, char red5, char red6, char red7, char red8, char grn1, char grn2, char grn3, char grn4, char grn5, char grn6, char grn7, char grn8)
{
char data[17];
data[0]=130; //2 + 128 (AutoInc for brightness)
data[1] = red1;
data[3] = red2;
data[5] = red3;
data[7] = red4;
data[9] = red5;
data[11] = red6;
data[13] = red7;
data[15] = red8;
data[2] = grn1;
data[4] = grn2;
data[6] = grn3;
data[8] = grn4;
data[10] = grn5;
data[12] = grn6;
data[14] = grn7;
data[16] = grn8;
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,17);
}
i2c_lock = 0;
}
void Led::all_off()
{
char data[17];
data[0]=130; //2 + 128 (AutoInc for brightness)
for(int i=0; i<8; i++) {
red_led_states[i] = 0;
green_led_states[i] = 0;
data[(i+i)+1] = 0;
data[(i+i)+2] = 0;
}
if(animation_on == 1) stop_animation();
else {
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,17);
}
i2c_lock = 0;
}
}
void Led::set_green_led (char led, char brightness)
{
if(led < 8) {
green_led_states [led] = brightness;
char data[2];
data[0] = 3 + led + led;
data[1] = brightness;
if(animation_on == 1) stop_animation();
else {
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,2);
}
i2c_lock = 0;
}
}
}
void Led::set_red_led (char led, char brightness)
{
if(led < 8) {
red_led_states [led] = brightness;
char data[2];
data[0] = 2 + led + led;
data[1] = brightness;
if(animation_on == 1) stop_animation();
else {
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,2);
}
i2c_lock = 0;
}
}
}
void Led::_set_all (char red_brightness, char green_brightness)
{
char data[17];
data[0]=130; //2 + 128 (AutoInc for brightness)
for(int i=0; i<8; i++) {
data[(i+i)+1] = red_brightness;
data[(i+i)+2] = green_brightness;
}
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,17);
}
i2c_lock = 0;
}
void Led::_set_green_led (char led, char brightness)
{
if(led < 8) {
char data[2];
data[0] = 3 + led + led;
data[1] = brightness;
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,2);
}
i2c_lock = 0;
}
}
void Led::_set_red_led (char led, char brightness)
{
if(led < 8) {
char data[2];
data[0] = 2 + led + led;
data[1] = brightness;
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,2);
}
i2c_lock = 0;
}
}
void Led::_set_leds (char * red_brightness, char * green_brightness)
{
char data[17];
data[0]=130; //2 + 128 (AutoInc for brightness)
for(int i=0; i<8; i++) {
data[(i+i)+1] = red_brightness[i];
data[(i+i)+2] = green_brightness[i];
}
if(i2c_lock==0) {
i2c_lock = 1;
primary_i2c.write(LED_ADDRESS,data,17);
}
i2c_lock = 0;
}
void Led::set_green_led_stored_state (char led, char brightness)
{
if(led < 8) {
green_led_states [led] = brightness;
}
}
void Led::set_red_led_stored_state (char led, char brightness)
{
if(led < 8) {
red_led_states [led] = brightness;
}
}
int Led::reset_led_driver()
{
// The TLC59116 has a software reset option over i2c:
// The bytes A5h, 5Ah are sent to special address D6h
char data[2];
data[0] = 0xA5;
data[1] = 0x5A;
return primary_i2c.write(0xD6,data,2);
}
int Led::init_led_driver()
{
// This code enables the LED outputs so all LEDs are on, and are set to work on individual PWM control
//
// First we write 0x80 to the Mode 1 register (0x00) to enable the oscillator and register auto-increment
char data[2];
data[0]=0x00;
data[1]=0x80;
int okay = primary_i2c.write(LED_ADDRESS,data,2,false);
if(okay == 0) {
// Now we turn all the LEDs the enable on PWM control
data[0]=0x14;
data[1]=0xAA;
primary_i2c.write(LED_ADDRESS,data,2,false);
data[0]=0x15;
data[1]=0xAA;
primary_i2c.write(LED_ADDRESS,data,2,false);
data[0]=0x16;
data[1]=0xAA;
primary_i2c.write(LED_ADDRESS,data,2,false);
data[0]=0x17;
data[1]=0xAA;
primary_i2c.write(LED_ADDRESS,data,2,false);
}
return okay;
}
//Animations
void Led::stop_animation()
{
if(animation_on == 1) {
animation_on = 0;
led_ticker.detach();
restore_states();
}
}
void Led::start_animation(char mode, char speed)
{
//Speed is ranged so 0 is fastest
int us_delay = 4000 + (speed * 1000);
animation_on = 1;
animation_mode = mode;
animation_state = 0;
animation_substate = 0;
increment = 1;
//Turn off all LEDs
_set_all(0,0);
led_ticker.attach_us(this,&Led::animation_cycle,us_delay);
}
void Led::animation_cycle()
{
char red_brightness;
char green_brightness;
char working_led;
switch(animation_mode) {
case 0:
// Flood-fade (boot-up animation)
animation_substate += increment;
increment += 1;
if(animation_substate > 250) {
animation_substate = 0;
animation_state ++;
increment = 1;
if (animation_state == 24) animation_state = 0;
}
working_led = animation_state % 8;
if(animation_state < 8 || animation_state > 15) _set_green_led(working_led, animation_substate );
else _set_green_led(working_led, 0);
if(animation_state > 7) _set_red_led(working_led, animation_substate );
else _set_red_led(working_led, 0);
break;
case 1:
// R-O-G Colour sweep
switch(animation_state) {
case 0:
red_brightness = 255;
green_brightness = animation_substate * 4;
break;
case 1:
red_brightness = 255 - (animation_substate * 4);
green_brightness = 128 + (animation_substate * 4);
break;
case 2:
green_brightness = 255;
red_brightness = 128 - (animation_substate * 4);
break;
case 3:
green_brightness = 255 - (animation_substate * 4);
red_brightness = animation_substate * 4;
break;
case 4:
green_brightness = 128 - (animation_substate * 4);
red_brightness = 128 + (animation_substate * 4);
break;
}
animation_substate ++;
if(animation_substate == 32) {
animation_substate = 0;
animation_state ++;
if(animation_state == 5) animation_state = 0;
}
_set_all(red_brightness,green_brightness);
break;
case 2:
// Red Strobe
if(animation_state < 6) _set_all(animation_state * 48, 0);
if(animation_state == 6) _set_all(255,0);
if(animation_state == 12) _set_all(0,0);
animation_state ++;
if(animation_state == 18) animation_state = 0;
break;
case 3:
// Green Strobe
if(animation_state < 6) _set_all(0,animation_state * 48);
if(animation_state == 6) _set_all(0,255);
if(animation_state == 12) _set_all(0,0);
animation_state ++;
if(animation_state == 18) animation_state = 0;
break;
case 4:
// Orange Strobe
if(animation_state < 6) _set_all(animation_state * 48,animation_state * 48);
if(animation_state == 6) _set_all(255,255);
if(animation_state == 12) _set_all(0,0);
animation_state ++;
if(animation_state == 18) animation_state = 0;
break;
case 5:
// R:G Strobe
if(animation_state == 0) _set_all(0,64);
if(animation_state == 1) _set_all(0,128);
if(animation_state == 2) _set_all(0,192);
if(animation_state == 3) _set_all(0,255);
if(animation_state == 7) _set_all(32,64);
if(animation_state == 8) _set_all(64,0);
if(animation_state == 9) _set_all(128,0);
if(animation_state == 10) _set_all(192,0);
if(animation_state == 11) _set_all(255,0);
if(animation_state == 15) _set_all(64,32);
animation_state ++;
if(animation_state == 16) animation_state = 0;
break;
case 6:
// Forward red chase
if(animation_state == 0) _set_leds( 0, 0, 0, 64, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 1) _set_leds( 0, 0, 64,128,128, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 2) _set_leds( 0, 64,128,255,255,128, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 3) _set_leds( 64,128,255,255,255,255,128, 64, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 4) _set_leds(128,255,255,128,128,255,255,128, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 5) _set_leds(255,255,128, 64, 64,128,255,255, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 6) _set_leds(255,220, 92, 32, 32, 92,220,255, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 8) _set_leds(255,192, 64, 16, 16, 64,192,255, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 10) _set_leds(255,160, 32, 8, 8, 32,160,255, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 12) _set_leds(255,128, 16, 0, 0, 16,128,255, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 13) _set_leds(255, 64, 4, 0, 0, 4, 64,255, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 14) _set_leds(255, 0, 0, 0, 0, 0, 0,255, 0, 0, 0, 0, 0, 0, 0, 0);
animation_state ++;
if(animation_state == 16)animation_state = 0;
break;
case 7:
// Reverse red chase
if(animation_state == 0) _set_leds( 64, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 1) _set_leds(128, 64, 0, 0, 0, 0, 64,128, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 2) _set_leds(255,128, 64, 0, 0, 64,128,255, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 3) _set_leds(255,255,128, 64, 64,128,255,255, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 4) _set_leds(128,255,255,128,128,255,255,128, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 5) _set_leds( 64,128,255,255,255,255,128, 64, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 6) _set_leds( 32, 92,220,255,255,220, 92, 32, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 8) _set_leds( 16, 64,192,255,255,192, 64, 16, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 10) _set_leds( 8, 32,160,255,255,160, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 12) _set_leds( 0, 16,128,255,255,128, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 13) _set_leds( 0, 4, 64,255,255, 64, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0);
if(animation_state == 14) _set_leds( 0, 0, 0,255,255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
animation_state ++;
if(animation_state == 16)animation_state = 0;
break;
case 8:
// Forward green chase
if(animation_state == 0) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 64, 64, 0, 0, 0);
if(animation_state == 1) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 64,128,128, 64, 0, 0);
if(animation_state == 2) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 0, 64,128,255,255,128, 64, 0);
if(animation_state == 3) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 64,128,255,255,255,255,128, 64);
if(animation_state == 4) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,128,255,255,128,128,255,255,128);
if(animation_state == 5) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,255,255,128, 64, 64,128,255,255);
if(animation_state == 6) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,255,220, 92, 32, 32, 92,220,255);
if(animation_state == 8) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,255,192, 64, 16, 16, 64,192,255);
if(animation_state == 10) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,255,160, 32, 8, 8, 32,160,255);
if(animation_state == 12) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,255,128, 16, 0, 0, 16,128,255);
if(animation_state == 13) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,255, 64, 4, 0, 0, 4, 64,255);
if(animation_state == 14) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,255, 0, 0, 0, 0, 0, 0,255);
animation_state ++;
if(animation_state == 16)animation_state = 0;
break;
case 9:
// Reverse green chase
if(animation_state == 0) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 0, 0, 0, 64);
if(animation_state == 1) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,128, 64, 0, 0, 0, 0, 64,128);
if(animation_state == 2) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,255,128, 64, 0, 0, 64,128,255);
if(animation_state == 3) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,255,255,128, 64, 64,128,255,255);
if(animation_state == 4) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0,128,255,255,128,128,255,255,128);
if(animation_state == 5) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 64,128,255,255,255,255,128, 64);
if(animation_state == 6) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 32, 92,220,255,255,220, 92, 32);
if(animation_state == 8) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 16, 64,192,255,255,192, 64, 16);
if(animation_state == 10) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 8, 32,160,255,255,160, 32, 8);
if(animation_state == 12) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 0, 16,128,255,255,128, 16, 0);
if(animation_state == 13) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 64,255,255, 64, 4, 0);
if(animation_state == 14) _set_leds( 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,255,255, 0, 0, 0);
animation_state ++;
if(animation_state == 16)animation_state = 0;
break;
case 10:
// Forward orange chase
if(animation_state == 0) _set_leds( 0, 0, 0, 64, 64, 0, 0, 0, 0, 0, 0, 64, 64, 0, 0, 0);
if(animation_state == 1) _set_leds( 0, 0, 64,128,128, 64, 0, 0, 0, 0, 64,128,128, 64, 0, 0);
if(animation_state == 2) _set_leds( 0, 64,128,255,255,128, 64, 0, 0, 64,128,255,255,128, 64, 0);
if(animation_state == 3) _set_leds( 64,128,255,255,255,255,128, 64, 64,128,255,255,255,255,128, 64);
if(animation_state == 4) _set_leds(128,255,255,128,128,255,255,128,128,255,255,128,128,255,255,128);
if(animation_state == 5) _set_leds(255,255,128, 64, 64,128,255,255,255,255,128, 64, 64,128,255,255);
if(animation_state == 6) _set_leds(255,220, 92, 32, 32, 92,220,255,255,220, 92, 32, 32, 92,220,255);
if(animation_state == 8) _set_leds(255,192, 64, 16, 16, 64,192,255,255,192, 64, 16, 16, 64,192,255);
if(animation_state == 10) _set_leds(255,160, 32, 8, 8, 32,160,255,255,160, 32, 8, 8, 32,160,255);
if(animation_state == 12) _set_leds(255,128, 16, 0, 0, 16,128,255,255,128, 16, 0, 0, 16,128,255);
if(animation_state == 13) _set_leds(255, 64, 4, 0, 0, 4, 64,255,255, 64, 4, 0, 0, 4, 64,255);
if(animation_state == 14) _set_leds(255, 0, 0, 0, 0, 0, 0,255,255, 0, 0, 0, 0, 0, 0,255);
animation_state ++;
if(animation_state == 16)animation_state = 0;
break;
case 11:
// Reverse orange chase
if(animation_state == 0) _set_leds( 64, 0, 0, 0, 0, 0, 0, 64, 64, 0, 0, 0, 0, 0, 0, 64);
if(animation_state == 1) _set_leds(128, 64, 0, 0, 0, 0, 64,128,128, 64, 0, 0, 0, 0, 64,128);
if(animation_state == 2) _set_leds(255,128, 64, 0, 0, 64,128,255,255,128, 64, 0, 0, 64,128,255);
if(animation_state == 3) _set_leds(255,255,128, 64, 64,128,255,255,255,255,128, 64, 64,128,255,255);
if(animation_state == 4) _set_leds(128,255,255,128,128,255,255,128,128,255,255,128,128,255,255,128);
if(animation_state == 5) _set_leds( 64,128,255,255,255,255,128, 64, 64,128,255,255,255,255,128, 64);
if(animation_state == 6) _set_leds( 32, 92,220,255,255,220, 92, 32, 32, 92,220,255,255,220, 92, 32);
if(animation_state == 8) _set_leds( 16, 64,192,255,255,192, 64, 16, 16, 64,192,255,255,192, 64, 16);
if(animation_state == 10) _set_leds( 8, 32,160,255,255,160, 32, 8, 8, 32,160,255,255,160, 32, 8);
if(animation_state == 12) _set_leds( 0, 16,128,255,255,128, 16, 0, 0, 16,128,255,255,128, 16, 0);
if(animation_state == 13) _set_leds( 0, 4, 64,255,255, 64, 4, 0, 0, 4, 64,255,255, 64, 4, 0);
if(animation_state == 14) _set_leds( 0, 0, 0,255,255, 0, 0, 0, 0, 0, 0,255,255, 0, 0, 0);
animation_state ++;
if(animation_state == 16)animation_state = 0;
break;
case 12:
// Alternate red:green
if(animation_state == 0) _set_leds( 64, 0, 64, 0, 64, 0, 64, 0, 0, 64, 0, 64, 0, 64, 0, 64);
if(animation_state == 1) _set_leds(128, 0,128, 0,128, 0,128, 0, 0,128, 0,128, 0,128, 0,128);
if(animation_state == 2) _set_leds(255, 0,255, 0,255, 0,255, 0, 0,255, 0,255, 0,255, 0,255);
if(animation_state == 6) _set_leds(128, 0,128, 0,128, 0,128, 0, 0,128, 0,128, 0,128, 0,128);
if(animation_state == 7) _set_leds( 64, 0, 64, 0, 64, 0, 64, 0, 0, 64, 0, 64, 0, 64, 0, 64);
if(animation_state == 8) _set_leds( 0, 64, 0, 64, 0, 64, 0, 64, 64, 0, 64, 0, 64, 0, 64, 0);
if(animation_state == 9) _set_leds( 0,128, 0,128, 0,128, 0,128,128, 0,128, 0,128, 0,128, 0);
if(animation_state == 10) _set_leds( 0,255, 0,255, 0,255, 0,255,255, 0,255, 0,255, 0,255, 0);
if(animation_state == 14) _set_leds( 0,128, 0,128, 0,128, 0,128,128, 0,128, 0,128, 0,128, 0);
if(animation_state == 15) _set_leds( 0, 64, 0, 64, 0, 64, 0, 64, 64, 0, 64, 0, 64, 0, 64, 0);
animation_state ++;
if(animation_state == 16)animation_state = 0;
break;
}
}