From 29bea338803502ee6ec43d784e987c582c207faa Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Fri, 22 May 2009 11:46:40 +0200 Subject: * digital/io/src: - enhance function to get the next unload area. --- digital/io/src/Makefile | 1 + digital/io/src/init_cb.c | 3 ++ digital/io/src/top.c | 101 ++++++++++++++++++++++++++++++++++++++++++----- digital/io/src/top.h | 6 +++ 4 files changed, 101 insertions(+), 10 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 0f4a18ef..882856ce 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -14,6 +14,7 @@ CONFIGFILE = avrconfig.h AVR_MCU = atmega128 # Optimize for speed. OPTIMIZE = -O2 +HOST_LIBS = -lm # FSMs. FSM_SOURCES := top move filterbridge elevator cylinder init diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c index f79ca49a..5e00d8b8 100644 --- a/digital/io/src/init_cb.c +++ b/digital/io/src/init_cb.c @@ -32,6 +32,7 @@ #include "aquajim.h" #include "switch.h" #include "chrono.h" +#include "top.h" #include "playground.h" #include "modules/trace/trace.h" #include "modules/path/path.h" @@ -71,6 +72,8 @@ init__WAIT_FIRST_JACK_OUT__jack_removed_from_bot (void) trace_init (); /* Get the color. */ bot_color = switch_get_color (); + /* Top initialization. */ + top_init (); return init_next (WAIT_FIRST_JACK_OUT, jack_removed_from_bot); } diff --git a/digital/io/src/top.c b/digital/io/src/top.c index af9d0ff7..4ef0f2dc 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -24,13 +24,64 @@ * }}} */ #include "common.h" #include "top.h" +#include "move.h" #include "playground.h" +#include "modules/math/fixed/fixed.h" +#include /* Reset to 0. */ uint8_t top_total_puck_taken = 0; uint8_t top_puck_inside_bot = 0; +/** + * Number of unload position. + */ +#define TOP_UNLOAD_POSITION_COUNT 8 + +#define TOP_UNLOAD_DISTANCE_TO_CENTER (PG_LENGTH / 2 - 600) + +/** + * Structure of a unload position with costs. + */ +typedef struct top_unload_position_t +{ + move_position_t position; + uint8_t cost; +} top_unload_position_t; + +top_unload_position_t top_unload_position[TOP_UNLOAD_POSITION_COUNT]; + +void +top_init (void) +{ + uint8_t i, cost; + uint32_t angle; + for (i = 0; i < TOP_UNLOAD_POSITION_COUNT; i++) + { + /* Compute predefined positions. */ + angle = 0x1000000 * i / TOP_UNLOAD_POSITION_COUNT; + top_unload_position[i].position.x = PG_WIDTH / 2 + + fixed_mul_f824 (TOP_UNLOAD_DISTANCE_TO_CENTER, + fixed_cos_f824 (angle)); + top_unload_position[i].position.y = PG_LENGTH / 2 + + fixed_mul_f824 (TOP_UNLOAD_DISTANCE_TO_CENTER, + fixed_sin_f824 (angle)); + top_unload_position[i].position.a = angle >> 8; + /* Initialize costs. */ + if (i < TOP_UNLOAD_POSITION_COUNT / 4 + || i > TOP_UNLOAD_POSITION_COUNT * 3 / 4) + cost = bot_color ? 6 : 0; + else if (i > TOP_UNLOAD_POSITION_COUNT / 4 && i < + TOP_UNLOAD_POSITION_COUNT * 3 / 4) + cost = bot_color ? 0 : 6; + else + cost = 0; + top_unload_position[i].cost = cost; + } +} + + uint8_t top_get_next_position_to_get_puck_on_the_ground (asserv_position_t *position, uint8_t restart) @@ -139,19 +190,49 @@ top_get_next_position_to_get_distributor (asserv_position_t *clean_position, void top_get_next_position_to_unload_puck (asserv_position_t *position) { - /* TODO: enahnce. */ - static uint8_t index = 0; - static const asserv_position_t unload[] = + uint8_t i, pos, diff; + uint8_t dynamic_cost[TOP_UNLOAD_POSITION_COUNT]; + /* Compute angle from table center to current position, and find the + * nearest position. */ + asserv_position_t current_position; + asserv_get_position (¤t_position); + pos = (uint8_t) (atan2 (PG_LENGTH / 2 - (int16_t) current_position.y, + PG_WIDTH / 2 - (int16_t) current_position.x) + * (1.0 / (2 * M_PI)) * TOP_UNLOAD_DISTANCE_TO_CENTER + + 1.0 / (TOP_UNLOAD_POSITION_COUNT * 2)) + % TOP_UNLOAD_POSITION_COUNT; + /* Compute dynamic cost. Nearest position costs 0, near half circle costs + * 1, far half circle costs 2. */ + for (i = 0; i < TOP_UNLOAD_POSITION_COUNT; i++) { - { 1500, 600, 270 }, - { 1500, 2100 - 600, 90 }, - }; + /* Compute difference between this position and nearest position. */ + diff = (i - pos + TOP_UNLOAD_POSITION_COUNT) + % TOP_UNLOAD_POSITION_COUNT; + if (diff > TOP_UNLOAD_POSITION_COUNT / 2) + diff = TOP_UNLOAD_POSITION_COUNT - diff; + /* Apply cost. Always add diff to split draws. */ + dynamic_cost[i] = top_unload_position[i].cost + * TOP_UNLOAD_POSITION_COUNT; + if (diff == 0) + dynamic_cost[i] += 0 + diff; + else if (diff <= TOP_UNLOAD_POSITION_COUNT / 4) + dynamic_cost[i] += 1 * TOP_UNLOAD_POSITION_COUNT + diff; + else + dynamic_cost[i] += 2 * TOP_UNLOAD_POSITION_COUNT + diff; + } + /* Now find the cheapest position. */ + uint8_t best_pos = 0; + for (i = 1; i < TOP_UNLOAD_POSITION_COUNT; i++) + { + if (dynamic_cost[i] < dynamic_cost[best_pos]) + best_pos = i; + } /* Sanity check. */ if (position) { - position->x = PG_X_VALUE_COMPUTING (unload[index].x); - position->y = unload[index].y; - position->a = PG_A_VALUE_COMPUTING (unload[index].a * BOT_ANGLE_DEGREE); - index = (index + 1) % 2; + position->x = top_unload_position[best_pos].position.x; + position->y = top_unload_position[best_pos].position.y; + position->a = top_unload_position[best_pos].position.a; } + top_unload_position[best_pos].cost++; } diff --git a/digital/io/src/top.h b/digital/io/src/top.h index 1492405d..986035e8 100644 --- a/digital/io/src/top.h +++ b/digital/io/src/top.h @@ -27,6 +27,12 @@ #include "asserv.h" +/** + * Initialize top module. + */ +void +top_init (void); + /** * Get the next position to get pucks on the ground. * About the @p restart parameter, you need to set to 1 when for example, you -- cgit v1.2.3