From 8a98a550ef7bf6e242538e01e0244443e4e79682 Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Fri, 30 Apr 2010 08:34:15 +0200
Subject: [PATCH] vidle: Handle color switch

---
 src/eb_vidle/main.c | 31 ++++++++++++++++++-------------
 1 file changed, 18 insertions(+), 13 deletions(-)

diff --git a/src/eb_vidle/main.c b/src/eb_vidle/main.c
index 7226d5f2..af8ec078 100644
--- 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);
 	
-- 
2.39.2