eb_sberac added puck sensor detection

diff --git a/src/eb_sberac_09/Makefile.omk b/src/eb_sberac_09/Makefile.omk
index c057a3cd1ca98e5cdcbeaba540a8012a662d87b2..5afec76ed50fa62745f7ba012dc209905e0c281e 100644 (file)
--- a/src/eb_sberac_09/Makefile.omk
+++ b/src/eb_sberac_09/Makefile.omk
@@ -5,4 +5,6 @@ bin_PROGRAMS = eb_sberac_09
 eb_sberac_09_SOURCES = main.c 
 eb_sberac_09_LIBS = can ebb uart_nozen
 
+link_VARIANTS = flash
+
 

diff --git a/src/eb_sberac_09/main.c b/src/eb_sberac_09/main.c
index bdf0c061070228d9f3768742505beba432cefe87..81f4b85e6df35b27c038ea676f5676d0a754ae18 100644 (file)
--- a/src/eb_sberac_09/main.c
+++ b/src/eb_sberac_09/main.c
@@ -73,7 +73,7 @@
 
 
 
-#define START_TIME     50
+#define START_TIME     16
 
 
 
@@ -189,7 +189,7 @@ void go_to_sleep()
        deb_led_off(LEDY);
        deb_led_off(LEDB);
        deb_led_off(LEDG);
-       deb_led_on(LEDR);
+//     deb_led_on(LEDR);
        PCON = 2;                               // bye bye 
 }
 
@@ -203,9 +203,7 @@ void can_rx(can_msg_t *msg) {
        switch (rx_msg.id) {            
                case CAN_CHELAE:
                        deb_led_on(LEDB);
-                       time_timeout = time_ms + TIMEOUT_DELAY;
-                       deb_led_off(LEDY);
-                       
+                       time_timeout = time_ms + TIMEOUT_DELAY;                 
                        set_servo(CHELAE_L , CHELAE_L_MIN + rx_msg.data[0] * (CHELAE_L_MAX - CHELAE_L_MIN)/255 );
                        set_servo(CHELAE_R , CHELAE_R_MIN + (0xFF - rx_msg.data[1]) * (CHELAE_R_MAX - CHELAE_R_MIN)/255 );
 
@@ -221,7 +219,6 @@ void can_rx(can_msg_t *msg) {
                        
                        deb_led_on(LEDB);
                        time_timeout = time_ms + TIMEOUT_DELAY;
-                       deb_led_off(LEDY);
                        set_pwm(PWM_CHAN_B, rx_msg.data[0]);
                        set_pwm(PWM_CHAN_A, rx_msg.data[1]);
                        break;
@@ -278,26 +275,65 @@ void init_perip(void)        // inicializace periferii mikroprocesoru
 
 void timeout(void)
 {
-       set_servo(CHELAE_L , CHELAE_L_MIN) ;
-       set_servo(CHELAE_R , CHELAE_R_MAX) ;
-       set_servo(HOKUYO , HOKUYO_AHEAD) ;
+//     set_servo(CHELAE_L , CHELAE_L_MIN) ;
+//     set_servo(CHELAE_R , CHELAE_R_MAX) ;
+//     set_servo(HOKUYO , HOKUYO_AHEAD) ;
        engine_A_pwm(0);
        engine_B_pwm(0);
 }
 
 
 
+#define PUCK_PIN       18      // MISO1
+uint32_t time_puck = 0;
+#define  PUCK_TIME     50
+
+void puck_switch(void)
+{
+    if (time_puck == 0) time_puck= time_ms + PUCK_TIME;        
+    
+    if (time_ms > time_puck)
+    {
+      time_puck = time_ms + PUCK_TIME;
+      
+      can_msg_t msg_puck; 
+       msg_puck.id = CAN_PUCK;
+       msg_puck.dlc = 1;
+       
+      if(IO0PIN & (1<< PUCK_PIN))
+      {
+       msg_puck.data[0] = 0;
+       deb_led_off(LEDR);
+      }
+      else
+      {
+       msg_puck.data[0] = 1;
+       deb_led_on(LEDR);
+      }
+      
+      while (can_tx_msg(&msg_puck));
+       
+    }
+}
+
+
 uint8_t start_bt = 0;
 uint32_t time_start = 0;
+uint8_t start_cnt = 0;
 #define START_PIN      15              // start pin
 
 void start_button(void)
 {
        if (start_bt > 10) 
        {
-               if ((IO0PIN & (1<<START_PIN))==0) start_bt=0;
+               if ((IO0PIN & (1<<START_PIN))==0) 
+               {       
+                 start_bt = 0;
+                 start_cnt = 0;
+               }
                return;
        }
+       
        if (time_start == 0) time_start = time_ms + START_TIME; 
        
        
@@ -308,20 +344,24 @@ void start_button(void)
                
                if (IO0PIN & (1<<START_PIN)) 
                {
-                       ++start_bt;
-                       msg.data[0] = 1;
-                       deb_led_off(LEDY);
+                       ++start_cnt;
+                       if(start_cnt > 3)
+                       {
+                         ++start_bt;
+                         msg.data[0] = 1;
+                         deb_led_off(LEDY);
+                       }
                }
                else 
                {
                        deb_led_on(LEDY);
                        msg.data[0] = 0;
+                       start_cnt = 0;
                }
 
                msg.id = CAN_ROBOT_CMD;
                msg.flags = 0;
                msg.dlc = 1;
-               //msg.data[0] = (~(IO0PIN >> START_PIN)) & 1;
                while (can_tx_msg(&msg));
        }
 }
@@ -355,18 +395,20 @@ int main (void)  {
 
        
        SET_PIN(PINSEL0, START_PIN, PINSEL_0);
+       SET_PIN(PINSEL1, (PUCK_PIN - 15), PINSEL_0);
 
        while(1)
        {
 
-//             if( time_ms > time_timeout) 
-//             {
-//                     timeout();
-//                     deb_led_on(LEDY);
-//             }
+               if( time_ms > time_timeout) 
+               {
+                       timeout();
+                       //deb_led_on(LEDY);
+               }
                start_button();
                led_blik(LEDG);
                send_adc();
+               puck_switch();
 
                if(adc_update_adc & 1)  adc_filter(0);
                if(adc_update_adc & 2)  adc_filter(1);