From 5d55416cac3fa485d61fb01c95ca8067c2ca17c7 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 20 May 2009 16:34:31 +0200 Subject: * digital/io/src: - implement top FSM. --- digital/io/src/top.c | 128 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 128 insertions(+) (limited to 'digital/io/src/top.c') diff --git a/digital/io/src/top.c b/digital/io/src/top.c index 7201f192..40c6280b 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -22,4 +22,132 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" #include "top.h" + +#include "playground.h" + +/* Reset to 0. */ +uint8_t top_total_puck_taken = 0; +uint8_t top_puck_inside_bot = 0; + +uint8_t +top_get_next_position_to_get_puck_on_the_ground (asserv_position_t *position, + uint8_t restart) +{ + /* All positions, in order. */ + static const asserv_position_t ground_puck[] = + { + /* Position: 1 */ + { 600, 1525, 270 }, + /* Position: 4 */ + { 600, 925, 270 }, + /* Position: 6 */ + { 1100, 925, 0 }, + /* Position: 9 */ + { 1100, 1525, 90 }, + /* Position: 10 */ + { 850, 1525, 180 }, + /* Position: 10 */ + { 850, 925, 270 }, + }; + static uint8_t current_position = 0; + + if (current_position >= (sizeof (ground_puck) + / sizeof (asserv_position_t))) + /* No more position. */ + return 0; + /* Sanity check. */ + if (position) + { + /* Fill the position. */ + position->x = PG_X_VALUE_COMPUTING (ground_puck[current_position].x); + position->y = ground_puck[current_position].y; + position->a = PG_A_VALUE_COMPUTING (ground_puck[current_position].a * + BOT_ANGLE_DEGREE); + /* Go to next position only if we are restarting the procedure. */ + if (!restart) + current_position++; + } + return 1; +} + +void +top_get_next_position_to_get_distributor (asserv_position_t *clean_position, + asserv_position_t *front_position) +{ + /* Number of distributor. */ +#define DISTRIBUTOR_COUNT 3 + /* Table of distributor, first index is clean, second is front. */ + static const asserv_position_t distributor[DISTRIBUTOR_COUNT][2] = + { + /* First distributor. */ + { { 2500, 170, 0 }, { 2711, 170, 280 } }, + /* Second distributor. */ + { { 2830, 600, 180 }, { 2830, 800, 0 } }, + /* Third distributor. */ + { { 2830, 1100, 180 }, { 2830, 1300, 0 } }, + }; + /* Maximum number of remaining puck by distributor. */ + static uint8_t distributor_puck_count[DISTRIBUTOR_COUNT] = { 5, 5, 5 }; + /* Current distributor. */ + static uint8_t current_position = DISTRIBUTOR_COUNT; + /* Keep track of how many pucks we have get. This variable will be used to + * know how many puck were taken for each distributor. */ + static uint8_t previous_total_puck_taken = 0; + if (!previous_total_puck_taken) + previous_total_puck_taken = top_total_puck_taken; + + /* Sanity check. */ + if (clean_position && front_position) + { + /* Compute taken pucks since last time. */ + uint8_t puck_count = top_total_puck_taken - previous_total_puck_taken; + /* If some pucks were taken. */ + if (puck_count != 0) + { + /* Remove them. */ + distributor_puck_count[current_position] -= (top_total_puck_taken - + previous_total_puck_taken); + } + /* If no puck taken or distributor empty. */ + if (puck_count == 0 || distributor_puck_count[current_position] == 0) + { + /* Next distributor. */ + current_position++; + current_position %= 3; + } + /* Update taken pucks. */ + previous_total_puck_taken = top_total_puck_taken; + + /* Fill the clean position. */ + clean_position->x = PG_X_VALUE_COMPUTING + (distributor[current_position][0].x); + clean_position->y = distributor[current_position][0].y; + clean_position->a = PG_A_VALUE_COMPUTING + (distributor[current_position][0].a * BOT_ANGLE_DEGREE); + /* Fill the distributor position. */ + front_position->x = PG_X_VALUE_COMPUTING + (distributor[current_position][1].x); + front_position->y = distributor[current_position][1].y; + front_position->a = PG_A_VALUE_COMPUTING + (distributor[current_position][1].a * BOT_ANGLE_DEGREE); + } +} + +void +top_get_next_position_to_unload_puck (asserv_position_t *position) +{ + /* TODO: enahnce. */ + static const asserv_position_t unload[] = + { + { 1500, 600, 90 }, + }; + /* Sanity check. */ + if (position) + { + position->x = PG_X_VALUE_COMPUTING (unload->x); + position->y = unload->y; + position->a = PG_A_VALUE_COMPUTING (unload->a * BOT_ANGLE_DEGREE); + } +} -- cgit v1.2.3