summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-14 08:51:29 +0200
committerNicolas Schodet2010-05-14 08:51:29 +0200
commit844edc0f018c2bdae308f1a005837acb7d2aa84d (patch)
treee6d87faaa1a96d2124d35b224852a1b75f9e28c2 /digital
parent4bb5b4ac79cb486fef1a05fbdc6baedf51a0b152 (diff)
digital/io/src: add slow motion for corn
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_top_cb.c47
-rw-r--r--digital/io/src/bot.h3
-rw-r--r--digital/io/src/food.c12
-rw-r--r--digital/io/src/food.h4
-rw-r--r--digital/io/src/top.c10
-rw-r--r--digital/io/src/top.fsm13
-rw-r--r--digital/io/src/top.h3
7 files changed, 86 insertions, 6 deletions
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.
*/