Library for the PsiSwarm Robot for Headstart Lab - Version 0.5

Dependents:   UKESF_Lab

Fork of PsiSwarmLibrary by James Hilder

Revision:
0:d6269d17c8cf
Child:
2:c6986ee3c7c5
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/led.cpp	Thu Feb 04 21:48:54 2016 +0000
@@ -0,0 +1,125 @@
+/* University of York Robotics Laboratory PsiSwarm Library: LED Functions Source File
+ * 
+ * File: led.cpp
+ *
+ * (C) Dept. Electronics & Computer Science, University of York
+ * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
+ *
+ * PsiSwarm Library Version: 0.4
+ *
+ * February 2016
+ *
+ *
+ */
+
+#include "psiswarm.h"
+
+char green_led_states;
+char red_led_states;
+char center_led_state;
+
+char held_red_states;
+char held_green_states;
+Timeout blink_led_timeout;
+
+unsigned short get_led_states(){
+    return (green_led_states << 8) + red_led_states;  
+}
+
+void set_leds(char green, char red){
+    green_led_states = green;
+    red_led_states = red;
+    IF_update_leds();
+}
+
+void set_base_led(char state){
+    IF_set_base_LED(state); 
+}
+
+void set_green_leds(char green){
+    green_led_states = green;
+    IF_update_leds();
+}
+
+void set_red_leds(char red){
+    red_led_states = red;
+    IF_update_leds();
+}
+
+void set_led(char led, char state){
+    if(state % 2 == 1) red_led_states |= 1 << led;
+    else red_led_states &= ~(1 << led);
+    if(state / 2) green_led_states |= 1 << led;
+    else green_led_states &= ~(1 << led);
+    IF_update_leds();   
+}
+
+void blink_leds(float timeout){
+    save_led_states();
+    set_leds(0xFF,0xFF);
+    blink_led_timeout.attach(&restore_led_states, timeout);
+}
+
+void set_center_led(char state){
+    set_center_led(state, center_led_brightness);
+}
+
+void set_center_led(char state, float brightness){
+    center_led_brightness = brightness;
+    center_led_state = state;
+    switch(center_led_state){
+        case 0: center_led_red.write(0);
+                center_led_green.write(0);
+                break;
+        case 1: center_led_red.write(center_led_brightness / 4);
+                center_led_green.write(0);
+                break;
+        case 2: center_led_red.write(0);
+                center_led_green.write(center_led_brightness);
+                break;
+        case 3: center_led_red.write(center_led_brightness / 4);
+                center_led_green.write(center_led_brightness);
+                break;
+    }
+}
+
+void set_center_led_brightness(float brightness){
+    set_center_led(center_led_state,brightness);
+}
+
+void save_led_states(){
+    held_red_states = red_led_states;
+    held_green_states = green_led_states;   
+}
+
+void restore_led_states(){
+    set_leds(held_green_states,held_red_states);
+}
+
+void IF_init_leds(){
+    green_led_states = 0;
+    red_led_states = 0;
+    center_led_red.period_us(100);
+    center_led_green.period_us(100); 
+    set_center_led(0,0.2);
+}
+
+void IF_update_leds(){
+    char led_byte_0 =  (((green_led_states & (1 << 3)) == 0) << 7) +
+                       (((green_led_states & (1 << 2)) == 0) << 5) +
+                       (((green_led_states & (1 << 1)) == 0) << 3) +
+                       (((green_led_states & (1)) == 0) << 1) +
+                       (((red_led_states & (1 << 3)) == 0) << 6) +
+                       (((red_led_states & (1 << 2)) == 0) << 4) +
+                       (((red_led_states & (1 << 1)) == 0) << 2) +
+                       (((red_led_states & (1)) == 0));
+    char led_byte_1 =  (((green_led_states & (1 << 7)) == 0) << 7) +
+                       (((green_led_states & (1 << 6)) == 0) << 5) +
+                       (((green_led_states & (1 << 5)) == 0) << 3) +
+                       (((green_led_states & (1 << 4)) == 0) << 1) +
+                       (((red_led_states & (1 << 7)) == 0) << 6) +
+                       (((red_led_states & (1 << 6)) == 0) << 4) +
+                       (((red_led_states & (1 << 5)) == 0) << 2) +
+                       (((red_led_states & (1 << 4)) == 0));  
+    IF_write_to_led_ic(led_byte_0,led_byte_1);            
+}
\ No newline at end of file