summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJérémy Dufour2009-05-22 11:46:40 +0200
committerJérémy Dufour2009-05-22 11:46:40 +0200
commit29bea338803502ee6ec43d784e987c582c207faa (patch)
treea804a9213fe1b8e0aa08b26ca0c9bac2598eb751
parent66f289007a58331f7101344c7e20c4128a850f0e (diff)
* digital/io/src:
- enhance function to get the next unload area.
-rw-r--r--digital/io/src/Makefile1
-rw-r--r--digital/io/src/init_cb.c3
-rw-r--r--digital/io/src/top.c101
-rw-r--r--digital/io/src/top.h6
4 files changed, 101 insertions, 10 deletions
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 <math.h>
/* 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 (&current_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
@@ -28,6 +28,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
* have unload some pucks and restart the procedure to get new one.