From f409e9a7eeef4bbccd782931c296ff9cde079487 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 22:26:49 +0200 Subject: digital/io/src: hacks for first round --- digital/io/src/ai_loader_cb.c | 18 ++++++++++++++++-- digital/io/src/ai_top_cb.c | 24 +++++++++++++++++++----- digital/io/src/loader.c | 1 + digital/io/src/loader.fsm | 7 ++++++- digital/io/src/loader.h | 2 ++ digital/io/src/top.c | 5 ++++- digital/io/src/top.fsm | 7 ++++++- 7 files changed, 54 insertions(+), 10 deletions(-) (limited to 'digital') diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index f486bbc6..f5571fbf 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -408,12 +408,16 @@ ai__LOADER_LOAD_UPING__elevator_failed (void) /* * LOADER_LOAD_UNLOADING =elevator_succeed=> - * => LOADER_LOAD_UNLOADING_OPEN + * choucroute => LOADER_HACK_MATCH1 + * merguez => LOADER_LOAD_UNLOADING_OPEN */ fsm_branch_t ai__LOADER_LOAD_UNLOADING__elevator_succeed (void) { - return ai_next (LOADER_LOAD_UNLOADING, elevator_succeed); + if (loader_choucroute) + return ai_next_branch (LOADER_LOAD_UNLOADING, elevator_succeed, choucroute); + else + return ai_next_branch (LOADER_LOAD_UNLOADING, elevator_succeed, merguez); } /* @@ -450,3 +454,13 @@ ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void) return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_succeed); } +/* + * LOADER_HACK_MATCH1 =state_timeout=> + * => LOADER_LOAD_UNLOADING_OPEN + */ +fsm_branch_t +ai__LOADER_HACK_MATCH1__state_timeout (void) +{ + return ai_next (LOADER_HACK_MATCH1, state_timeout); +} + diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 173df388..c4a698cf 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -307,18 +307,17 @@ ai__UNLOAD_BACK_BIN__bot_move_failed (void) /* * UNLOAD_UNLOAD =state_timeout=> - * => COLLECT + * => HACK_MATCH1 * close gate - * loader down - * choose best food to collect + * move to hack position */ fsm_branch_t ai__UNLOAD_UNLOAD__state_timeout (void) { loader_elements = 0; asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); - loader_down (); - top_collect (1); + position_t pos = PG_POSITION_DEG (2400, 378, 29); + move_start (pos, 0); return ai_next (UNLOAD_UNLOAD, state_timeout); } @@ -350,3 +349,18 @@ ai__COLLECT__move_fsm_failed (void) return ai_next_branch (COLLECT, move_fsm_failed, unload); } +/* + * HACK_MATCH1 =move_fsm_succeed=> + * => COLLECT + * loader down + * choose best food to collect + */ +fsm_branch_t +ai__HACK_MATCH1__move_fsm_succeed (void) +{ + loader_down (); + top_collect (1); + loader_choucroute = 1; + return ai_next (HACK_MATCH1, move_fsm_succeed); +} + diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c index 179cf3b8..36f90046 100644 --- a/digital/io/src/loader.c +++ b/digital/io/src/loader.c @@ -28,6 +28,7 @@ #include "fsm.h" uint8_t loader_elements; +uint8_t loader_choucroute; void loader_up (void) diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 92c0d839..df5723ca 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -51,6 +51,7 @@ States: check clamp is open LOADER_LOAD_EMPTY_OPEN open clamp due to no elements to pick + LOADER_HACK_MATCH1[timeout=225] Events: elevator_succeed @@ -182,10 +183,14 @@ LOADER_LOAD_UPING: post loader_errored event LOADER_LOAD_UNLOADING: - elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN + elevator_succeed: choucroute -> LOADER_HACK_MATCH1 + elevator_succeed: merguez -> LOADER_LOAD_UNLOADING_OPEN elevator_failed -> LOADER_ERROR post loader_errored event +LOADER_HACK_MATCH1: + state_timeout -> LOADER_LOAD_UNLOADING_OPEN + LOADER_LOAD_UNLOADING_OPEN: clamp_succeed -> LOADER_DOWNING move down diff --git a/digital/io/src/loader.h b/digital/io/src/loader.h index b8150891..2669626a 100644 --- a/digital/io/src/loader.h +++ b/digital/io/src/loader.h @@ -28,6 +28,8 @@ /** Number of loaded elements. */ extern uint8_t loader_elements; +extern uint8_t loader_choucroute; + /** Move loader up, in a rest position. */ void loader_up (void); diff --git a/digital/io/src/top.c b/digital/io/src/top.c index 61d0e085..9e550968 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -39,7 +39,8 @@ top_init (void) uint8_t top_collect (uint8_t force) { - if (loader_elements < 3 || force) + static uint8_t max = 3; + if (loader_elements < max || force) { position_t robot_position; asserv_get_position (&robot_position); @@ -49,6 +50,8 @@ top_collect (uint8_t force) vect_t food_v; food_pos (food, &food_v); move_start_noangle (food_v, 0); + max--; + if (max == 0) max = 1; return 1; } else diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 9cb3a044..3657dc54 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -29,6 +29,7 @@ States: unloading, wait COLLECT collecting elements + HACK_MATCH1 Events: start @@ -127,8 +128,12 @@ UNLOAD_BACK_BIN: unload UNLOAD_UNLOAD: - state_timeout -> COLLECT + state_timeout -> HACK_MATCH1 close gate + move to hack position + +HACK_MATCH1: + move_fsm_succeed -> COLLECT loader down choose best food to collect -- cgit v1.2.3