From: Jiri Kubias <jiri.kubias@gmail.com>
Date: Thu, 23 Apr 2009 19:46:07 +0000 (+0200)
Subject: eb_sberac 	added puck sensor detection
X-Git-Tag: eb2009~40^2~2^2~1
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/74cd5a57a712214a8d9d75dc901b1c1f95e01fd4

eb_sberac 	added puck sensor detection
---

diff --git a/src/eb_sberac_09/Makefile.omk b/src/eb_sberac_09/Makefile.omk
index c057a3cd..5afec76e 100644
--- 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 bdf0c061..81f4b85e 100644
--- 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);