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 57d44ff..edcfb3a 100644 (file)
--- a/projects/microb2010/ballboard/state.c
+++ b/projects/microb2010/ballboard/state.c
@@ -1,6 +1,6 @@
-/*  
+/*
  *  Copyright Droids Corporation (2009)
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -56,6 +56,7 @@
 
 static struct vt100 local_vt100;
 static volatile uint8_t state_mode;
+static volatile uint8_t state_status;
 static volatile uint8_t ball_count;
 
 /* short aliases */
@@ -63,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;
 
@@ -122,48 +121,154 @@ static uint8_t state_want_exit(void)
        return 0;
 }
 
-uint8_t state_get_mode(void)
+uint8_t state_get_status(void)
 {
-       return state_mode;
+       return state_status;
 }
 
 /* harvest balls from area */
 static void state_do_harvest(void)
 {
        //state_debug_wait_key_pressed();
-       roller_on();
+
+       if (bd_get(&ballboard.roller.bd)) {
+               STMCH_DEBUG("%s(): roller blocked", __FUNCTION__);
+               roller_reverse();
+               time_wait_ms(500);
+               bd_reset(&ballboard.roller.bd);
+               STMCH_DEBUG("%s(): roller restart", __FUNCTION__);
+               return;
+       }
+
+       /* deduct ball count */
+       if (sensor_get(S_LOW_BARRIER) && !sensor_get(S_HIGH_BARRIER))
+               ball_count = 1;
+       else if (sensor_get(S_LOW_BARRIER) && sensor_get(S_HIGH_BARRIER))
+               ball_count = 3;
+
+       if (sensor_get(S_HIGH_BARRIER))
+               roller_off();
+       else
+               roller_on();
 }
 
 /* eject balls */
 static void state_do_eject(void)
 {
-       roller_reverse();
-       time_wait_ms(2000);
+       uint8_t i, blocked;
+       microseconds us;
+
+       for (i = 0; i < 3; i ++) {
+
+               roller_reverse();
+
+               us = time_get_us2();
+               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) &&
+                           !sensor_get(S_HIGH_BARRIER)) {
+                               STMCH_DEBUG("%s(): no more balls", __FUNCTION__);
+                               break;
+                       }
+
+                       /* timeout */
+                       if ((time_get_us2() - us) > 2000UL * 1000UL) {
+                               STMCH_DEBUG("%s(): eject timeout", __FUNCTION__);
+                               blocked = 1;
+                               break;
+                       }
+
+                       /* blocking ! */
+                       if (bd_get(&ballboard.roller.bd)) {
+                               blocked = 1;
+                               break;
+                       }
+               }
+               fork_pack();
+
+               if (!blocked)
+                       break;
+
+               STMCH_DEBUG("%s(): roller blocked", __FUNCTION__);
+               roller_on();
+               time_wait_ms(500);
+               bd_reset(&ballboard.roller.bd);
+               STMCH_DEBUG("%s(): roller restart", __FUNCTION__);
+       }
 }
 
 /* main state machine */
 void state_machine(void)
 {
+       uint8_t mode = 0;
+
        while (state_want_exit() == 0) {
 
+               if (state_mode != mode) {
+                       mode = state_mode;
+                       STMCH_DEBUG("%s(): mode=%x ", __FUNCTION__, mode);
+               }
+
                switch (state_mode) {
 
                case INIT:
-                       state_mode = OFF;
                        state_init();
+                       fork_pack();
+                       state_mode = OFF;
+                       state_status = I2C_BALLBOARD_STATUS_F_READY;
                        break;
 
                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_mode = HARVEST;
+                       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:
@@ -176,5 +281,6 @@ void state_init(void)
 {
        vt100_init(&local_vt100);
        state_mode = 0;
+       state_status = I2C_BALLBOARD_STATUS_F_READY;
        ball_count = 0;
 }