-/*
+/*
* 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
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 */
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) {
+
+ /* no more balls (sensor is heavily filtered) */
+ if (!sensor_get(S_LOW_BARRIER)) {
+ STMCH_DEBUG("%s(): no more balls", __FUNCTION__);
+ break;
+ }
+
+ /* timeout */
+ if ((time_get_us2() - us) > 1000UL * 1000UL) {
+ STMCH_DEBUG("%s(): eject timeout", __FUNCTION__);
+ blocked = 1;
+ break;
+ }
+
+ /* blocking ! */
+ if (bd_get(&ballboard.roller.bd)) {
+ blocked = 1;
+ break;
+ }
+ }
+
+ 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 */
switch (state_mode) {
case INIT:
- state_mode = OFF;
state_init();
+ state_mode = OFF;
+ state_status = I2C_BALLBOARD_STATUS_F_READY;
break;
case OFF:
+ state_status = I2C_BALLBOARD_STATUS_F_READY;
roller_off();
break;
case HARVEST:
+ state_status = I2C_BALLBOARD_STATUS_F_READY;
state_do_harvest();
break;
case EJECT:
- state_mode = HARVEST;
+ state_status = I2C_BALLBOARD_STATUS_F_BUSY;
state_do_eject();
+ state_status = I2C_BALLBOARD_STATUS_F_READY;
+ state_mode = HARVEST;
break;
default:
{
vt100_init(&local_vt100);
state_mode = 0;
+ state_status = I2C_BALLBOARD_STATUS_F_READY;
ball_count = 0;
}