led.cpp

00001 /* University of York Robotics Laboratory PsiSwarm Library: LED Functions Source File
00002  * 
00003  * File: led.cpp
00004  *
00005  * (C) Dept. Electronics & Computer Science, University of York
00006  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
00007  *
00008  * PsiSwarm Library Version: 0.41
00009  *
00010  * March 2016
00011  *
00012  *
00013  */
00014 
00015 #include "psiswarm.h"
00016 
00017 char green_led_states;
00018 char red_led_states;
00019 char center_led_state;
00020 
00021 char held_red_states;
00022 char held_green_states;
00023 Timeout blink_led_timeout;
00024 
00025 unsigned short get_led_states(){
00026     return (green_led_states << 8) + red_led_states;  
00027 }
00028 
00029 void set_leds(char green, char red){
00030     green_led_states = green;
00031     red_led_states = red;
00032     IF_update_leds();
00033 }
00034 
00035 void set_base_led(char state){
00036     IF_set_base_LED(state); 
00037 }
00038 
00039 void set_green_leds(char green){
00040     green_led_states = green;
00041     IF_update_leds();
00042 }
00043 
00044 void set_red_leds(char red){
00045     red_led_states = red;
00046     IF_update_leds();
00047 }
00048 
00049 void set_led(char led, char state){
00050     if(state % 2 == 1) red_led_states |= 1 << led;
00051     else red_led_states &= ~(1 << led);
00052     if(state / 2) green_led_states |= 1 << led;
00053     else green_led_states &= ~(1 << led);
00054     IF_update_leds();   
00055 }
00056 
00057 void blink_leds(float timeout){
00058     save_led_states();
00059     set_leds(0xFF,0xFF);
00060     blink_led_timeout.attach(&restore_led_states, timeout);
00061 }
00062 
00063 void set_center_led(char state){
00064     set_center_led(state, center_led_brightness);
00065 }
00066 
00067 void set_center_led(char state, float brightness){
00068     center_led_brightness = brightness;
00069     center_led_state = state;
00070     switch(center_led_state){
00071         case 0: center_led_red.write(0);
00072                 center_led_green.write(0);
00073                 break;
00074         case 1: center_led_red.write(center_led_brightness / 4);
00075                 center_led_green.write(0);
00076                 break;
00077         case 2: center_led_red.write(0);
00078                 center_led_green.write(center_led_brightness);
00079                 break;
00080         case 3: center_led_red.write(center_led_brightness / 4);
00081                 center_led_green.write(center_led_brightness);
00082                 break;
00083     }
00084 }
00085 
00086 void set_center_led_brightness(float brightness){
00087     set_center_led(center_led_state,brightness);
00088 }
00089 
00090 void save_led_states(){
00091     held_red_states = red_led_states;
00092     held_green_states = green_led_states;   
00093 }
00094 
00095 void restore_led_states(){
00096     set_leds(held_green_states,held_red_states);
00097 }
00098 
00099 void IF_init_leds(){
00100     green_led_states = 0;
00101     red_led_states = 0;
00102     center_led_red.period_us(100);
00103     center_led_green.period_us(100); 
00104     set_center_led(0,0.2);
00105 }
00106 
00107 void IF_update_leds(){
00108     char led_byte_0 =  (((green_led_states & (1 << 3)) == 0) << 7) +
00109                        (((green_led_states & (1 << 2)) == 0) << 5) +
00110                        (((green_led_states & (1 << 1)) == 0) << 3) +
00111                        (((green_led_states & (1)) == 0) << 1) +
00112                        (((red_led_states & (1 << 3)) == 0) << 6) +
00113                        (((red_led_states & (1 << 2)) == 0) << 4) +
00114                        (((red_led_states & (1 << 1)) == 0) << 2) +
00115                        (((red_led_states & (1)) == 0));
00116     char led_byte_1 =  (((green_led_states & (1 << 7)) == 0) << 7) +
00117                        (((green_led_states & (1 << 6)) == 0) << 5) +
00118                        (((green_led_states & (1 << 5)) == 0) << 3) +
00119                        (((green_led_states & (1 << 4)) == 0) << 1) +
00120                        (((red_led_states & (1 << 7)) == 0) << 6) +
00121                        (((red_led_states & (1 << 6)) == 0) << 4) +
00122                        (((red_led_states & (1 << 5)) == 0) << 2) +
00123                        (((red_led_states & (1 << 4)) == 0));  
00124     IF_write_to_led_ic(led_byte_0,led_byte_1);            
00125 }