Sille Van Landschoot
/
RacingRobotsLib
Racing Robots Session
Fork of racing_robots by
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