vidle: Handle color switch

[eurobot/public.git] / src / eb_vidle / main.c

diff --git a/src/eb_vidle/main.c b/src/eb_vidle/main.c
index 7226d5f2d3323e68c7db7209c77b3a64d178a471..af8ec078f1b4008ba7946c17ddaa6cd1c579493f 100644 (file)
--- a/src/eb_vidle/main.c
+++ b/src/eb_vidle/main.c
@@ -124,12 +124,6 @@ void start_button(void)
        
        start_condition = (IO0PIN & (1<<START_PIN)) == 0;
 
-       if (start_condition)
-               deb_led_on(LEDY);
-       else
-               deb_led_off(LEDY);
-
-
        if (start_condition != last_start_condition) {
                last_start_condition = start_condition;
                count = 0;
@@ -254,6 +248,7 @@ void blink_led()
 
 
 #define BUMPER_PIN     17              // bumper pin  (SCK1)
+#define COLOR_PIN      8               // bumper pin  (SDA/P0.3)
 
 void handle_bumper()
 {
@@ -269,16 +264,25 @@ void handle_bumper()
                        
                        
                if (IO0PIN & (1<<BUMPER_PIN))
-               {    
                        sw &= ~CAN_SWITCH_BUMPER;
-                       deb_led_off(LEDR);
-               }
                else
-               {
                        sw |= CAN_SWITCH_BUMPER;
-                       deb_led_on(LEDR);
-               }
 
+               if (IO0PIN & (1<<COLOR_PIN))
+                       sw |= CAN_SWITCH_COLOR;
+               else
+                       sw &= ~CAN_SWITCH_COLOR;
+
+               if (sw & CAN_SWITCH_COLOR)
+                       deb_led_off(LEDY);
+               else
+                       deb_led_on(LEDY);
+               
+               if (sw & CAN_SWITCH_BUMPER)
+                       deb_led_on(LEDR);
+               else
+                       deb_led_off(LEDR);
+               
                /* TODO: Put color to the second bit */
                        
                tx_msg.id = CAN_ROBOT_SWITCHES;
@@ -309,10 +313,11 @@ int main(void)
        
        
        SET_PIN(PINSEL0, START_PIN, PINSEL_0);          // inicializace start pinu
+       SET_PIN(PINSEL0, COLOR_PIN, PINSEL_0);          // inicializace start pinu
        SET_PIN(PINSEL1, 1, PINSEL_0);          // inicializace bumper pinu (FIXME SET_PIN je BLBA implemetace, musim ji nekdy opravit)
        
        
-       IO0DIR &= ~((1<<START_PIN) | (1<<BUMPER_PIN));
+       IO0DIR &= ~((1<<START_PIN) | (1<<BUMPER_PIN) | (1 << COLOR_PIN));
        
        IO1DIR &= ~(3<<20);