summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-14 12:25:27 +0200
committerNicolas Schodet2010-05-14 12:25:27 +0200
commit323ad8abc26d3fe2f89c07a36288469ad6a4139f (patch)
treebd375d40ea96e8100c26726f13c0d0dbe964eee7 /digital
parent050f746a20e818660102a94c5107788a491fcac3 (diff)
digital/io/src: handle black corn
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_loader_cb.c7
-rw-r--r--digital/io/src/ai_top_cb.c44
-rw-r--r--digital/io/src/bot.h4
-rw-r--r--digital/io/src/food.c14
-rw-r--r--digital/io/src/loader.fsm4
-rw-r--r--digital/io/src/top.fsm12
6 files changed, 80 insertions, 5 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index 632b7504..b556c9eb 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -449,13 +449,16 @@ ai__LOADER_LOAD_UPING__elevator_unload_position (void)
/*
* LOADER_LOAD_UPING =elevator_failed=>
* => LOADER_ERROR
- * post loader_errored event
+ * post loader_errored or loader_black event
* open clamp
*/
fsm_branch_t
ai__LOADER_LOAD_UPING__elevator_failed (void)
{
- main_post_event (AI_EVENT_loader_errored);
+ if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP)
+ main_post_event (AI_EVENT_loader_black);
+ else
+ main_post_event (AI_EVENT_loader_errored);
mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
return ai_next (LOADER_LOAD_UPING, elevator_failed);
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index c2d05012..2041ab7b 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -313,4 +313,48 @@ ai__COLLECT_SLOW_MOTION__bot_move_failed (void)
return ai_next_branch (COLLECT_SLOW_MOTION, bot_move_failed, unload);
}
+/*
+ * COLLECT_SLOW_MOTION =loader_black=>
+ * => COLLECT_BLACK
+ * speed up
+ * move backward
+ * mark as black
+ */
+fsm_branch_t
+ai__COLLECT_SLOW_MOTION__loader_black (void)
+{
+ asserv_set_speed (BOT_MOVE_SLOW);
+ asserv_move_linearly (-90);
+ food_black (top_food);
+ return ai_next (COLLECT_SLOW_MOTION, loader_black);
+}
+
+/*
+ * COLLECT_BLACK =bot_move_succeed=>
+ * unload => UNLOAD
+ * collect => COLLECT
+ */
+fsm_branch_t
+ai__COLLECT_BLACK__bot_move_succeed (void)
+{
+ if (top_collect (0))
+ return ai_next_branch (COLLECT_BLACK, bot_move_succeed, collect);
+ else
+ return ai_next_branch (COLLECT_BLACK, bot_move_succeed, unload);
+}
+
+/*
+ * COLLECT_BLACK =bot_move_failed=>
+ * unload => UNLOAD
+ * collect => COLLECT
+ */
+fsm_branch_t
+ai__COLLECT_BLACK__bot_move_failed (void)
+{
+ if (top_collect (0))
+ return ai_next_branch (COLLECT_BLACK, bot_move_failed, collect);
+ else
+ return ai_next_branch (COLLECT_BLACK, bot_move_failed, unload);
+}
+
diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h
index 58b7d6c0..1349de3a 100644
--- a/digital/io/src/bot.h
+++ b/digital/io/src/bot.h
@@ -85,6 +85,10 @@
/** Elevator resting up position in steps. */
#define BOT_ELEVATOR_REST_STEP 4111
+/** Elevator position under which a blocking is considered as due to a black
+ * corn in steps. */
+#define BOT_ELEVATOR_BLACK_THRESHOLD_STEP 3000
+
/** Elevator down position in steps. */
#define BOT_ELEVATOR_DOWN_STEP 160
diff --git a/digital/io/src/food.c b/digital/io/src/food.c
index 3096a2ad..e2cb462d 100644
--- a/digital/io/src/food.c
+++ b/digital/io/src/food.c
@@ -46,6 +46,7 @@ enum food_type_t
{
FOOD_TYPE_TOMATO,
FOOD_TYPE_CORN,
+ FOOD_TYPE_CORN_BLACK,
};
/** Food information. */
@@ -111,7 +112,7 @@ uint8_t
food_blocking (uint8_t food)
{
assert (food < UTILS_COUNT (food_table) || food == 0xff);
- return food != 0xff && food_table[food].type == FOOD_TYPE_CORN;
+ return food != 0xff && food_table[food].type != FOOD_TYPE_TOMATO;
}
uint8_t
@@ -165,8 +166,10 @@ food_score (position_t robot_pos, uint8_t food)
/* Type of food. */
if (food_table[food].type == FOOD_TYPE_TOMATO)
score += 100;
+ else if (food_table[food].type == FOOD_TYPE_CORN)
+ score -= 500;
else
- score -= 200;
+ score -= 100000;
/* Distance to robot. */
food_pos (food, &v);
int32_t dr = distance_point_point (&v, &robot_pos.v);
@@ -265,3 +268,10 @@ food_slow_motion (uint8_t food)
return 0;
}
+void
+food_black (uint8_t food)
+{
+ assert (food < UTILS_COUNT (food_table));
+ food_table[food].valid = 1;
+ food_table[food].type = FOOD_TYPE_CORN_BLACK;
+}
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index 410021b9..fb37dc81 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -79,6 +79,8 @@ Events:
posted when loader successfully moved up
loader_errored
posted when loader in an unknown state due to error while moving up
+ loader_black
+ loader blocked, due to a black corn
LOADER_IDLE:
start -> LOADER_WAIT_JACK_IN
@@ -189,7 +191,7 @@ LOADER_LOAD_UPING:
elevator_unload_position -> LOADER_LOAD_UNLOADING
open clamp
elevator_failed -> LOADER_ERROR
- post loader_errored event
+ post loader_errored or loader_black event
open clamp
LOADER_LOAD_UNLOADING:
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index 5801932e..c33923f6 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -27,6 +27,8 @@ States:
collecting elements
COLLECT_SLOW_MOTION
slowly move toward element to collect
+ COLLECT_BLACK
+ moving backward after trying to collect a black corn
Events:
start
@@ -129,3 +131,13 @@ COLLECT_SLOW_MOTION:
bot_move_failed: unload -> UNLOAD
bot_move_failed: collect -> COLLECT
same as above
+ loader_black -> COLLECT_BLACK
+ speed up
+ move backward
+ mark as black
+
+COLLECT_BLACK:
+ bot_move_succeed: unload -> UNLOAD
+ bot_move_succeed: collect -> COLLECT
+ bot_move_failed: unload -> UNLOAD
+ bot_move_failed: collect -> COLLECT