X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fballboard%2Fstate.c;h=edcfb3abd3c90ed7c7058d4672287c712c2981c6;hp=6c229a40b9607a51d2413b3c6f80c7884631efad;hb=04f1061f2c5959ce87f632591576097f540dfc14;hpb=e6e4582229cd4ed8ffe73bdfbae79d236129df3b

diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c
index 6c229a4..edcfb3a 100644
--- a/projects/microb2010/ballboard/state.c
+++ b/projects/microb2010/ballboard/state.c
@@ -166,9 +166,15 @@ 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;
 			}
@@ -186,6 +192,7 @@ static void state_do_eject(void)
 				break;
 			}
 		}
+		fork_pack();
 
 		if (!blocked)
 			break;
@@ -246,8 +253,21 @@ void state_machine(void)
 
 		case TAKE_FORK:
 			roller_off();
-			fork_mid();
-			time_wait_ms(1300);
+			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;