french cup + idf cup

[aversive.git] / projects / microb2010 / ballboard / state.c

diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c
index a322d17..edcfb3a 100644 (file)
--- 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;
                }