eb_jaws_11: recognise three color + home button

diff --git a/src/eb_jaws_11/main.c b/src/eb_jaws_11/main.c
index 1ecb64bb9c804569cd62b8066e8be1a0a73b6580..350d07b98c1bd7beba30718103d929e3e30e6bc4 100644 (file)
--- a/src/eb_jaws_11/main.c
+++ b/src/eb_jaws_11/main.c
@@ -64,6 +64,7 @@
 #define COLOR_PIN1             EXPPORT_5
 #define SWITCH_STRATEGY_PIN    EXPPORT_8
 #define START_PIN              EXPPORT_7
+#define SWITCH_HOME_PIN                EXPPORT_6
        
 
 struct fsm fsm_jaw_right;
@@ -169,10 +170,29 @@ void robot_switches_handler()
                else
                        sw |= CAN_SWITCH_STRATEGY;
                
-               if ((sw & CAN_SWITCH_COLOR_0) || (sw & CAN_SWITCH_COLOR_1)){
+               if (IO0PIN & (1<<SWITCH_HOME_PIN))
+                       sw &= ~CAN_SWITCH_HOME;
+               else
+                       sw |= CAN_SWITCH_HOME;
+               
+               if ((sw & CAN_SWITCH_COLOR_0) && (sw & CAN_SWITCH_COLOR_1)){
                        deb_led_off(LEDY);
                
-                       send_rs_str("color\n");}
+                       send_rs_str("white\n");}
+               else
+                       deb_led_on(LEDY);
+               
+               if ((sw & CAN_SWITCH_COLOR_0) && !(sw & CAN_SWITCH_COLOR_1)){
+                       deb_led_off(LEDY);
+               
+                       send_rs_str("green\n");}
+               else
+                       deb_led_on(LEDY);
+               
+               if (!(sw & CAN_SWITCH_COLOR_0) && (sw & CAN_SWITCH_COLOR_1)){
+                       deb_led_off(LEDY);
+               
+                       send_rs_str("yellow\n");}
                else
                        deb_led_on(LEDY);
                
@@ -183,6 +203,13 @@ void robot_switches_handler()
                }
                else
                        deb_led_on(LEDY);
+               if (sw & CAN_SWITCH_HOME){
+                       deb_led_off(LEDY);
+               
+                       send_rs_str("home\n");
+               }
+               else
+                       deb_led_on(LEDY);
 
 //             send_rs_int(IO1PIN);
 //             send_rs_int(sw);
@@ -534,10 +561,11 @@ int main(void)
        SET_PIN(PINSEL0, START_PIN, PINSEL_0);  
        SET_PIN(PINSEL1, (SWITCH_STRATEGY_PIN-16), PINSEL_0);
        SET_PIN(PINSEL1, (COLOR_PIN1-16), PINSEL_0);
+       SET_PIN(PINSEL1, (SWITCH_HOME_PIN - 16), PINSEL_0);
                        
        PINSEL2 &= ~(1 << 3) ;          //setting P1.25:16 as GPIO pins -> GPIO PORT on eb_board
        IO1DIR &= ~((1<<BUMPER_REAR) | /*(1<<BUMPER_REAR_LEFT) |*/ (1<<BUMPER_RIGHT) | (1<<BUMPER_LEFT) | (1<<BUMPER_RIGHT_ACROSS) | (1<<BUMPER_LEFT_ACROSS));  
-       IO0DIR &= ~((1<<COLOR_PIN0) | (1<<COLOR_PIN1) | (1<<START_PIN) | (1<<SWITCH_STRATEGY_PIN));
+       IO0DIR &= ~((1<<COLOR_PIN0) | (1<<COLOR_PIN1)| (1<<SWITCH_HOME_PIN) | (1<<START_PIN) | (1<<SWITCH_STRATEGY_PIN));
        cl_sensor_init();