From 844edc0f018c2bdae308f1a005837acb7d2aa84d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 May 2010 08:51:29 +0200 Subject: digital/io/src: add slow motion for corn --- digital/io/src/ai_top_cb.c | 47 +++++++++++++++++++++++++++++++++++++++++++++- digital/io/src/bot.h | 3 +++ digital/io/src/food.c | 12 +++++++++++- digital/io/src/food.h | 4 ++++ digital/io/src/top.c | 10 ++++++---- digital/io/src/top.fsm | 13 +++++++++++++ digital/io/src/top.h | 3 +++ 7 files changed, 86 insertions(+), 6 deletions(-) (limited to 'digital') diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index f8596c1f..c2d05012 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -33,6 +33,7 @@ #include "playground.h" #include "asserv.h" #include "loader.h" +#include "food.h" /* * IDLE =start=> @@ -245,11 +246,21 @@ ai__UNLOAD_UNLOAD__state_timeout (void) * COLLECT =move_fsm_succeed=> * unload => UNLOAD * collect => COLLECT + * slow_motion => COLLECT_SLOW_MOTION + * slow down + * move to food */ fsm_branch_t ai__COLLECT__move_fsm_succeed (void) { - if (top_collect (0)) + int16_t slow_motion = food_slow_motion (top_food); + if (slow_motion) + { + asserv_set_speed (BOT_MOVE_SNAYLE); + asserv_move_linearly (slow_motion); + return ai_next_branch (COLLECT, move_fsm_succeed, slow_motion); + } + else if (top_collect (0)) return ai_next_branch (COLLECT, move_fsm_succeed, collect); else return ai_next_branch (COLLECT, move_fsm_succeed, unload); @@ -269,3 +280,37 @@ ai__COLLECT__move_fsm_failed (void) return ai_next_branch (COLLECT, move_fsm_failed, unload); } +/* + * COLLECT_SLOW_MOTION =bot_move_succeed=> + * unload => UNLOAD + * collect => COLLECT + * speed up + * collect + */ +fsm_branch_t +ai__COLLECT_SLOW_MOTION__bot_move_succeed (void) +{ + asserv_set_speed (BOT_MOVE_SLOW); + if (top_collect (0)) + return ai_next_branch (COLLECT_SLOW_MOTION, bot_move_succeed, collect); + else + return ai_next_branch (COLLECT_SLOW_MOTION, bot_move_succeed, unload); +} + +/* + * COLLECT_SLOW_MOTION =bot_move_failed=> + * unload => UNLOAD + * collect => COLLECT + * same as above + */ +fsm_branch_t +ai__COLLECT_SLOW_MOTION__bot_move_failed (void) +{ + asserv_set_speed (BOT_MOVE_SLOW); + if (top_collect (0)) + return ai_next_branch (COLLECT_SLOW_MOTION, bot_move_failed, collect); + else + return ai_next_branch (COLLECT_SLOW_MOTION, bot_move_failed, unload); +} + + diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h index 6c455906..65fa6e81 100644 --- a/digital/io/src/bot.h +++ b/digital/io/src/bot.h @@ -73,6 +73,9 @@ /** Slow parameters. */ #define BOT_MOVE_SLOW 0x20, 0x20, 0x10, 0x10 +/** Very slow parameters. */ +#define BOT_MOVE_SNAYLE 0x8, 0x8, 0x8, 0x8 + /** Elevator stroke in steps. */ #define BOT_ELEVATOR_STROKE_STEP 7089 diff --git a/digital/io/src/food.c b/digital/io/src/food.c index c081e4ae..3096a2ad 100644 --- a/digital/io/src/food.c +++ b/digital/io/src/food.c @@ -228,7 +228,7 @@ food_shorten (uint8_t food) int16_t shorten = 0; /* Corns. */ if (food_table[food].type == FOOD_TYPE_CORN) - shorten = BOT_SIZE_FRONT + 50; + shorten = BOT_SIZE_FRONT + 70; /* Food on playground sides. */ if (food <= 4 || (food >= 14 && food <= 18)) shorten = BOT_SIZE_FRONT; @@ -255,3 +255,13 @@ food_taken (position_t robot_pos) } } +int16_t +food_slow_motion (uint8_t food) +{ + assert (food < UTILS_COUNT (food_table)); + if (food_table[food].type == FOOD_TYPE_CORN) + return 50; + else + return 0; +} + diff --git a/digital/io/src/food.h b/digital/io/src/food.h index cc8ba4f1..aa47f322 100644 --- a/digital/io/src/food.h +++ b/digital/io/src/food.h @@ -63,4 +63,8 @@ food_taken (position_t robot_pos); int16_t food_shorten (uint8_t food); +/** Slow motion distance to be used to pick this food. */ +int16_t +food_slow_motion (uint8_t food); + #endif /* food_h */ diff --git a/digital/io/src/top.c b/digital/io/src/top.c index a93496c1..7222bcc1 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -32,6 +32,8 @@ #include "chrono.h" #include "playground.h" +uint8_t top_food; + /** Maximum elements to load before unloading. */ #define TOP_LOADER_MAX 3 /** Time to reserve for unloading at end of round. */ @@ -52,12 +54,12 @@ top_collect (uint8_t force) { position_t robot_position; asserv_get_position (&robot_position); - uint8_t food = food_best (robot_position); - if (food == 0xff) + top_food = food_best (robot_position); + if (top_food == 0xff) return 0; vect_t food_v; - food_pos (food, &food_v); - move_start_noangle (food_v, 0, food_shorten (food)); + food_pos (top_food, &food_v); + move_start_noangle (food_v, 0, food_shorten (top_food)); return 1; } else diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index a105e87d..5801932e 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -25,6 +25,8 @@ States: unloading, wait COLLECT collecting elements + COLLECT_SLOW_MOTION + slowly move toward element to collect Events: start @@ -113,6 +115,17 @@ UNLOAD_UNLOAD: COLLECT: move_fsm_succeed: unload -> UNLOAD move_fsm_succeed: collect -> COLLECT + move_fsm_succeed: slow_motion -> COLLECT_SLOW_MOTION + slow down + move to food move_fsm_failed: unload -> UNLOAD move_fsm_failed: collect -> COLLECT +COLLECT_SLOW_MOTION: + bot_move_succeed: unload -> UNLOAD + bot_move_succeed: collect -> COLLECT + speed up + collect + bot_move_failed: unload -> UNLOAD + bot_move_failed: collect -> COLLECT + same as above diff --git a/digital/io/src/top.h b/digital/io/src/top.h index 7595a24c..f5ce89ff 100644 --- a/digital/io/src/top.h +++ b/digital/io/src/top.h @@ -25,6 +25,9 @@ * * }}} */ +/** Currently collected food. */ +extern uint8_t top_food; + /** * Initialize top module. */ -- cgit v1.2.3