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

diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c
index 08c104d..a322d17 100644
--- 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 */
@@ -122,23 +123,81 @@ 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) {
+
+			/* 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 */
@@ -156,21 +215,26 @@ void state_machine(void)
 		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:
@@ -183,5 +247,6 @@ void state_init(void)
 {
 	vt100_init(&local_vt100);
 	state_mode = 0;
+	state_status = I2C_BALLBOARD_STATUS_F_READY;
 	ball_count = 0;
 }