From 9b99ef1a8bbf4a109b70b22ebb3842fdb3221651 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 10:50:40 +0200 Subject: digital/io/src: add food chooser --- digital/io/src/ai_loader_cb.c | 4 +++ digital/io/src/food.c | 81 +++++++++++++++++++++++++++++++++++++++++++ digital/io/src/food.h | 13 +++++++ digital/io/src/trace.trc | 2 ++ 4 files changed, 100 insertions(+) (limited to 'digital/io/src') 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." -- cgit v1.2.3