From 323ad8abc26d3fe2f89c07a36288469ad6a4139f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 May 2010 12:25:27 +0200 Subject: digital/io/src: handle black corn --- digital/io/src/ai_loader_cb.c | 7 +++++-- digital/io/src/ai_top_cb.c | 44 +++++++++++++++++++++++++++++++++++++++++++ digital/io/src/bot.h | 4 ++++ digital/io/src/food.c | 14 ++++++++++++-- digital/io/src/loader.fsm | 4 +++- digital/io/src/top.fsm | 12 ++++++++++++ 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 -- cgit v1.2.3