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