X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;f=projects%2Fmicrob2010%2Fballboard%2Fstate.c;h=edcfb3abd3c90ed7c7058d4672287c712c2981c6;hb=HEAD;hp=a322d17c8615eb885d5c429099f48cbbe90eb086;hpb=063af91cd38fceba6cbcf3a9b8abd4c48684d990;p=aversive.git

diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c
index a322d17..edcfb3a 100644
--- a/projects/microb2010/ballboard/state.c
+++ b/projects/microb2010/ballboard/state.c
@@ -64,10 +64,8 @@ static volatile uint8_t ball_count;
 #define OFF I2C_BALLBOARD_MODE_OFF
 #define HARVEST I2C_BALLBOARD_MODE_HARVEST
 #define EJECT I2C_BALLBOARD_MODE_EJECT
-#define PREP_L_FORK I2C_BALLBOARD_MODE_PREP_L_FORK
-#define TAKE_L_FORK I2C_BALLBOARD_MODE_TAKE_L_FORK
-#define PREP_R_FORK I2C_BALLBOARD_MODE_PREP_R_FORK
-#define TAKE_R_FORK I2C_BALLBOARD_MODE_TAKE_R_FORK
+#define PREP_FORK I2C_BALLBOARD_MODE_PREP_FORK
+#define TAKE_FORK I2C_BALLBOARD_MODE_TAKE_FORK
 
 uint8_t state_debug = 0;
 
@@ -168,15 +166,21 @@ static void state_do_eject(void)
 		blocked = 0;
 
 		while (1) {
+			/* move fork during ball ejection */
+			if ((us % 600) < 300)
+				fork_eject();
+			else
+				fork_pack();
 
 			/* no more balls (sensor is heavily filtered) */
-			if (!sensor_get(S_LOW_BARRIER)) {
+			if (!sensor_get(S_LOW_BARRIER) &&
+			    !sensor_get(S_HIGH_BARRIER)) {
 				STMCH_DEBUG("%s(): no more balls", __FUNCTION__);
 				break;
 			}
 
 			/* timeout */
-			if ((time_get_us2() - us) > 1000UL * 1000UL) {
+			if ((time_get_us2() - us) > 2000UL * 1000UL) {
 				STMCH_DEBUG("%s(): eject timeout", __FUNCTION__);
 				blocked = 1;
 				break;
@@ -188,6 +192,7 @@ static void state_do_eject(void)
 				break;
 			}
 		}
+		fork_pack();
 
 		if (!blocked)
 			break;
@@ -216,6 +221,7 @@ void state_machine(void)
 
 		case INIT:
 			state_init();
+			fork_pack();
 			state_mode = OFF;
 			state_status = I2C_BALLBOARD_STATUS_F_READY;
 			break;
@@ -223,20 +229,48 @@ void state_machine(void)
 		case OFF:
 			state_status = I2C_BALLBOARD_STATUS_F_READY;
 			roller_off();
+			fork_pack();
 			break;
 
 		case HARVEST:
 			state_status = I2C_BALLBOARD_STATUS_F_READY;
+			fork_pack();
 			state_do_harvest();
 			break;
 
 		case EJECT:
 			state_status = I2C_BALLBOARD_STATUS_F_BUSY;
+			fork_pack();
 			state_do_eject();
 			state_status = I2C_BALLBOARD_STATUS_F_READY;
 			state_mode = HARVEST;
 			break;
 
+		case PREP_FORK:
+			roller_off();
+			fork_deploy();
+			break;
+
+		case TAKE_FORK:
+			roller_off();
+			fork_mid1();
+			time_wait_ms(666);
+			fork_mid2();
+			time_wait_ms(666);
+			while (1) {
+				uint8_t packed;
+
+				fork_pack();
+				packed = WAIT_COND_OR_TIMEOUT(fork_is_packed(),
+							      500);
+				if (packed)
+					break;
+				fork_mid2();
+				time_wait_ms(200);
+			}
+			state_mode = OFF;
+			break;
+
 		default:
 			break;
 		}