summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-13 10:50:40 +0200
committerNicolas Schodet2010-05-13 10:50:40 +0200
commit9b99ef1a8bbf4a109b70b22ebb3842fdb3221651 (patch)
tree2f734b877c42cb461d88f330121c39db59e7428e /digital
parentf3d54d8e62fe76b78bcefd707182408324d25ab5 (diff)
digital/io/src: add food chooser
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_loader_cb.c4
-rw-r--r--digital/io/src/food.c81
-rw-r--r--digital/io/src/food.h13
-rw-r--r--digital/io/src/trace.trc2
4 files changed, 100 insertions, 0 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index 18d60c74..2960cdd9 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -29,6 +29,7 @@
#include "mimot.h"
#include "bot.h"
#include "main.h"
+#include "food.h"
/*
* LOADER_IDLE =start=>
@@ -364,6 +365,9 @@ ai__LOADER_LOAD_CLOSING__clamp_succeed (void)
{
asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
BOT_ELEVATOR_SPEED);
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ food_taken (robot_position);
return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full);
}
else
diff --git a/digital/io/src/food.c b/digital/io/src/food.c
index 62c79f3e..7371d462 100644
--- a/digital/io/src/food.c
+++ b/digital/io/src/food.c
@@ -27,6 +27,9 @@
#include "food.h"
#include "modules/utils/utils.h"
+#include "modules/math/geometry/distance.h"
+#include "modules/trace/trace.h"
+#include "events.h"
/** Maximum distance from a line segment to food center so that the robot is
* able to pick a food. */
@@ -151,3 +154,81 @@ food_blocking_path (vect_t a, vect_t b, int16_t ab)
return 0;
}
+static int32_t
+food_score (position_t robot_pos, uint8_t food)
+{
+ int32_t score = 0;
+ vect_t v;
+ assert (food < UTILS_COUNT (food_table));
+ /* Type of food. */
+ if (food_table[food].type == FOOD_TYPE_TOMATO)
+ score += 100;
+ else
+ score -= 1000;
+ /* Distance to robot. */
+ food_pos (food, &v);
+ int32_t dr = distance_point_point (&v, &robot_pos.v);
+ score += dr * -1;
+ /* Alignment with robot. */
+ if (dr > 100)
+ {
+ vect_t vr = v; vect_sub (&vr, &robot_pos.v);
+ vect_t u; vect_from_polar_uf016 (&u, 100, robot_pos.a);
+ int32_t dp = vect_dot_product (&u, &vr);
+ int32_t align = dp / dr;
+ score += align;
+ }
+ /* Distance to unloading area. */
+ /* Done. */
+ return score;
+}
+
+uint8_t
+food_best (position_t robot_pos)
+{
+ uint8_t i;
+ int32_t score;
+ uint8_t best = 0xff;
+ int32_t best_score = 0;
+ for (i = 0; i < UTILS_COUNT (food_table); i++)
+ {
+ if (!food_table[i].valid)
+ continue;
+ score = food_score (robot_pos, i);
+ if (best == 0xff || best_score < score)
+ {
+ best = i;
+ best_score = score;
+ }
+ }
+ TRACE (TRACE_FOOD__BEST, best);
+ return best;
+}
+
+void
+food_pos (uint8_t food, vect_t *v)
+{
+ assert (food < UTILS_COUNT (food_table));
+ *v = food_table[food].pos;
+}
+
+void
+food_taken (position_t robot_pos)
+{
+ uint8_t i;
+ for (i = 0; i < UTILS_COUNT (food_table); i++)
+ {
+ vect_t v;
+ food_pos (i, &v);
+ if (robot_pos.v.x > v.x - 450 / 2
+ && robot_pos.v.x < v.x + 450 / 2
+ && robot_pos.v.y > v.y - 250 / 2
+ && robot_pos.v.y < v.y + 250 / 2)
+ {
+ food_table[i].valid = 0;
+ TRACE (TRACE_FOOD__TAKEN, i);
+ break;
+ }
+ }
+}
+
diff --git a/digital/io/src/food.h b/digital/io/src/food.h
index a4d6b64d..a0b7b8eb 100644
--- a/digital/io/src/food.h
+++ b/digital/io/src/food.h
@@ -46,4 +46,17 @@ food_blocking (uint8_t food);
uint8_t
food_blocking_path (vect_t a, vect_t b, int16_t ab);
+/** Choose the best next food to pick.
+ * - robot_pos: current robot position. */
+uint8_t
+food_best (position_t robot_pos);
+
+/** Get position of a food. */
+void
+food_pos (uint8_t food, vect_t *v);
+
+/** Mark a food has taken. */
+void
+food_taken (position_t robot_pos);
+
#endif /* food_h */
diff --git a/digital/io/src/trace.trc b/digital/io/src/trace.trc
index 988c3a93..4cdcf8e0 100644
--- a/digital/io/src/trace.trc
+++ b/digital/io/src/trace.trc
@@ -1,3 +1,5 @@
EVENT main_timer__late "MainTimer: we are late."
EVENT fsm__handle_event from 1 event 1 to 1 "FSM: %i =%i=> %i."
EVENT move__go_to xd 2 yd 2 "Move: (%d, %d)."
+EVENT food__best food 1 "Food best: %d."
+EVENT food__taken food 1 "Food taken: %d."