Diff: robot_logic.cpp

Revision:

5:355240d7126b

Parent:

3:b5d18690f357

Child:

6:0dc4e4225881

--- a/robot_logic.cpp	Tue Feb 24 09:06:29 2015 +0000
+++ b/robot_logic.cpp	Tue Feb 24 09:38:40 2015 +0000
@@ -17,6 +17,7 @@
 static int internal_i = 0;
 static int internal_d = 0;
 static int internal_previous_pos_of_line = 0;
+static int internal_led_state = 0;
 
 /*
  * Drive the robot forward or backward.
@@ -50,7 +51,7 @@
  */
 void turn(int turnspeed) {
     if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) {
-        printf("[ERROR] Turn speed out of range");
+        error("Speed out of range");
         return;
     }
 
@@ -222,4 +223,21 @@
  */
 void await(int milliseconds) {
     wait_ms(milliseconds);
+}
+
+
+void led(LedIndex i, LedState state) {
+    if(state == LED_ON){
+        internal_led_state |= (1 << i);
+    }
+    else if(state == LED_OFF) {
+        internal_led_state &= ~(1 << i);
+    }
+    else if(state == LED_TOGGLE){
+        internal_led_state ^= (1 << i);
+    }
+    else{
+        error("Illegal LED state");
+    }
+    m3pi.leds(internal_led_state);
 }
\ No newline at end of file