From 80b3da91dac28a5895a8cb9f0477a7008365dd56 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 1 Apr 2010 23:57:20 +0200 Subject: digital/io: wipe-out old robot code, closes #112 --- digital/io/doc/Makefile | 11 - digital/io/doc/getSamplesFsm.dot | 11 - digital/io/doc/loadBall.dot | 8 - digital/io/doc/mainFsm.dot | 16 - digital/io/doc/move.dot | 19 - digital/io/doc/proto_asserv.txt | 36 +- digital/io/doc/stdContenerFsm.dot | 10 - digital/io/src/Makefile | 2 +- digital/io/src/aquajim.h | 60 -- digital/io/src/asserv.c | 157 ++--- digital/io/src/asserv.h | 92 +-- digital/io/src/bot.h | 78 +++ digital/io/src/chrono.c | 2 +- digital/io/src/contact.c | 5 +- digital/io/src/contact.h | 6 +- digital/io/src/cylinder.c | 47 -- digital/io/src/cylinder.fsm | 170 ------ digital/io/src/cylinder.h | 56 -- digital/io/src/cylinder_cb.c | 441 -------------- digital/io/src/eeprom_dump.txt | 4 - digital/io/src/elevator.c | 119 ---- digital/io/src/elevator.fsm | 125 ---- digital/io/src/elevator.h | 91 --- digital/io/src/elevator_cb.c | 320 ---------- digital/io/src/filterbridge.c | 31 - digital/io/src/filterbridge.fsm | 74 --- digital/io/src/filterbridge.h | 32 - digital/io/src/filterbridge_cb.c | 149 ----- digital/io/src/fsm.h | 3 - digital/io/src/giboulee.h | 98 --- digital/io/src/init_cb.c | 9 +- digital/io/src/main.c | 204 +------ digital/io/src/main.h | 14 - digital/io/src/move_cb.c | 5 +- digital/io/src/playground.h | 49 +- digital/io/src/servo.avr.c | 6 +- digital/io/src/switch.h | 2 +- digital/io/src/test/common.host.c | 154 ----- digital/io/src/test/gutter/Makefile | 17 - digital/io/src/test/gutter/main.c | 49 -- digital/io/src/test/move/Makefile | 13 - digital/io/src/test/move/main.c | 135 ----- digital/io/src/test/testgetsamples/Makefile | 15 - digital/io/src/test/testgetsamples/main.c | 89 --- digital/io/src/top.c | 225 ------- digital/io/src/top.fsm | 211 +------ digital/io/src/top.h | 40 -- digital/io/src/top_cb.c | 883 +--------------------------- digital/io/src/trace.trc | 1 - digital/io/tools/test_simu.py | 124 ++++ digital/io/tools/test_simu_aquajim.py | 126 ---- digital/io/tools/test_simu_control.py | 38 +- digital/io/tools/test_simu_giboulee.py | 124 ---- 53 files changed, 337 insertions(+), 4469 deletions(-) delete mode 100644 digital/io/doc/Makefile delete mode 100644 digital/io/doc/getSamplesFsm.dot delete mode 100644 digital/io/doc/loadBall.dot delete mode 100644 digital/io/doc/mainFsm.dot delete mode 100644 digital/io/doc/move.dot delete mode 100644 digital/io/doc/stdContenerFsm.dot delete mode 100644 digital/io/src/aquajim.h create mode 100644 digital/io/src/bot.h delete mode 100644 digital/io/src/cylinder.c delete mode 100644 digital/io/src/cylinder.fsm delete mode 100644 digital/io/src/cylinder.h delete mode 100644 digital/io/src/cylinder_cb.c delete mode 100644 digital/io/src/eeprom_dump.txt delete mode 100644 digital/io/src/elevator.c delete mode 100644 digital/io/src/elevator.fsm delete mode 100644 digital/io/src/elevator.h delete mode 100644 digital/io/src/elevator_cb.c delete mode 100644 digital/io/src/filterbridge.c delete mode 100644 digital/io/src/filterbridge.fsm delete mode 100644 digital/io/src/filterbridge.h delete mode 100644 digital/io/src/filterbridge_cb.c delete mode 100644 digital/io/src/giboulee.h delete mode 100644 digital/io/src/test/common.host.c delete mode 100644 digital/io/src/test/gutter/Makefile delete mode 100644 digital/io/src/test/gutter/main.c delete mode 100644 digital/io/src/test/move/Makefile delete mode 100644 digital/io/src/test/move/main.c delete mode 100644 digital/io/src/test/testgetsamples/Makefile delete mode 100644 digital/io/src/test/testgetsamples/main.c create mode 100644 digital/io/tools/test_simu.py delete mode 100644 digital/io/tools/test_simu_aquajim.py delete mode 100644 digital/io/tools/test_simu_giboulee.py (limited to 'digital') diff --git a/digital/io/doc/Makefile b/digital/io/doc/Makefile deleted file mode 100644 index 3f03f686..00000000 --- a/digital/io/doc/Makefile +++ /dev/null @@ -1,11 +0,0 @@ -DOT = mainFsm.dot loadBall.dot getSamplesFsm.dot stdContenerFsm.dot move.dot - -PNG = $(DOT:%.dot=%.png) - -all: $(PNG) - -%.png: %.dot - dot -Tpng -o $@ $< - -clean: - rm -f $(PNG) diff --git a/digital/io/doc/getSamplesFsm.dot b/digital/io/doc/getSamplesFsm.dot deleted file mode 100644 index e53badbd..00000000 --- a/digital/io/doc/getSamplesFsm.dot +++ /dev/null @@ -1,11 +0,0 @@ -/* IO.avr module. Get Samples Final State Machine */ -digraph GetSamplesFsm -{ - node [color=lightblue2, style=filled]; - "Go to position" -> "Prepare Arm for ditributor" [label = "Position reached"]; - "Prepare Arm for ditributor" -> "Forward controled" [label = "Harm prepared"]; - "Forward controled" -> "Prepare classifier" [label = "Ready to peek samples"]; - "Prepare classifier" -> "loadballs" [label = "Classifier prepared"]; - "loadballs" -> "Continue classifier" [label = "balls loaded"]; - "Continue classifier" -> End [label = "Classification ended"]; -} diff --git a/digital/io/doc/loadBall.dot b/digital/io/doc/loadBall.dot deleted file mode 100644 index 3ce6ae93..00000000 --- a/digital/io/doc/loadBall.dot +++ /dev/null @@ -1,8 +0,0 @@ -/* IO.avr module. Charge balls Final State Machine */ -digraph loadBallFsm -{ - node [color=lightblue2, style=filled]; - "Rotate arm" -> "Go backward" [label = "desired balls loaded"]; - "Go backward" -> "Stop arm" [label = "Poisition reached"]; - "Stop arm" -> End [label = "arm stopped\narm at init position"]; -} diff --git a/digital/io/doc/mainFsm.dot b/digital/io/doc/mainFsm.dot deleted file mode 100644 index 796947a2..00000000 --- a/digital/io/doc/mainFsm.dot +++ /dev/null @@ -1,16 +0,0 @@ -/* IO.avr module. Main Final State Machine */ -digraph mainFsm -{ - node [color=lightblue2, style=filled]; - start -> "get samples" [label = "1"]; - "get samples" -> "get ice" [label = "Samples took"]; - "get ice" -> "Go to goal" [label = "Ice took"]; - "Go to goal" -> "get samples" [label = "Samples deposed"]; - - "get samples" -> "Go to goal" [label = "Samples took\n and ice distributor empty\n and not enough time"]; - "Go to goal" -> "get adv ice" [label = "Sample deposed\n and ice distributor empty"]; - "get adv ice" -> "get samples" [label = "Ice took"]; - "get samples" -> "get adv ice" [label = "Sample took and\n ice distributor empty\n and enough time"]; - "get adv ice" -> "Go to goal" [label = "Ice took\nCollector full"]; - "get samples" -> "Go to goal" [label = "Collector full"]; -} diff --git a/digital/io/doc/move.dot b/digital/io/doc/move.dot deleted file mode 100644 index 21f03398..00000000 --- a/digital/io/doc/move.dot +++ /dev/null @@ -1,19 +0,0 @@ -# Try to reach a position, this digram shall provide an start of solution when -# the robot is blocked or a when the robot has a obstacle in front of it. - -digraph move -{ - start -> "desired position" [label = "ok"]; - "desired position" -> end [label = "reached"]; - "desired position" -> "move on left" [label = "failed or blocked\n border left is far"]; - "desired position" -> "move on right" [label = "failed or blocked\n border right is far"]; - - "move on left" -> "move on left" [label = "failed or blocked"]; - "move on left" -> "desired position" [label = "reached"]; - - "move on right" -> "move on right" [label = "failed or blocked"]; - "move on right" -> "desired position" [label = "reached"]; - - "move on right" -> "move on left" [label = "near right border"]; - "move on left" -> "move on right" [label = "near left border"]; -} diff --git a/digital/io/doc/proto_asserv.txt b/digital/io/doc/proto_asserv.txt index 9fcf9808..259b0905 100644 --- a/digital/io/doc/proto_asserv.txt +++ b/digital/io/doc/proto_asserv.txt @@ -95,22 +95,20 @@ This table describe the list of supported commands by the *asserv* card: | | | - y (3b) | absolute angle, backward movement | | | | - a (2b) | allowed | +---------+-------+-----------------+------------------------------------------+ -| 'b' | Arm | - position (2b) | Move the arm to a desired position | +| 'b' | Mot0 | - position (2b) | Move the motor0 to a desired position | | | | - speed (1b) | at a specific speed | -| | | | - position: 250 * 4 * 80 / 15 steps by | -| | | | round | +| | | | - position: x steps by x | | | | | - speed: in step by cycle (4.44ms) | +---------+-------+-----------------+------------------------------------------+ -| 'B' | Arm | - speed (1b) | Find the zero position of the arm | +| 'B' | Mot0 | - speed (1b) | Find the zero position of the motor0 | | | | | - speed: in step by cycle (4.44ms) | -| | | | Offset for mechanical zero is 0x43e | +---------+-------+-----------------+------------------------------------------+ -| 'c' | Elvt | - position (2b) | Move the elevator to a desired position | +| 'c' | Mot1 | - position (2b) | Move the motor1 to a desired position | | | | - speed (1b) | at a specific speed | -| | | | - position: 72.34 steps by mm | +| | | | - position: x steps by x | | | | | - speed: in step by cycle (4.44ms) | +---------+-------+-----------------+------------------------------------------+ -| 'C' | Elvt | - speed (1b) | Find the zero position of the elevator | +| 'C' | Mot1 | - speed (1b) | Find the zero position of the motor1 | | | | | - speed: in step by cycle (4.44ms) | +---------+-------+-----------------+------------------------------------------+ | 'p' | Other | See next table | Change setting values | @@ -148,21 +146,21 @@ card. It is used to know if the last command sent by the *io* card is currently executed, is finished or has failed (i.e. we are blocked) and also to retreive -the current position (X, Y, alpha) of the bot, its arm and elevator. +the current position (X, Y, alpha) of the bot and its motors. The buffer is a static 12 bytes length one:: - 0 1 2 3 .. 5 6 .. 8 9 10 11 12 13 14 <-- byte position - +--------+------+-----+- - - -+- - - -+-------+---------+----------+ - | status | PINC | seq | X pos | Y pos | angle | arm pos | elevator | - +--------+------+-----+- - - -+- - - -+-------+---------+----------+ + 0 1 2 3 .. 5 6 .. 8 9 10 11 12 13 14 <-- byte position + +--------+------+-----+- - - -+- - - -+-------+----------+----------+ + | status | PINC | seq | X pos | Y pos | angle | mot0 pos | mot1 pos | + +--------+------+-----+- - - -+- - - -+-------+----------+----------+ Status value ------------ -Status contains flag bit to know the status of movement and arm commands: if -they have finished with success or with an error (i.e. blocked). +Status contains flag bit to know the status of movement and motors commands: +if they have finished with success or with an error (i.e. blocked). Here is a list of the bits used and their definitions: @@ -176,13 +174,13 @@ Here is a list of the bits used and their definitions: 3. Movement is going backward: the current linear speed is smaller than 0 (BCK); - 4. Arm movement finished with success (AF); + 4. Motor0 movement finished with success (A0F); - 5. Arm movement finished with failure: it should not happen (AB); + 5. Motor0 movement finished with failure: it should not happen (A0B); - 6. Elevator movement finished with success (EF); + 6. Motor1 movement finished with success (A1F); - 7. Elevator movement finished with failure: it should not happen (EB). + 7. Motor1 movement finished with failure: it should not happen (A1B). The others bits are unused. diff --git a/digital/io/doc/stdContenerFsm.dot b/digital/io/doc/stdContenerFsm.dot deleted file mode 100644 index 2ec2309f..00000000 --- a/digital/io/doc/stdContenerFsm.dot +++ /dev/null @@ -1,10 +0,0 @@ -/* IO.avr module. Chute Final State Machine */ -digraph StdContenerFsm -{ - node [color=lightblue2, style=filled]; - "Go to std contener" -> "Go to the wall backward" [label = "Near the std contener."]; - "Go to the wall backward" -> "Open std contener" [label = "On the wall"]; - "Open std contener" -> "Wait fixed time" [label = "std contener opened"]; - "Wait fixed time" -> "Close std contener" [label = "Time out"]; - "Close std contener" -> End [label = "std contener closed"]; -} diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 882856ce..0ec3d6ce 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -17,7 +17,7 @@ OPTIMIZE = -O2 HOST_LIBS = -lm # FSMs. -FSM_SOURCES := top move filterbridge elevator cylinder init +FSM_SOURCES := top move init # Include FSM makefile. include $(BASE)/make/Makefile.fsm diff --git a/digital/io/src/aquajim.h b/digital/io/src/aquajim.h deleted file mode 100644 index 4468ac73..00000000 --- a/digital/io/src/aquajim.h +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef aquajim_h -#define aquajim_h -/* aquajim.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 Dufour Jérémy - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ - -/** - * Some defines for the eurobot 2009 bot of APBTeam: Aqua Jim. - */ - -/** - * Duration of a match in milliseconds. - */ -#define MATCH_DURATION_MS 90000 - -/** - * filterbridge servo - */ - -#define SERVO_FINGER_ID 0 -#define SERVO_DOOR_ID 1 -#define SERVO_FINGER_IDLE 0 -#define SERVO_FINGER_PUSHING 1 -#define SERVO_DOOR_OPEN 0 -#define SERVO_DOOR_CLOSE 1 - -/** - * How to compute a angle for giboulee? - * One degree is 65536 / 360 - */ -#define BOT_ANGLE_DEGREE (65536 / 360) - -/** - * The size of the bot. - */ -#define BOT_LENGTH 300 -#define BOT_WIDTH 310 - -#endif /* aquajim_h */ diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 0853a11b..0ae1465f 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -31,7 +31,7 @@ #include "modules/math/fixed/fixed.h" #include "modules/utils/crc.h" #include "modules/trace/trace.h" -#include "giboulee.h" /* BOT_* */ +#include "bot.h" #include "io.h" #include "trace_event.h" @@ -62,14 +62,14 @@ enum asserv_status_flag_e asserv_status_flag_move_forward = 2, /** Bot is moving backward (linear speed smaller than 0). */ asserv_status_flag_move_backward = 3, - /** Arm movement finished with success. */ - asserv_status_flag_arm_succeed = 4, - /** Arm movement finished with failure (can not happen). */ - asserv_status_flag_arm_failed = 5, - /** Elevator movement finished with success. */ - asserv_status_flag_elevator_succeed = 6, - /** Elevator movement finished with failure (can not happen). */ - asserv_status_flag_elevator_failed = 7, + /** Motor0 movement finished with success. */ + asserv_status_flag_motor0_succeed = 4, + /** Motor0 movement finished with failure (can not happen). */ + asserv_status_flag_motor0_failed = 5, + /** Motor1 movement finished with success. */ + asserv_status_flag_motor1_succeed = 6, + /** Motor1 movement finished with failure (can not happen). */ + asserv_status_flag_motor1_failed = 7, }; typedef enum asserv_status_flag_e asserv_status_flag_e; @@ -106,10 +106,10 @@ typedef struct asserv_struct_s uint8_t seq; /** Bot position. */ asserv_position_t position; - /** Arm position. */ - uint16_t arm_position; - /** Elevator position. */ - uint16_t elevator_position; + /** Motor0 position. */ + uint16_t motor0_position; + /** Motor1 position. */ + uint16_t motor1_position; } asserv_struct_s; /** @@ -132,11 +132,6 @@ static uint8_t asserv_retransmit_counter; */ static uint8_t asserv_retransmit_length; -/** - * The arm position of notification for the get sample FSM. - */ -static uint16_t asserv_arm_notify_position; - /** * Update TWI module until request (send or receive) is finished. * This functions is blocking. @@ -173,20 +168,19 @@ static inline uint8_t asserv_twi_send (uint8_t length); /** - * Move the arm. - * A complete rotation correspond to 5333 steps. + * Move the motor0. * @param position desired goal position (in step). * @param speed speed of the movement. */ void -asserv_move_arm_absolute (uint16_t position, uint8_t speed); +asserv_move_motor0_absolute (uint16_t position, uint8_t speed); /** - * Current position of the arm. + * Current position of the motor0. * We need to maintain it by ourself as it is more accurate than the one sent * by the asserv board. */ -static uint16_t asserv_arm_current_position; +static uint16_t asserv_motor0_current_position; /* Update TWI module until request (send or receive) is finished. */ static inline void @@ -279,8 +273,8 @@ asserv_update_status (void) asserv_status.position.y = ((int32_t) v8_to_v32 (status_buffer[6], status_buffer[7], status_buffer[8], 0)) >> 8; asserv_status.position.a = v8_to_v16 (status_buffer[9], status_buffer[10]); - asserv_status.arm_position = v8_to_v16 (status_buffer[11], status_buffer[12]); - asserv_status.elevator_position = v8_to_v16 (status_buffer[13], status_buffer[14]); + asserv_status.motor0_position = v8_to_v16 (status_buffer[11], status_buffer[12]); + asserv_status.motor1_position = v8_to_v16 (status_buffer[13], status_buffer[14]); /* Update moving direction. */ if (asserv_get_moving_direction () != 0) asserv_last_moving_direction = asserv_get_moving_direction (); @@ -335,41 +329,29 @@ asserv_move_cmd_status (void) return none; } -/* Is last arm class command has successfully ended? */ +/* Is last motor0 class command has successfully ended? */ asserv_status_e -asserv_arm_cmd_status (void) +asserv_motor0_cmd_status (void) { - /* Check Arm Finished flag */ - if (asserv_status.status & _BV (asserv_status_flag_arm_succeed)) + /* Check Motor0 Finished flag */ + if (asserv_status.status & _BV (asserv_status_flag_motor0_succeed)) return success; - /* Check Arm Blocked flag */ - else if (asserv_status.status & _BV (asserv_status_flag_arm_failed)) + /* Check Motor0 Blocked flag */ + else if (asserv_status.status & _BV (asserv_status_flag_motor0_failed)) return failure; /* Otherwise, not finished nor failure */ return none; } -/* Is the optical fence see a puck? */ -uint8_t -asserv_arm_of_status(void) -{ -#ifdef HOST - return 1; -#endif - if(asserv_status.input_port & _BV (5)) - return 1; - return 0; -} - -/* Is last elevator class command has successfully ended? */ +/* Is last motor1 class command has successfully ended? */ asserv_status_e -asserv_elevator_cmd_status (void) +asserv_motor1_cmd_status (void) { - /* Check Elevator Finished flag */ - if (asserv_status.status & _BV (asserv_status_flag_elevator_succeed)) + /* Check Motor1 Finished flag */ + if (asserv_status.status & _BV (asserv_status_flag_motor1_succeed)) return success; - /* Check Elevator Blocked flag */ - else if (asserv_status.status & _BV (asserv_status_flag_elevator_failed)) + /* Check Motor1 Blocked flag */ + else if (asserv_status.status & _BV (asserv_status_flag_motor1_failed)) return failure; /* Otherwise, not finished nor failure */ return none; @@ -390,20 +372,20 @@ asserv_get_position (asserv_position_t *current_position) } } -/* Get the arm position. */ +/* Get the motor0 position. */ uint16_t -asserv_get_arm_position (void) +asserv_get_motor0_position (void) { - /* Return the position of the arm of the current status buffer */ - return asserv_status.arm_position; + /* Return the position of the motor0 of the current status buffer */ + return asserv_status.motor0_position; } -/* Get the elevator position. */ +/* Get the motor1 position. */ uint16_t -asserv_get_elevator_position (void) +asserv_get_motor1_position (void) { - /* Return the position of the elevator of the current status buffer */ - return asserv_status.elevator_position; + /* Return the position of the motor1 of the current status buffer */ + return asserv_status.motor1_position; } /* Are we moving forward/backward? */ @@ -457,7 +439,7 @@ asserv_move_linearly (int32_t distance) asserv_twi_send_command ('l', 3); } - /* Move angularly (turn). */ +/* Move angularly (turn). */ void asserv_move_angularly (int16_t angle) { @@ -513,48 +495,37 @@ asserv_go_to_the_wall (uint8_t backward) asserv_twi_send_command ('f', 1); } -/* Move forward to approach a ditributor. */ +/* Move the motor0. */ void -asserv_go_to_distributor (void) -{ - /* Put direction and delay as parameters */ - asserv_twi_buffer_param[0] = 0; - asserv_twi_buffer_param[1] = 25; - /* Send the go the wall command to the asserv board */ - asserv_twi_send_command ('g', 2); -} - -/* Move the arm. */ -void -asserv_move_arm_absolute (uint16_t position, uint8_t speed) +asserv_move_motor0_absolute (uint16_t position, uint8_t speed) { /* Put position and speed as parameters */ asserv_twi_buffer_param[0] = v16_to_v8 (position, 1); asserv_twi_buffer_param[1] = v16_to_v8 (position, 0); asserv_twi_buffer_param[2] = speed; - /* Send the move the arm command to the asserv board */ + /* Send the move the motor0 command to the asserv board */ asserv_twi_send_command ('b', 3); } -/* Move the arm to a certain number of steps. */ +/* Move the motor0 to a certain number of steps. */ void -asserv_move_arm (int16_t offset, uint8_t speed) +asserv_move_motor0 (int16_t offset, uint8_t speed) { /* Compute the new desired position with the desired offset */ - asserv_arm_current_position += offset; - /* Move the arm to the desired position */ - asserv_move_arm_absolute (asserv_arm_current_position, speed); + asserv_motor0_current_position += offset; + /* Move the motor0 to the desired position */ + asserv_move_motor0_absolute (asserv_motor0_current_position, speed); } -/* Move the elevator. */ +/* Move the motor1. */ void -asserv_move_elevator_absolute (uint16_t position, uint8_t speed) +asserv_move_motor1_absolute (uint16_t position, uint8_t speed) { /* Put position and speed as parameters */ asserv_twi_buffer_param[0] = v16_to_v8 (position, 1); asserv_twi_buffer_param[1] = v16_to_v8 (position, 0); asserv_twi_buffer_param[2] = speed; - /* Send the move the elevator command to the asserv board */ + /* Send the move the motor1 command to the asserv board */ asserv_twi_send_command ('c', 3); } @@ -689,28 +660,6 @@ asserv_goto_back (uint32_t x, uint32_t y) asserv_twi_send_command ('x', 7); } -/* Notify get samples FSM when the arm reach desired position. */ -void -asserv_arm_set_position_reached (uint16_t position) -{ - /* Store the position */ - asserv_arm_notify_position = position; -} - -/* Check if notification of the get sample FSM is required in term of position - * of the arm. - */ -uint8_t -asserv_arm_position_reached (void) -{ - /* If the position has been reached */ - if (asserv_arm_notify_position && - ((int16_t) (asserv_get_arm_position () - - asserv_arm_notify_position) >= 0)) - return 1; - return 0; -} - /* Set scale. */ void asserv_set_scale (uint32_t scale) @@ -720,14 +669,14 @@ asserv_set_scale (uint32_t scale) } void -asserv_arm_zero_position (void) +asserv_motor0_zero_position (void) { asserv_twi_buffer_param[0] = 0x05; asserv_twi_send_command ('B', 1); } void -asserv_elevator_zero_position (void) +asserv_motor1_zero_position (void) { asserv_twi_buffer_param[0] = -0x10; asserv_twi_send_command ('C', 1); diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 253ef0a5..7e94d19b 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -38,10 +38,10 @@ * ignored. */ -#define ASSERV_ARM_SPEED_DEFAULT 0x0C -#define ASSERV_ELVT_SPEED_DEFAULT 0x46 +#define ASSERV_MOTOR0_SPEED_DEFAULT 0x0C +#define ASSERV_MOTOR1_SPEED_DEFAULT 0x46 -#define ASSERV_ARM_STEP_BY_DEGREE 14.814814 +#define ASSERV_MOTOR0_STEP_BY_DEGREE 14.814814 /** Use backward movements. */ #define ASSERV_BACKWARD 1 @@ -92,7 +92,7 @@ uint8_t asserv_retransmit (void); /** - * Status of a move or arm class command. + * Status of a move or motor0 class command. * It is return by status functions. */ typedef enum asserv_status_e @@ -101,7 +101,7 @@ typedef enum asserv_status_e none, /** The command has succeed. */ success, - /** The command has failed. The bot or the arm is blocked */ + /** The command has failed. The bot or the motor0 is blocked */ failure } asserv_status_e; @@ -115,31 +115,22 @@ asserv_status_e asserv_move_cmd_status (void); /** - * Is last arm class command has successfully ended? - * This function is used to know the status of the last arm command. It looks - * at the status register. + * Is last motor0 class command has successfully ended? + * This function is used to know the status of the last motor0 command. It + * looks at the status register. * @return the status of the last move class command. */ asserv_status_e -asserv_arm_cmd_status (void); +asserv_motor0_cmd_status (void); /** - * - * Is the optical fence see a puck? - * This function get the arm optical fence status - * @return the status of the arm optical fence - */ -uint8_t -asserv_arm_of_status(void); - -/** - * Is last elevator class command has successfully ended? - * This function is used to know the status of the last elevator command. It + * Is last motor1 class command has successfully ended? + * This function is used to know the status of the last motor1 command. It * looks at the status register. * @return the status of the last move class command. */ asserv_status_e -asserv_elevator_cmd_status (void); +asserv_motor1_cmd_status (void); /** * Structure for storing a position for the bot. @@ -162,18 +153,18 @@ void asserv_get_position (asserv_position_t *current_position); /** - * Get the arm position. - * @return the position of the arm (in steps). + * Get the motor0 position. + * @return the position of the motor0 (in steps). */ uint16_t -asserv_get_arm_position (void); +asserv_get_motor0_position (void); /** - * Get the elevator position. - * @return the position of the elevator (in steps). + * Get the motor1 position. + * @return the position of the motor1 (in steps). */ uint16_t -asserv_get_elevator_position (void); +asserv_get_motor1_position (void); /** * Are we moving forward/backward? @@ -249,15 +240,8 @@ void asserv_go_to_the_wall (uint8_t backward); /** - * Move forward to approach a distributor. - * Move class command. - */ -void -asserv_go_to_distributor (void); - -/** - * Move the arm to a certain number of steps. - * Arm class command. + * Move the motor0 to a certain number of steps. + * Motor0 class command. * This function take the number of steps you want to move to. This is not an * absolute position, this is only the number of steps you want to add to the * current position. @@ -265,18 +249,18 @@ asserv_go_to_distributor (void); * @param speed speed of the movement. */ void -asserv_move_arm (int16_t offset, uint8_t speed); +asserv_move_motor0 (int16_t offset, uint8_t speed); /** - * Move the elevator. - * Elevator class command. + * Move the motor1. + * Motor1 class command. * This function take the number of steps you want to move to. This is an * absolute position. * @param position desired goal position (in step). * @param speed speed of the movement. */ void -asserv_move_elevator_absolute (uint16_t position, uint8_t speed); +asserv_move_motor1_absolute (uint16_t position, uint8_t speed); /** * Set current X position. @@ -344,26 +328,6 @@ asserv_goto (uint32_t x, uint32_t y, uint8_t backward); void asserv_goto_back (uint32_t x, uint32_t y); -/** - * Set the notifier of get samples FSM when the arm reach desired position. - * You should called this function from the get sample FSM to tell the asserv - * module you want to be notified when the arm reached the desired position. - * @param position the desired position of the arm (absolute). Note, 0 is a - * reset value do disable the notifier. If you want to use it, please add 1. - */ -void -asserv_arm_set_position_reached (uint16_t position); - -/** - * Check if notification of the get sample FSM is required in term of - * position of the arm. - * @return - * - 0 if the notification is not needed ; - * - 1 if the notification of the get sample FSM is required. - */ -uint8_t -asserv_arm_position_reached (void); - /** Set scale. * @param scale vous avez qu'à deviner (f8.24). */ @@ -371,16 +335,16 @@ void asserv_set_scale (uint32_t scale); /** - * Reset the ARM to the zero position. + * Reset the motor0 to the zero position. */ void -asserv_arm_zero_position (void); +asserv_motor0_zero_position (void); /** - * Reset the elevator to the zero position. + * Reset the motor1 to the zero position. */ void -asserv_elevator_zero_position (void); +asserv_motor1_zero_position (void); /** * Get the last moving direction of the bot. diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h new file mode 100644 index 00000000..aff6ada0 --- /dev/null +++ b/digital/io/src/bot.h @@ -0,0 +1,78 @@ +#ifndef bot_h +#define bot_h +/* bot.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** + * Specific defines about the robot dimensions and competion rules. + */ + +/** + * Duration of a match in milliseconds. + */ +#define MATCH_DURATION_MS 90000 + +/** + * How to compute a angle for the robot? + * One degree is 65536 / 360 + */ +#define BOT_ANGLE_DEGREE (65536 / 360) + +/** + * The scaling factor, millimeter per step. + */ +#ifdef HOST +# define BOT_SCALE 0.0395840674352314 +#else +# define BOT_SCALE 0.0413530725332892 +#endif + +/** + * The size of the bot. + */ +#define BOT_LENGTH 300 +#define BOT_WIDTH 310 + +/** + * Distance required to be away from a border to be able to turn freely. + * In millimeter. + */ +#define BOT_MIN_DISTANCE_TURN_FREE 300 + +/** + * Definition of the colors. + */ +enum team_color_e +{ + RED_TEAM = 0, + BLUE_TEAM = 1 +}; + +/** + * Our color. + */ +extern enum team_color_e bot_color; + +#endif /* bot_h */ diff --git a/digital/io/src/chrono.c b/digital/io/src/chrono.c index 579ed4bf..631c7096 100644 --- a/digital/io/src/chrono.c +++ b/digital/io/src/chrono.c @@ -25,7 +25,7 @@ #include "common.h" -#include "aquajim.h" +#include "bot.h" #include "main_timer.h" #include "asserv.h" diff --git a/digital/io/src/contact.c b/digital/io/src/contact.c index 6a47c254..4483fef3 100644 --- a/digital/io/src/contact.c +++ b/digital/io/src/contact.c @@ -29,7 +29,6 @@ void contact_init (void) { - IO_SET (CONTACT_FILTER_BRIDGE_PUCK); - IO_SET (CONTACT_ELEVATOR_DOOR); - IO_SET (CONTACT_PUCK_CYLINDER); + /* Pull-up. */ + //IO_SET (CONTACT_EXAMPLE); } diff --git a/digital/io/src/contact.h b/digital/io/src/contact.h index c752fc56..e50a0833 100644 --- a/digital/io/src/contact.h +++ b/digital/io/src/contact.h @@ -26,9 +26,7 @@ * }}} */ #include "simu.host.h" -#define CONTACT_FILTER_BRIDGE_PUCK C, 0 -#define CONTACT_ELEVATOR_DOOR C, 1 -#define CONTACT_PUCK_CYLINDER C, 2 +#define CONTACT_EXAMPLE C, 0 /** * Initialize the contact module. @@ -38,7 +36,7 @@ contact_init (void); /** * To get a state of a contact, just do: - * IO_GET (CONTACT_FILTER_BRIDGE_PUCK); + * IO_GET (CONTACT_EXAMPLE); */ #endif /* contact_h */ diff --git a/digital/io/src/cylinder.c b/digital/io/src/cylinder.c deleted file mode 100644 index cceb4013..00000000 --- a/digital/io/src/cylinder.c +++ /dev/null @@ -1,47 +0,0 @@ -/* cylinder.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "cylinder.h" - -/* last state of optical fence */ -uint8_t cylinder_of_state = 0; - -/* nb puck in cylinder */ -uint8_t cylinder_nb_puck = 0; - -uint8_t cylinder_puck_on_of = 0; - -/* force cylinder to be closed */ -uint8_t cylinder_close_order = 0; - -/* flush puck on the cylinder */ -uint8_t cylinder_flush_order = 0; - -/* distributor fucked */ -uint8_t cylinder_distributor_fucked = 0; - -/* distributor empty */ -uint8_t cylinder_distributor_empty = 0; - -/* cylinder order */ diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm deleted file mode 100644 index bb63d96c..00000000 --- a/digital/io/src/cylinder.fsm +++ /dev/null @@ -1,170 +0,0 @@ -# Cylinder FSM -# Control the puck's entrance -cylinder - -States: - IDLE - waiting for init - WAIT_FOR_JACK_IN - waiting for jack - RESET_POS - reset the cylinder position - INIT_POS - initializing cylinder to a good position (open) - WAIT_A_PUCK - waiting a puck enter in cylinder - WAIT_BRIDGE_READY - waiting fb is ready to accept new puck - TURN_PLUS_1_AND_OFO - turn 1+of_offset position after when we see a new puck - TURN_PLUS_1_AND_MINUS_OFO - turn 1-of_offset position after to be ready to get a new puck - WAIT_BOT_NOT_FULL - wait the bot to drop pucks - WAIT_CLEAR_ORDER - wait the clear order (unset close_order flags) - TURN_PLUS_1_OPEN - open cylinder after close (bot full/close_order) - TURN_PLUS_1_FLUSH - open cylinder to prepare cyliner flush - WAIT_BRIDGE_READY_FLUSH - we wait fb is ready during the flush - TURN_PLUS_3_FLUSH - drop 1 puck to the fb - TURN_PLUS_1_CLOSE - close the cylinder - WAIT_BRIDGE_READY_DISTRIB - waiting for the bridge - TURN_PLUS_1_AND_OFO_DISTRIB - get a puck on distrib - TURN_PLUS_1_AND_MINUS_OFO_DISTRIB - we neutralize the offset before return in normal mode - TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP - blah - PROBE_OF - we probe the OF - - -Events: - start - start event - jack_inserted_into_bot - the init signal - new_puck - there is a puck in position 1 - move_done - cylinder move finished - bridge_ready - the fb is ready to get new pucks - close_order - we receive the order to close the cylinder - no_close_order - we can reopen the cylinder - bot_not_full - the bot dont have 4 pucks - flush_order - we receive a flush order - distrib_fucked - we are really in the distributor - of_puck - we see a puck to the of - of_no_puck - we don't see a puck in of - state_timeout - useless here - -IDLE: - start -> WAIT_FOR_JACK_IN - we wait the jack before moving anything - -WAIT_FOR_JACK_IN: - jack_inserted_into_bot -> RESET_POS - we init the cylinder position - -RESET_POS: - move_done -> INIT_POS - move the cylinder to open it - -INIT_POS: - move_done -> WAIT_A_PUCK - the cylinder is ready to get pucks - -WAIT_A_PUCK: - new_puck -> WAIT_BRIDGE_READY - look if the bridge is ready before move - close_order -> TURN_PLUS_1_CLOSE - we close cylinder as requested - flush_order -> WAIT_BRIDGE_READY_FLUSH - flush all pucks to the bridge - distrib_fucked -> WAIT_BRIDGE_READY_DISTRIB - we verify bridge - -WAIT_BRIDGE_READY_DISTRIB: - bridge_ready -> TURN_PLUS_1_AND_OFO_DISTRIB - we turn to check the of - -TURN_PLUS_1_AND_OFO_DISTRIB: - move_done -> PROBE_OF - We probe the puck - -PROBE_OF: - of_puck: bot_full -> WAIT_BOT_NOT_FULL - return to normal mode with a bot full of puck - of_puck: bot_not_full -> TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP - we get a puck and we are hungry yet, go eat an another puck - of_no_puck -> TURN_PLUS_1_AND_MINUS_OFO_DISTRIB - Distributor empty, go elsewhere - -TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP: - move_done -> WAIT_BRIDGE_READY_DISTRIB - ready to get another puck - -WAIT_BRIDGE_READY: - bridge_ready -> TURN_PLUS_1_AND_OFO - open the cylinder with the puck or not. - -TURN_PLUS_1_AND_OFO: - move_done: bot_not_full -> TURN_PLUS_1_AND_MINUS_OFO - open the cylinder to wait a new puck - move_done: bot_full -> WAIT_BOT_NOT_FULL - bot full, waiting for pucks teleportation - -TURN_PLUS_1_AND_MINUS_OFO_DISTRIB: - move_done -> WAIT_A_PUCK - bridge empty, return to normal mode - -TURN_PLUS_1_AND_MINUS_OFO: - move_done -> WAIT_A_PUCK - ready for other pucks - -WAIT_BOT_NOT_FULL: - bot_not_full -> WAIT_CLEAR_ORDER - the bot is not full, we go testing the other close condition - flush_order -> TURN_PLUS_1_FLUSH - flush order received, go open the cylinder - -WAIT_CLEAR_ORDER: - no_close_order -> TURN_PLUS_1_OPEN - no close order, we reopen cylinder to get other pucks - flush_order -> TURN_PLUS_1_FLUSH - flush order received, go open the cylinder - -TURN_PLUS_1_OPEN: - move_done -> WAIT_A_PUCK - cylinder ready to get other pucks - -TURN_PLUS_1_FLUSH: - move_done -> WAIT_BRIDGE_READY_FLUSH - we wait the bridge before moving - -WAIT_BRIDGE_READY_FLUSH: - bridge_ready -> TURN_PLUS_3_FLUSH - bridge is ready, flush gordon - -TURN_PLUS_3_FLUSH: - move_done -> WAIT_BOT_NOT_FULL - cylinder flushed, we test the 2 close conditions before reopen cylinder - -TURN_PLUS_1_CLOSE: - move_done -> WAIT_BOT_NOT_FULL - close order executed, test the 2 close conditions before reopen cylinder diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h deleted file mode 100644 index 84e1a401..00000000 --- a/digital/io/src/cylinder.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef cylinder_h -#define cylinder_h -/* cylinder.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ - -#include "common.h" - -/* last state of optical fence */ -extern uint8_t cylinder_of_state; - -/* nb puck in cylinder */ -extern uint8_t cylinder_nb_puck; - -/* of state */ -extern uint8_t cylinder_puck_on_of; - -/* force cylinder to be closed */ -extern uint8_t cylinder_close_order; - -/* flush puck on the cylinder */ -extern uint8_t cylinder_flush_order; - -/* distributor fucked */ -extern uint8_t cylinder_distributor_fucked; - -/* distributor empty */ -extern uint8_t cylinder_distributor_empty; - -/* mecanical offset */ -#define CYLINDER_OFFSET 0x43e - -/* mecanical offset for of probe */ -#define CYLINDER_OF_OFFSET 0.7 -#endif /* cylinder_h */ diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c deleted file mode 100644 index 9e7ecdda..00000000 --- a/digital/io/src/cylinder_cb.c +++ /dev/null @@ -1,441 +0,0 @@ -/* cylinder_cb.c - cylinder FSM callbacks. */ -/* {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "fsm.h" -#include "cylinder_cb.h" -#include "asserv.h" -#include "cylinder.h" -#include "filterbridge.h" -#include "elevator.h" -#include "top.h" -#include "modules/trace/trace.h" -#include "trace_event.h" - -/* locales variables */ -/* is there a puck on pos 2 or 3 */ -uint8_t puck_on_cylinder = 0; -/* is the cylinder are in of_offset mode */ -uint8_t of_offset_enabled = 0; - -/* - * IDLE =start=> - * => WAIT_FOR_JACK_IN - * we wait the jack before moving anything - */ -fsm_branch_t -cylinder__IDLE__start (void) -{ - return cylinder_next (IDLE, start); -} - -/* - * WAIT_FOR_JACK_IN =jack_inserted_into_bot=> - * => RESET_POS - * we init the cylinder position - */ -fsm_branch_t -cylinder__WAIT_FOR_JACK_IN__jack_inserted_into_bot (void) -{ - asserv_arm_zero_position(); - return cylinder_next (WAIT_FOR_JACK_IN, jack_inserted_into_bot); -} - -/* - * RESET_POS =move_done=> - * => INIT_POS - * move the cylinder to open it - */ -fsm_branch_t -cylinder__RESET_POS__move_done (void) -{ - asserv_move_arm(CYLINDER_OFFSET, - ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next (RESET_POS, move_done); -} - -/* - * INIT_POS =move_done=> - * => WAIT_A_PUCK - * the cylinder is ready to get pucks - */ -fsm_branch_t -cylinder__INIT_POS__move_done (void) -{ - return cylinder_next (INIT_POS, move_done); -} - -/* - * WAIT_A_PUCK =new_puck=> - * => WAIT_BRIDGE_READY - * look if the bridge is ready before move - */ -fsm_branch_t -cylinder__WAIT_A_PUCK__new_puck (void) -{ - return cylinder_next (WAIT_A_PUCK, new_puck); -} - -/* - * WAIT_A_PUCK =close_order=> - * => TURN_PLUS_1_CLOSE - * we close cylinder as requested - */ -fsm_branch_t -cylinder__WAIT_A_PUCK__close_order (void) -{ - asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - /*TODO check new puck */ - return cylinder_next (WAIT_A_PUCK, close_order); -} - -/* - * WAIT_A_PUCK =flush_order=> - * => WAIT_BRIDGE_READY_FLUSH - * flush all pucks to the bridge - */ -fsm_branch_t -cylinder__WAIT_A_PUCK__flush_order (void) -{ - return cylinder_next (WAIT_A_PUCK, flush_order); -} - -/* - * WAIT_A_PUCK =distrib_fucked=> - * => WAIT_BRIDGE_READY_DISTRIB - * we verify bridge - */ -fsm_branch_t -cylinder__WAIT_A_PUCK__distrib_fucked (void) -{ - return cylinder_next (WAIT_A_PUCK, distrib_fucked); -} - -/* - * WAIT_BRIDGE_READY =bridge_ready=> - * => TURN_PLUS_1_AND_OFO - * open the cylinder with the puck or not. - */ -fsm_branch_t -cylinder__WAIT_BRIDGE_READY__bridge_ready (void) -{ - of_offset_enabled = 1; - asserv_move_arm((1+CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next (WAIT_BRIDGE_READY, bridge_ready); -} - -/* - * TURN_PLUS_1_AND_OFO =move_done=> - * bot_not_full => TURN_PLUS_1_AND_MINUS_OFO - * open the cylinder to wait a new puck - * bot_full => WAIT_BOT_NOT_FULL - * bot full, waiting for pucks teleportation - */ -fsm_branch_t -cylinder__TURN_PLUS_1_AND_OFO__move_done (void) -{ - /* we verify if we drop a puck to the bridge */ - if(puck_on_cylinder) - { - if(cylinder_nb_puck) --cylinder_nb_puck; - ++fb_nb_puck; - } - /* We probe the OF to see if we have a new puck */ - puck_on_cylinder = asserv_arm_of_status(); - if(puck_on_cylinder) - { - ++top_total_puck_taken; - ++top_puck_inside_bot; - ++cylinder_nb_puck; - } - TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, - cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); - if(top_puck_inside_bot < 4) - { - of_offset_enabled = 0; - asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next_branch (TURN_PLUS_1_AND_OFO, move_done, bot_not_full); - } - return cylinder_next_branch (TURN_PLUS_1_AND_OFO, move_done, bot_full); -} - -/* - * TURN_PLUS_1_AND_MINUS_OFO =move_done=> - * => WAIT_A_PUCK - * ready for other pucks - */ -fsm_branch_t -cylinder__TURN_PLUS_1_AND_MINUS_OFO__move_done (void) -{ - return cylinder_next (TURN_PLUS_1_AND_MINUS_OFO, move_done); -} - -/* - * WAIT_BOT_NOT_FULL =bot_not_full=> - * => WAIT_CLEAR_ORDER - * the bot is not full, we go testing the other close condition - */ -fsm_branch_t -cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void) -{ - cylinder_distributor_fucked = 0; - return cylinder_next (WAIT_BOT_NOT_FULL, bot_not_full); -} - -/* - * WAIT_BOT_NOT_FULL =flush_order=> - * => TURN_PLUS_1_FLUSH - * flush order received, go open the cylinder - */ -fsm_branch_t -cylinder__WAIT_BOT_NOT_FULL__flush_order (void) -{ - cylinder_distributor_fucked = 0; - if(of_offset_enabled) - { - asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - of_offset_enabled = 0; - } - else - asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next (WAIT_BOT_NOT_FULL, flush_order); -} - -/* - * WAIT_CLEAR_ORDER =no_close_order=> - * => TURN_PLUS_1_OPEN - * no close order, we reopen cylinder to get other pucks - */ -fsm_branch_t -cylinder__WAIT_CLEAR_ORDER__no_close_order (void) -{ - if(of_offset_enabled) - { - asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - of_offset_enabled = 0; - } - else - asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next (WAIT_CLEAR_ORDER, no_close_order); -} - -/* - * WAIT_CLEAR_ORDER =flush_order=> - * => TURN_PLUS_1_FLUSH - * flush order received, go open the cylinder - */ -fsm_branch_t -cylinder__WAIT_CLEAR_ORDER__flush_order (void) -{ - if(of_offset_enabled) - { - asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - of_offset_enabled = 0; - } - else - asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next (WAIT_CLEAR_ORDER, flush_order); -} - -/* - * TURN_PLUS_1_OPEN =move_done=> - * => WAIT_A_PUCK - * cylinder ready to get other pucks - */ -fsm_branch_t -cylinder__TURN_PLUS_1_OPEN__move_done (void) -{ - return cylinder_next (TURN_PLUS_1_OPEN, move_done); -} - -/* - * TURN_PLUS_1_FLUSH =move_done=> - * => WAIT_BRIDGE_READY_FLUSH - * we wait the bridge before moving - */ -fsm_branch_t -cylinder__TURN_PLUS_1_FLUSH__move_done (void) -{ - return cylinder_next (TURN_PLUS_1_FLUSH, move_done); -} - -/* - * WAIT_BRIDGE_READY_FLUSH =bridge_ready=> - * => TURN_PLUS_3_FLUSH - * bridge is ready, flush gordon - */ -fsm_branch_t -cylinder__WAIT_BRIDGE_READY_FLUSH__bridge_ready (void) -{ - asserv_move_arm(3*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next (WAIT_BRIDGE_READY_FLUSH, bridge_ready); -} - -/* - * TURN_PLUS_3_FLUSH =move_done=> - * => WAIT_BOT_NOT_FULL - * cylinder flushed, we test the 2 close conditions before reopen cylinder - */ -fsm_branch_t -cylinder__TURN_PLUS_3_FLUSH__move_done (void) -{ - cylinder_flush_order = 0; - if(puck_on_cylinder) - { - if(cylinder_nb_puck) --cylinder_nb_puck; - ++fb_nb_puck; - puck_on_cylinder = 0; - } - TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, - cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); - return cylinder_next (TURN_PLUS_3_FLUSH, move_done); -} - -/* - * TURN_PLUS_1_CLOSE =move_done=> - * => WAIT_BOT_NOT_FULL - * close order executed, test the 2 close conditions before reopen cylinder - */ -fsm_branch_t -cylinder__TURN_PLUS_1_CLOSE__move_done (void) -{ - if(puck_on_cylinder) - { - if(cylinder_nb_puck) --cylinder_nb_puck; - ++fb_nb_puck; - puck_on_cylinder = 0; - } - TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, - cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); - return cylinder_next (TURN_PLUS_1_CLOSE, move_done); -} - -/* - * WAIT_BRIDGE_READY_DISTRIB =bridge_ready=> - * => TURN_PLUS_1_AND_OFO_DISTRIB - * we turn to check the of - */ -fsm_branch_t -cylinder__WAIT_BRIDGE_READY_DISTRIB__bridge_ready (void) -{ - asserv_move_arm((1+CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - of_offset_enabled = 1; - return cylinder_next (WAIT_BRIDGE_READY_DISTRIB, bridge_ready); -} - -/* - * TURN_PLUS_1_AND_OFO_DISTRIB =move_done=> - * => PROBE_OF - * We probe the puck - */ -fsm_branch_t -cylinder__TURN_PLUS_1_AND_OFO_DISTRIB__move_done (void) -{ - if(puck_on_cylinder) - { - if(cylinder_nb_puck) --cylinder_nb_puck; - ++fb_nb_puck; - puck_on_cylinder = 0; - } - TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, - cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); - return cylinder_next (TURN_PLUS_1_AND_OFO_DISTRIB, move_done); -} - -/* - * TURN_PLUS_1_AND_MINUS_OFO_DISTRIB =move_done=> - * => WAIT_A_PUCK - * bridge empty, return to normal mode - */ -fsm_branch_t -cylinder__TURN_PLUS_1_AND_MINUS_OFO_DISTRIB__move_done (void) -{ - cylinder_distributor_fucked = 0; - return cylinder_next (TURN_PLUS_1_AND_MINUS_OFO_DISTRIB, move_done); -} - -/* - * TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP =move_done=> - * => WAIT_BRIDGE_READY_DISTRIB - * ready to get another puck - */ -fsm_branch_t -cylinder__TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP__move_done (void) -{ - return cylinder_next (TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP, move_done); -} - -/* - * PROBE_OF =of_puck=> - * bot_full => WAIT_BOT_NOT_FULL - * return to normal mode with a bot full of puck - * bot_not_full => TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP - * we get a puck and we are hungry yet, go eat an another puck - */ -fsm_branch_t -cylinder__PROBE_OF__of_puck (void) -{ - /* we have a new puck on cylinder */ - puck_on_cylinder =1; - ++cylinder_nb_puck; - ++top_total_puck_taken; - ++top_puck_inside_bot; - if(top_puck_inside_bot < 4) - { - asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - of_offset_enabled = 0; - return cylinder_next_branch (PROBE_OF, of_puck, bot_not_full); - } - cylinder_distributor_fucked = 0; - TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, - cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); - return cylinder_next_branch (PROBE_OF, of_puck, bot_full); -} - -/* - * PROBE_OF =of_no_puck=> - * => TURN_PLUS_1_AND_MINUS_OFO_DISTRIB - * Distributor empty, go elsewhere - */ -fsm_branch_t -cylinder__PROBE_OF__of_no_puck (void) -{ - cylinder_distributor_empty = 1; - asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - of_offset_enabled = 0; - return cylinder_next (PROBE_OF, of_no_puck); -} diff --git a/digital/io/src/eeprom_dump.txt b/digital/io/src/eeprom_dump.txt deleted file mode 100644 index 1fdef894..00000000 --- a/digital/io/src/eeprom_dump.txt +++ /dev/null @@ -1,4 +0,0 @@ -# Servo Configuration -# Last update, Saturday 09 may (pre-coupe Origin, with Marcel). -!p002573 -!p017028 diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c deleted file mode 100644 index 1843fb42..00000000 --- a/digital/io/src/elevator.c +++ /dev/null @@ -1,119 +0,0 @@ -/* elevator.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "elevator.h" - -/** - * State of the elevator - */ -uint8_t elvt_is_ready = 0; - -/** - * nb puck in the elevator - */ -uint8_t elvt_nb_puck = 0; - -/** - * new_puck information (from filterbridge) - */ -uint8_t elvt_new_puck = 0; - -/** - * elevator orders - */ -elvt_order_e elvt_order_position = CLOSE; -uint8_t elvt_order_in_progress = 0; -uint8_t elvt_degraded_mode = 0; -uint8_t elvt_position_required = 0; - -/** - * We prepare the elevator -*/ -void -elvt_prepare(uint8_t pos) -{ - if(elvt_order_position == CLOSE) - { - elvt_order_in_progress = 1; - elvt_order_position = PREPARE; - elvt_position_required = pos; - } - else /* if we are already prepared, just say it's done */ - elvt_order_in_progress = 0; -} - -/** - * We open the elevator -*/ -void -elvt_open(uint8_t pos) -{ - if(elvt_order_position == PREPARE) - { - elvt_order_in_progress = 1; - elvt_order_position = OPEN; - elvt_position_required = pos; - elvt_degraded_mode = 0; - } - else /* We are already open, just say it's done and whistle */ - elvt_order_in_progress = 0; -} - -/** - * We open the elevator in degradad mode -*/ -void -elvt_open_degraded(uint8_t pos) -{ - if(elvt_order_position == PREPARE) - { - elvt_order_in_progress = 1; - elvt_order_position = OPEN; - elvt_position_required = pos; - elvt_degraded_mode = 1; - } - else /* We are already open, just say it's done and whistle */ - elvt_order_in_progress = 0; -} - - -/** - * We close the elevator and go away -*/ -void -elvt_close(void) -{ - if(elvt_order_position == OPEN) - { - elvt_order_in_progress = 1; - elvt_order_position = CLOSE; - } - else /* We are already close, just say it's done and play maracas */ - elvt_order_in_progress = 0; -} - -/* +AutoDec */ -/* -AutoDec */ - diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm deleted file mode 100644 index fee7b6ba..00000000 --- a/digital/io/src/elevator.fsm +++ /dev/null @@ -1,125 +0,0 @@ -# Elevator FSM -# Control the elevator's position and doors. -elevator - -States: - IDLE - waiting for the beginning of the round - WAIT_JACK_IN - waiting for the jack - INIT - init the elevator position and open door - GO_TO_POS_X - go to a position to be ready to get a new puck - WAIT_A_PUCK - waiting for a new puck (hurry up filter bridge!) - WAIT_BRIDGE_EMPTY[timeout=675] - waiting bridge empty - WAIT_POS_ORDER - waiting for position order (depending where we want to drop our column) - GO_TO_POS_Y - go to a position + 1cm to be ready to release pucks - WAIT_FOR_RELEASE_ORDER - ready to open doors at your command - LAND_ELEVATOR - land elevator to the ground - MINI_CLOSE[timeout=45] - try to unblock pucks - OPEN_DOORS[timeout=450] - releasing pucks - WAIT_FOR_CLOSE_ORDER - ready to close doors at your command - CLOSE_DOORS[timeout=675] - closing the elevator's doors - - -Events: - start - io power on! - jack_inserted_into_bot - jack into the bot - in_position - elevator is in position - new_puck - we received a new puck from FB (filter bridge, not facebook) - order_bypass - Dont wait to have 4 pucks but we wait we don't have puck before bridge - bridge_empty - the bridge is empty - order_received - we received a new order - doors_opened - doors are opened - state_timeout - -IDLE: - start -> WAIT_JACK_IN - waiting for jack - -WAIT_JACK_IN: - jack_inserted_into_bot -> INIT - open doors. - find the zero of the elevator. - -INIT: - doors_opened -> GO_TO_POS_X - doors opened, close doors. - -GO_TO_POS_X: - in_position -> WAIT_A_PUCK -# FIXME, we should go to a WAIT_FIRST_POS because we are not really ready to -# load some pucks (but this will not happen...). - elevator in position zero, move to first position. - -WAIT_A_PUCK: - new_puck: ok_for_other_pucks -> GO_TO_POS_X - incrementing nb_puck var - update elevator position to get a new puck - new_puck: not_ok_for_other_pucks -> WAIT_POS_ORDER - incrementing nb_puck var - no more time to wait a new puck - order_bypass -> WAIT_BRIDGE_EMPTY - elevator filling has been shut, get ready to drop pucks - -WAIT_BRIDGE_EMPTY: - bridge_empty -> GO_TO_POS_Y - bridge is empty, we are happy, we drop the column - state_timeout -> GO_TO_POS_Y - we suppose bridge is empty and we said it to the fsm - new_puck -> WAIT_A_PUCK - We have a new puck, elevator must move, we go to WAIT_A_PUCK - - -WAIT_POS_ORDER: - order_received -> GO_TO_POS_Y - go to position Y - -GO_TO_POS_Y: - in_position -> WAIT_FOR_RELEASE_ORDER - ready to release pucks at altitude Y - -WAIT_FOR_RELEASE_ORDER: - order_received -> LAND_ELEVATOR - make the elevator touch the target zone - -LAND_ELEVATOR: - in_position -> OPEN_DOORS - release pucks to the target position (I hope) - -OPEN_DOORS: - doors_opened -> WAIT_FOR_CLOSE_ORDER - wait for close order - state_timeout -> MINI_CLOSE - try to unblock doors - -MINI_CLOSE: - state_timeout -> OPEN_DOORS - try to release pucks again - -WAIT_FOR_CLOSE_ORDER: - order_received -> CLOSE_DOORS - closing doors - -CLOSE_DOORS: - state_timeout -> GO_TO_POS_X - pucks are released and elevator is ready to make a new column diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h deleted file mode 100644 index c8d3a780..00000000 --- a/digital/io/src/elevator.h +++ /dev/null @@ -1,91 +0,0 @@ -#ifndef elevator_h -#define elevator_h -/* elevator.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ - -/** - * State of the elevator - */ -extern uint8_t elvt_is_ready; - -/** - * nb puck in the elevator - */ -extern uint8_t elvt_nb_puck; - -/** - * new_puck information (from filterbridge) - */ -extern uint8_t elvt_new_puck; - -/** - * elevator orders - */ -typedef enum elvt_order_e {CLOSE, PREPARE, OPEN} elvt_order_e; -extern elvt_order_e elvt_order_position; -extern uint8_t elvt_order_in_progress; -extern uint8_t elvt_degraded_mode; -extern uint8_t elvt_position_required; - -/** - * We prepare the elevator -*/ -void -elvt_prepare(uint8_t pos); - -/** - * We open the elevator -*/ -void -elvt_open(uint8_t pos); - -/** - * We open the elevator in degradad mode -*/ -void -elvt_open_degraded(uint8_t pos); - -/** - * We close the elevator and go away -*/ -void -elvt_close(void); - -/** - * conversion stop/millimeter - */ - -#define ELEVATOR_MM_TO_STEP 72.34 - -/** - * pwm constant for elevator doors - */ - -#define OPEN_DOOR_PWM 0x200 -#define CLOSE_DOOR_PWM -0x200 -#define TIME_DOORS_PWM 0xB0 -#define TIME_LIGHT_DOORS_PWM 0xaa - -#endif // elevator_h diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c deleted file mode 100644 index 6d6c79c6..00000000 --- a/digital/io/src/elevator_cb.c +++ /dev/null @@ -1,320 +0,0 @@ -/* elevator_cb.c - elevator FSM callbacks. */ -/* {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "fsm.h" -#include "elevator_cb.h" -#include "elevator.h" -#include "asserv.h" -#include "pwm.h" -#include "chrono.h" -#include "filterbridge.h" -#include "top.h" -#include "modules/trace/trace.h" -#include "trace_event.h" -#include "cylinder.h" - -/* Positions when waiting a puck*/ -uint16_t posx[4] = -{ - 3 * ELEVATOR_MM_TO_STEP, - 20 * ELEVATOR_MM_TO_STEP, - 50 * ELEVATOR_MM_TO_STEP, - 80 * ELEVATOR_MM_TO_STEP -}; -/* Positions when we go to a target zone */ -uint16_t posy[3] = -{ - 135 * ELEVATOR_MM_TO_STEP, - 105 * ELEVATOR_MM_TO_STEP, - 75 * ELEVATOR_MM_TO_STEP -}; - -/* increase/decrease of pos y */ -#define MAJ_POSY (10 * ELEVATOR_MM_TO_STEP) -#define MIN_POSY (5 * ELEVATOR_MM_TO_STEP) - -/* time limit */ -/* TODO to be define */ -#define OK_TIME_LIMIT 20 - -/* - * IDLE =start=> - * => WAIT_JACK_IN - * waiting for jack - */ -fsm_branch_t -elevator__IDLE__start (void) -{ - return elevator_next (IDLE, start); -} - -/* - * WAIT_JACK_IN =jack_inserted_into_bot=> - * => INIT - * open doors. - * find the zero of the elevator. - */ -fsm_branch_t -elevator__WAIT_JACK_IN__jack_inserted_into_bot (void) -{ - /* Open doors. */ - pwm_set (OPEN_DOOR_PWM, 0); - /* Find the zero of the elevator. */ - asserv_elevator_zero_position (); - return elevator_next (WAIT_JACK_IN, jack_inserted_into_bot); -} - -/* - * INIT =doors_opened=> - * => GO_TO_POS_X - * doors opened, close doors. - */ -fsm_branch_t -elevator__INIT__doors_opened (void) -{ - /* FIXME: why this is here? */ - elvt_is_ready = 0; - /* Close the door. */ - pwm_set (CLOSE_DOOR_PWM, 2*TIME_DOORS_PWM); - return elevator_next (INIT, doors_opened); -} - -/* - * GO_TO_POS_X =in_position=> - * => WAIT_A_PUCK - * elevator in position zero, move to first position. - */ -fsm_branch_t -elevator__GO_TO_POS_X__in_position (void) -{ - /* FIXME: this sucks, look at elevator.fsm for a real fix. */ - elvt_is_ready = 1; - /* move to first position. */ - asserv_move_elevator_absolute(posx[elvt_nb_puck], - ASSERV_ELVT_SPEED_DEFAULT); - return elevator_next (GO_TO_POS_X, in_position); -} - -/* - * WAIT_A_PUCK =new_puck=> - * ok_for_other_pucks => GO_TO_POS_X - * incrementing nb_puck var - * update elevator position to get a new puck - * not_ok_for_other_pucks => WAIT_POS_ORDER - * incrementing nb_puck var - * no more time to wait a new puck - */ -fsm_branch_t -elevator__WAIT_A_PUCK__new_puck (void) -{ - elvt_is_ready = 0; - elvt_new_puck = 0; - // TODO time_ok - if(elvt_nb_puck < 4) - { -/* && - ((chrono_remaining_time() - OK_TIME_LIMIT > 0) - || nb_puck_fb != 0)) - */ - asserv_move_elevator_absolute(posx[elvt_nb_puck], - ASSERV_ELVT_SPEED_DEFAULT); - return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks); - } - else - return elevator_next_branch (WAIT_A_PUCK, new_puck, not_ok_for_other_pucks); -} - -/* - * WAIT_A_PUCK =order_bypass=> - * => WAIT_BRIDGE_EMPTY - * elevator filling has been shut, get ready to drop pucks - */ -fsm_branch_t -elevator__WAIT_A_PUCK__order_bypass (void) -{ - return elevator_next (WAIT_A_PUCK, order_bypass); -} - -/* - * WAIT_BRIDGE_EMPTY =bridge_empty=> - * => GO_TO_POS_Y - * bridge is empty, we are happy, we drop the column - */ -fsm_branch_t -elevator__WAIT_BRIDGE_EMPTY__bridge_empty (void) -{ - elvt_is_ready = 0; - asserv_move_elevator_absolute(posy[elvt_position_required - 1] - MAJ_POSY, - ASSERV_ELVT_SPEED_DEFAULT); - return elevator_next (WAIT_BRIDGE_EMPTY, bridge_empty); -} - -/* - * WAIT_BRIDGE_EMPTY =state_timeout=> - * => GO_TO_POS_Y - * we suppose bridge is empty and we said it to the fsm - */ -fsm_branch_t -elevator__WAIT_BRIDGE_EMPTY__state_timeout (void) -{ - elvt_is_ready = 0; - /* XXX bridge is empty but he don't know it yet */ - fb_nb_puck = 0; - asserv_move_elevator_absolute(posy[elvt_position_required - 1] - MAJ_POSY, - ASSERV_ELVT_SPEED_DEFAULT); - TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, - cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); - return elevator_next (WAIT_BRIDGE_EMPTY, state_timeout); -} - -/* - * WAIT_BRIDGE_EMPTY =new_puck=> - * => WAIT_A_PUCK - * We have a new puck, elevator must move, we go to WAIT_A_PUCK - */ -fsm_branch_t -elevator__WAIT_BRIDGE_EMPTY__new_puck (void) -{ - return elevator_next (WAIT_BRIDGE_EMPTY, new_puck); -} - -/* - * WAIT_POS_ORDER =order_received=> - * => GO_TO_POS_Y - * go to position Y - */ -fsm_branch_t -elevator__WAIT_POS_ORDER__order_received (void) -{ - asserv_move_elevator_absolute(posy[elvt_position_required - 1] - MAJ_POSY, - ASSERV_ELVT_SPEED_DEFAULT); - return elevator_next (WAIT_POS_ORDER, order_received); -} - -/* - * GO_TO_POS_Y =in_position=> - * => WAIT_FOR_RELEASE_ORDER - * ready to release pucks at altitude Y - */ -fsm_branch_t -elevator__GO_TO_POS_Y__in_position (void) -{ - elvt_order_in_progress = 0; - return elevator_next (GO_TO_POS_Y, in_position); -} - -/* - * WAIT_FOR_RELEASE_ORDER =order_received=> - * => LAND_ELEVATOR - * make the elevator touch the target zone - */ -fsm_branch_t -elevator__WAIT_FOR_RELEASE_ORDER__order_received (void) -{ - uint16_t true_pos = elvt_degraded_mode?posy[elvt_position_required - 1]: - posy[elvt_position_required - 1] + MIN_POSY; - asserv_move_elevator_absolute(true_pos, ASSERV_ELVT_SPEED_DEFAULT); - return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received); -} - -/* - * LAND_ELEVATOR =in_position=> - * => OPEN_DOORS - * release pucks to the target position (I hope) - */ -fsm_branch_t -elevator__LAND_ELEVATOR__in_position (void) -{ - pwm_set(OPEN_DOOR_PWM, 0); - return elevator_next (LAND_ELEVATOR, in_position); -} - -/* - * MINI_CLOSE =state_timeout=> - * => OPEN_DOORS - * try to release pucks again - */ -fsm_branch_t -elevator__MINI_CLOSE__state_timeout (void) -{ - pwm_set(OPEN_DOOR_PWM, 0); - return elevator_next (MINI_CLOSE, state_timeout); -} - -/* - * OPEN_DOORS =doors_opened=> - * => WAIT_FOR_CLOSE_ORDER - * wait for close order - */ -fsm_branch_t -elevator__OPEN_DOORS__doors_opened (void) -{ - top_puck_inside_bot -= elvt_nb_puck; - elvt_nb_puck = 0; - pwm_set(0,0); - elvt_order_in_progress = 0; - TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, - cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); - return elevator_next (OPEN_DOORS, doors_opened); -} - -/* - * OPEN_DOORS =state_timeout=> - * => MINI_CLOSE - * try to unblock doors - */ -fsm_branch_t -elevator__OPEN_DOORS__state_timeout (void) -{ - pwm_set(CLOSE_DOOR_PWM,0); - return elevator_next (OPEN_DOORS, state_timeout); -} - -/* - * WAIT_FOR_CLOSE_ORDER =order_received=> - * => CLOSE_DOORS - * closing doors - */ -fsm_branch_t -elevator__WAIT_FOR_CLOSE_ORDER__order_received (void) -{ - pwm_set(CLOSE_DOOR_PWM, 0); - return elevator_next (WAIT_FOR_CLOSE_ORDER, order_received); -} - -/* - * CLOSE_DOORS =state_timeout=> - * => GO_TO_POS_X - * pucks are released and elevator is ready to make a new column - */ -fsm_branch_t -elevator__CLOSE_DOORS__state_timeout (void) -{ - elvt_order_in_progress = 0; - pwm_set(0,0); - asserv_move_elevator_absolute(posx[elvt_nb_puck], - ASSERV_ELVT_SPEED_DEFAULT); - return elevator_next (CLOSE_DOORS, state_timeout); -} diff --git a/digital/io/src/filterbridge.c b/digital/io/src/filterbridge.c deleted file mode 100644 index 64fcbbfc..00000000 --- a/digital/io/src/filterbridge.c +++ /dev/null @@ -1,31 +0,0 @@ -/* filterbridge.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "filterbridge.h" - -uint8_t fb_nb_puck = 0; - -/* +AutoDec */ -/* -AutoDec */ - diff --git a/digital/io/src/filterbridge.fsm b/digital/io/src/filterbridge.fsm deleted file mode 100644 index 246aab0c..00000000 --- a/digital/io/src/filterbridge.fsm +++ /dev/null @@ -1,74 +0,0 @@ -# Filter Bridge FSM -# Filter column element (our colour -> go to lift, !our colour -> EJECT) -filterbridge - -States: - IDLE - waiting the lift is ready to accept new puck - WAIT_JACK_IN - waiting for the jack to be inserted into the bot - CLOSE_DOOR[timeout=225] - close access to the lift (and wait 1 sec) - WAIT_A_PUCK - waiting for a new puck - WAIT_LIFT_READY - waiting for elevator ready - MARCEL_WAIT[timeout=45] - waiting the puck is entirely in pos2 - OPEN_DOOR[timeout=225] - release puck to the lift and block a new puck on pos1 - PUSH_PUCK - push the puck to the lift - TEMPO_ELVETATOR_COMMUNICATION [timeout=225] - a temporisation for the communication between the eleveator and the FB. - -Events: - start - robot start - jack_inserted_into_bot - jack into the bot - lift_ready - the elevator is ready - puck_on_pos2 - a puck has been detected on bridge's position 2 - no_puck_on_pos2 - there is no puck on position 2 - state_timeout - timeout used to wait the end of servo move - -IDLE: - start -> WAIT_JACK_IN - we wait for the jack - -WAIT_JACK_IN: - jack_inserted_into_bot -> CLOSE_DOOR - we init the bridge - -CLOSE_DOOR: - state_timeout -> WAIT_A_PUCK - we are ready, we wait a puck - -WAIT_A_PUCK: - puck_on_pos2 -> WAIT_LIFT_READY - Puck ready to be dropped to the elevator. Test if elvt ready to get it. - -WAIT_LIFT_READY: - lift_ready -> MARCEL_WAIT - the lift is ready - -MARCEL_WAIT: - state_timeout -> OPEN_DOOR - lift ready, we deliver - -OPEN_DOOR: - state_timeout -> PUSH_PUCK - we push the puck to the lift - -PUSH_PUCK: - no_puck_on_pos2 -> TEMPO_ELVETATOR_COMMUNICATION - nothing to do. - -TEMPO_ELVETATOR_COMMUNICATION: - state_timeout -> CLOSE_DOOR - the puck disappears, we close doors - tell the elevator we are ready. diff --git a/digital/io/src/filterbridge.h b/digital/io/src/filterbridge.h deleted file mode 100644 index 235d29ee..00000000 --- a/digital/io/src/filterbridge.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef filterbridge_h -#define filterbridge_h -/* filterbridge.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ - -#include "common.h" - -extern uint8_t fb_nb_puck; - -#endif // filterbridge_h diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c deleted file mode 100644 index 2b4e452d..00000000 --- a/digital/io/src/filterbridge_cb.c +++ /dev/null @@ -1,149 +0,0 @@ -/* filterbridge_cb.c - filterbridge FSM callbacks. */ -/* {{{ - * - * Copyright (C) 2009 Nicolas Haller - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "fsm.h" -#include "filterbridge_cb.h" -#include "elevator.h" -#include "servo_pos.h" -#include "aquajim.h" -#include "filterbridge.h" -#include "top.h" -#include "cylinder.h" -#include "modules/trace/trace.h" -#include "trace_event.h" - -/* - * IDLE =start=> - * => WAIT_JACK_IN - * we wait for the jack - */ -fsm_branch_t -filterbridge__IDLE__start (void) -{ - return filterbridge_next (IDLE, start); -} - -/* - * WAIT_JACK_IN =jack_inserted_into_bot=> - * => CLOSE_DOOR - * we init the bridge - */ -fsm_branch_t -filterbridge__WAIT_JACK_IN__jack_inserted_into_bot (void) -{ - servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE); - servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE); - return filterbridge_next (WAIT_JACK_IN, jack_inserted_into_bot); -} - -/* - * CLOSE_DOOR =state_timeout=> - * => WAIT_A_PUCK - * we are ready, we wait a puck - */ -fsm_branch_t -filterbridge__CLOSE_DOOR__state_timeout (void) -{ - return filterbridge_next (CLOSE_DOOR, state_timeout); -} - -/* - * WAIT_A_PUCK =puck_on_pos2=> - * => WAIT_LIFT_READY - * Puck ready to be dropped to the elevator. Test if elvt ready to get it. - */ -fsm_branch_t -filterbridge__WAIT_A_PUCK__puck_on_pos2 (void) -{ - return filterbridge_next (WAIT_A_PUCK, puck_on_pos2); -} - -/* - * WAIT_LIFT_READY =lift_ready=> - * => MARCEL_WAIT - * the lift is ready - */ -fsm_branch_t -filterbridge__WAIT_LIFT_READY__lift_ready (void) -{ - return filterbridge_next (WAIT_LIFT_READY, lift_ready); -} - -/* - * MARCEL_WAIT =state_timeout=> - * => OPEN_DOOR - * lift ready, we deliver - */ -fsm_branch_t -filterbridge__MARCEL_WAIT__state_timeout (void) -{ - servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_OPEN); - return filterbridge_next (MARCEL_WAIT, state_timeout); -} - -/* - * OPEN_DOOR =state_timeout=> - * => PUSH_PUCK - * we push the puck to the lift - */ -fsm_branch_t -filterbridge__OPEN_DOOR__state_timeout (void) -{ - servo_pos_move_to(SERVO_FINGER_ID,SERVO_FINGER_PUSHING); - return filterbridge_next (OPEN_DOOR, state_timeout); -} - -/* - * PUSH_PUCK =no_puck_on_pos2=> - * => TEMPO_ELVETATOR_COMMUNICATION - * nothing to do. - */ -fsm_branch_t -filterbridge__PUSH_PUCK__no_puck_on_pos2 (void) -{ - return filterbridge_next (PUSH_PUCK, no_puck_on_pos2); -} - -/* - * TEMPO_ELVETATOR_COMMUNICATION =state_timeout=> - * => CLOSE_DOOR - * the puck disappears, we close doors - * tell the elevator we are ready. - */ -fsm_branch_t -filterbridge__TEMPO_ELVETATOR_COMMUNICATION__state_timeout (void) -{ - ++elvt_nb_puck; - if(fb_nb_puck) --fb_nb_puck; - top_puck_inside_bot = cylinder_nb_puck + fb_nb_puck + elvt_nb_puck; - elvt_new_puck = 1; - servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE); - servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE); - TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, - cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); - return filterbridge_next (TEMPO_ELVETATOR_COMMUNICATION, state_timeout); -} - - diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h index e08605fe..57e7ae30 100644 --- a/digital/io/src/fsm.h +++ b/digital/io/src/fsm.h @@ -109,9 +109,6 @@ fsm_handle_event (fsm_t *fsm, u8 event); #include "move_fsm.h" #include "top_fsm.h" -#include "filterbridge_fsm.h" -#include "elevator_fsm.h" -#include "cylinder_fsm.h" #include "init_fsm.h" #endif /* fsm_h */ diff --git a/digital/io/src/giboulee.h b/digital/io/src/giboulee.h deleted file mode 100644 index 88828a2a..00000000 --- a/digital/io/src/giboulee.h +++ /dev/null @@ -1,98 +0,0 @@ -#ifndef giboulee_h -#define giboulee_h -// giboulee.h -// io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ -// -// Copyright (C) 2008 Dufour Jérémy -// -// Robot APB Team/Efrei 2004. -// Web: http://assos.efrei.fr/robot/ -// Email: robot AT efrei DOT fr -// -// This program is free software; you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation; either version 2 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program; if not, write to the Free Software -// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. -// -// }}} - -/** - * @file Some defines for the bot itself. - * In this header file, you can find some configuration value for the length - * of a step when the bot is moving for example. - */ - -/** - * The speed of the arm. - * It is expressed in number of steps by asserv board cycle (4.44ms). - */ -#define BOT_ARM_SPEED 8 - -/** - * The number of step of a whole rotation. - */ -#define BOT_ARM_STEP_ROUND 5000 - -/** - * Number of step to make a third of a round. - */ -#define BOT_ARM_THIRD_ROUND (BOT_ARM_STEP_ROUND / 3) - -/** - * The number of steps to do to open the input hole when the arm is closing - * it. - * For the moment, 30 degree. - */ -#define BOT_ARM_MIN_TO_OPEN (BOT_ARM_STEP_ROUND * 30L / 360) - -/** - * Steps number relative to the beginning of a third round for the 'noted' - * position required by the get sample FSM. - */ -#define BOT_ARM_NOTED_POSITION (BOT_ARM_THIRD_ROUND - 100) - -/** - * How to compute a angle for giboulee? - * One degree is 65536 / 360 - */ -#define BOT_ANGLE_DEGREE (65536 / 360) - -/** - * The scaling factor, millimeter per step. - */ -#ifdef HOST -# define BOT_SCALE 0.0395840674352314 -#else -# define BOT_SCALE 0.0413530725332892 -#endif - -/** - * Distance required to be away from a border to be able to turn freely. - * In millimeter. - */ -#define BOT_MIN_DISTANCE_TURN_FREE 300 - -/** - * Definition of the colors. - */ -enum team_color_e -{ - RED_TEAM = 0, - BLUE_TEAM = 1 -}; - -/** - * Our color. - */ -extern enum team_color_e bot_color; - -#endif // giboulee_h diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c index d622cd68..5668d37c 100644 --- a/digital/io/src/init_cb.c +++ b/digital/io/src/init_cb.c @@ -29,10 +29,9 @@ #include "init.h" #include "playground.h" #include "main.h" -#include "aquajim.h" +#include "bot.h" #include "switch.h" #include "chrono.h" -#include "top.h" #include "playground.h" #include "modules/trace/trace.h" #include "modules/path/path.h" @@ -72,8 +71,6 @@ 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); } @@ -143,7 +140,6 @@ init__SET_ANGULAR_POSITION__asserv_last_cmd_ack (void) /* * GO_AWAY_FROM_THE_WALL =bot_move_succeed=> * => FACE_OTHER_WALL - * get and store the color of the bot. * turn to face the other wall. */ fsm_branch_t @@ -243,9 +239,6 @@ init__WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void) init_match_is_started = 1; /* Start the chrono. */ chrono_init (); - /* Set the central round to path module. */ - path_obstacle (0x01, PG_WIDTH / 2, PG_LENGTH / 2, 350, 0, - PATH_OBSTACLE_VALID_ALWAYS); return init_next (WAIT_SECOND_JACK_OUT, jack_removed_from_bot); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index ba47aac5..8d23b107 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -43,17 +43,13 @@ #include "asserv.h" /* Functions to control the asserv board */ #include "eeprom.h" /* Parameters loaded/stored in the EEPROM */ #include "fsm.h" /* fsm_* */ -#include "giboulee.h" /* team_color */ -/* #include "top.h" */ /* top_* */ +#include "bot.h" #include "servo_pos.h" #include "chrono.h" /* chrono_end_match */ #include "sharp.h" /* sharp module */ #include "pwm.h" #include "playground.h" #include "contact.h" -#include "elevator.h" -#include "filterbridge.h" -#include "cylinder.h" #include "init.h" #include "top.h" @@ -125,12 +121,6 @@ main_short_fsm_name (fsm_t *fsm) id = 'I'; else if (fsm == &move_fsm) id = 'M'; - else if (fsm == &elevator_fsm) - id = 'E'; - else if (fsm == &cylinder_fsm) - id = 'C'; - else if (fsm == &filterbridge_fsm) - id = 'F'; return id; } @@ -160,9 +150,6 @@ main_event_to_fsm (void) FSM_HANDLE_TIMEOUT (&move_fsm); FSM_HANDLE_TIMEOUT (&top_fsm); FSM_HANDLE_TIMEOUT (&init_fsm); - FSM_HANDLE_TIMEOUT (&filterbridge_fsm); - FSM_HANDLE_TIMEOUT (&elevator_fsm); - FSM_HANDLE_TIMEOUT (&cylinder_fsm); /* If we have entering this function, last command of the asserv board has * been aquited. */ @@ -171,13 +158,13 @@ main_event_to_fsm (void) asserv_status_e move_status = none, - arm_status = none, - elevator_status = none; + motor0_status = none, + motor1_status = none; - /* Get status of move, arm and elevator. */ + /* Get status of move, motor0 and motor1. */ move_status = asserv_move_cmd_status (); - arm_status = asserv_arm_cmd_status (); - elevator_status = asserv_elevator_cmd_status (); + motor0_status = asserv_motor0_cmd_status (); + motor1_status = asserv_motor1_cmd_status (); /* Check commands move status. */ if (move_status == success) @@ -199,92 +186,6 @@ main_event_to_fsm (void) TOP_EVENT_bot_move_failed); } - /* Check elevator status. */ - if (elevator_status == success) - { - FSM_HANDLE_EVENT (&elevator_fsm, - ELEVATOR_EVENT_in_position); - } - else if (elevator_status == failure) - { - /* TODO: */ - } - - /* Check cylinder status */ - if (arm_status != none) - { - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_move_done); - } - - /* check of status */ - if (asserv_arm_of_status()) - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_of_puck); - else - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_of_no_puck); - - - /* send event if elevator received an order */ - if (elvt_order_in_progress) - FSM_HANDLE_EVENT (&elevator_fsm, - ELEVATOR_EVENT_order_received); - else - FSM_HANDLE_EVENT (&top_fsm, - TOP_EVENT_elevator_order_done); - - /* relou case to avoid a loop */ - if(elvt_order_in_progress && !elvt_new_puck) - FSM_HANDLE_EVENT (&elevator_fsm, - ELEVATOR_EVENT_order_bypass); - - - /* elevator new puck (set by filterbridge) */ - if(elvt_new_puck) - FSM_HANDLE_EVENT (&elevator_fsm, - ELEVATOR_EVENT_new_puck); - /* elvt door switch */ - if(!IO_GET (CONTACT_ELEVATOR_DOOR)) - FSM_HANDLE_EVENT (&elevator_fsm, - ELEVATOR_EVENT_doors_opened); - /* bridge ready */ - if(fb_nb_puck < 2) - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_bridge_ready); - if(cylinder_distributor_fucked) - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_distrib_fucked); - /* elevator is ready */ - if(elvt_is_ready) - FSM_HANDLE_EVENT (&filterbridge_fsm, - FILTERBRIDGE_EVENT_lift_ready); - - /* bot empty */ - if(top_puck_inside_bot < 4) - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_bot_not_full); - - /* cylinder_close_order */ - if(cylinder_close_order) - { - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_close_order); - } - else - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_no_close_order); - - /* cylinder_flush_order */ - if(cylinder_flush_order) - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_flush_order); - - /* Generate the cylinder empty distributor. */ - if (cylinder_distributor_empty) - FSM_HANDLE_EVENT (&top_fsm, - TOP_EVENT_empty_distributor); - /* Jack */ if(switch_get_jack()) { @@ -295,12 +196,6 @@ main_event_to_fsm (void) { FSM_HANDLE_EVENT (&init_fsm, INIT_EVENT_jack_inserted_into_bot); - FSM_HANDLE_EVENT (&elevator_fsm, - ELEVATOR_EVENT_jack_inserted_into_bot); - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_jack_inserted_into_bot); - FSM_HANDLE_EVENT (&filterbridge_fsm, - CYLINDER_EVENT_jack_inserted_into_bot); } if (init_match_is_started) @@ -338,29 +233,6 @@ main_event_to_fsm (void) } } /* TODO: Check other sensors */ - /* TODO: implement filterbridge events */ - if(!IO_GET (CONTACT_FILTER_BRIDGE_PUCK)) - { - FSM_HANDLE_EVENT (&filterbridge_fsm, - FILTERBRIDGE_EVENT_puck_on_pos2); - } - else - { - FSM_HANDLE_EVENT (&filterbridge_fsm, - FILTERBRIDGE_EVENT_no_puck_on_pos2); - } - /* TODO check if we need !IO_GET or IO_GET */ - if(!IO_GET(CONTACT_PUCK_CYLINDER)) - { - FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_new_puck); - } - - if (top_puck_inside_bot >= 4) - { - FSM_HANDLE_EVENT (&top_fsm, - TOP_EVENT_bot_is_full_of_pucks); - } } /** @@ -385,21 +257,16 @@ main_init (void) switch_init (); /* Path module */ path_init (PG_BORDER_DISTANCE, PG_BORDER_DISTANCE, - PG_WIDTH - PG_BORDER_DISTANCE, PG_LENGTH - PG_BORDER_DISTANCE - - PG_BOTTOM_UNUSABLE_ZONE); + PG_WIDTH - PG_BORDER_DISTANCE, PG_LENGTH - PG_BORDER_DISTANCE); + /* Top initialization. */ + top_init (); /* Init all FSM (except move FSM) */ fsm_init(&top_fsm); fsm_init(&init_fsm); - fsm_init(&cylinder_fsm); - fsm_init(&elevator_fsm); - fsm_init(&filterbridge_fsm); /* Start all FSM (except move and top FSM) */ /* FIXME: who sould start top? init?. */ fsm_handle_event (&init_fsm, INIT_EVENT_start); fsm_handle_event (&top_fsm, TOP_EVENT_start); - fsm_handle_event (&filterbridge_fsm, FILTERBRIDGE_EVENT_start); - fsm_handle_event (&elevator_fsm, ELEVATOR_EVENT_start); - fsm_handle_event (&cylinder_fsm, CYLINDER_EVENT_start); /* Sharp module */ sharp_init (); /* PWM module */ @@ -541,41 +408,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) #define c(cmd, size) (cmd << 8 | size) switch (c (cmd, size)) { - /* cylinder orders */ - case c ('c', 1): - { - switch(args[0]) - { - case 'f': - cylinder_flush_order = 1; - break; - case 'C': - cylinder_close_order = 1; - break; - case 'c': - cylinder_close_order = 0; - break; - } - } - break; - - /* elevator (l like lift) */ - case c('l', 2): - { - switch(args[0]) - { - case 'o': - elvt_order_in_progress = 1; - elvt_position_required = args[1]; - break; - } - } - break; - - case c ('j', 0): - fsm_handle_event (&filterbridge_fsm, - FILTERBRIDGE_EVENT_jack_inserted_into_bot); - break; case c ('z', 0): /* Reset */ utils_reset (); @@ -720,10 +552,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Stop motor */ asserv_stop_motor (); break; - case 'F': - /* Go to the distributor */ - asserv_go_to_distributor (); - break; } } break; @@ -739,10 +567,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) switch (args[1]) { case 'e': - asserv_elevator_zero_position (); + asserv_motor1_zero_position (); break; case 'a': - asserv_arm_zero_position (); + asserv_motor0_zero_position (); break; } break; @@ -767,19 +595,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) switch (args[0]) { case 'b': - /* Move the arm + /* Move the motor0 * - 2b: offset angle ; * - 1b: speed. */ - asserv_move_arm (v8_to_v16 (args[1], args[2]), args[3]); + asserv_move_motor0 (v8_to_v16 (args[1], args[2]), args[3]); break; case 'c': - /* Move the elevator + /* Move the motor1 * - 2b: position ; * - 1b: speed. */ - asserv_move_elevator_absolute (v8_to_v16 (args[1], args[2]), - args[3]); + asserv_move_motor1_absolute (v8_to_v16 (args[1], args[2]), + args[3]); break; } } diff --git a/digital/io/src/main.h b/digital/io/src/main.h index 06f2a669..92793fcc 100644 --- a/digital/io/src/main.h +++ b/digital/io/src/main.h @@ -48,24 +48,10 @@ extern uint16_t main_sharp_ignore_event; */ extern uint16_t main_move_wait_cycle; -/** - * Post an event for the main loop to wake up the getsamples FSM in a certain - * count of cycles. - * This is used by the getsamples FSM to timeout if the arm is completely - * blocked. - */ -extern uint16_t main_getsamples_wait_cycle; - /** * Post an event for the init loop to wait a certain amount of time before * make any move */ extern uint16_t main_init_wait_cycle; -/** - * Flag for homologation, to disable the path finding and always stop in front - * of an obstacle and wait. - */ -extern uint8_t main_always_stop_for_obstacle; - #endif /* main_h */ diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index 28ee70db..900842b9 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -30,7 +30,7 @@ #include "playground.h" #include "move.h" #include "sharp.h" -#include "aquajim.h" +#include "bot.h" #include "trace_event.h" #include "main.h" /* main_post_event_for_top_fsm */ @@ -70,7 +70,7 @@ #define MOVE_OBSTACLE_RADIUS (MOVE_REAL_OBSTACLE_RADIUS + 250) /** - * The generic validity time (in term of number of cyles). + * The generic validity time (in term of number of cycles). */ #define MOVE_OBSTACLE_VALIDITY (6 * 225) @@ -274,7 +274,6 @@ move__MOVING__bot_move_succeed (void) } } - /* * MOVING =bot_move_failed=> * => MOVING_BACKWARD_TO_TURN_FREELY diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h index 8b863385..6e962423 100644 --- a/digital/io/src/playground.h +++ b/digital/io/src/playground.h @@ -34,7 +34,7 @@ * see it with the two start zone at the top of the scheme). */ -#include "giboulee.h" +#include "bot.h" /** * The width of the table, in millimeters. @@ -72,48 +72,6 @@ #define PG_Y_START (PG_HEIGHT - 70) #define PG_A_START (-90 * BOT_ANGLE_DEGREE) -/** - * The distance to remove from the real position of the distributor before - * doing the "go to the distributor". - */ -#define PG_DISTANCE_DISTRIBUTOR 300 - -/** - * The position of our ice distributor minus the distance to remove (only for the - * X axis). - */ -#define PG_DISTRIBUTOR_ICE_OUR_X \ - (PG_X_VALUE_COMPUTING (0 + PG_DISTANCE_DISTRIBUTOR)) -#define PG_DISTRIBUTOR_ICE_OUR_Y 1350 -#define PG_DISTRIBUTOR_ICE_OUR_A \ - (PG_A_VALUE_COMPUTING (180 * BOT_ANGLE_DEGREE)) - -/** - * The position of adverse ice distributor minus the distance to remove (only - * for the X axis). - */ -#define PG_DISTRIBUTOR_ICE_ADVERSE_X \ - (PG_X_VALUE_COMPUTING (PG_WIDTH - PG_DISTANCE_DISTRIBUTOR)) -#define PG_DISTRIBUTOR_ICE_ADVERSE_Y 1350 -#define PG_DISTRIBUTOR_ICE_ADVERSE_A \ - (PG_A_VALUE_COMPUTING (0)) - -/** - * The position of our distributor minus the distance to remove (only for the - * Y axis). - */ -#define PG_DISTRIBUTOR_SAMPLE_OUR_X \ - (PG_X_VALUE_COMPUTING (700)) -#define PG_DISTRIBUTOR_SAMPLE_OUR_Y (PG_HEIGHT - PG_DISTANCE_DISTRIBUTOR) -#define PG_DISTRIBUTOR_SAMPLE_OUR_A (90 * BOT_ANGLE_DEGREE) - -/** - * The position of the gutter. - */ -#define PG_GUTTER_X (PG_X_VALUE_COMPUTING (2500)) -#define PG_GUTTER_Y (300) -#define PG_GUTTER_A (90 * BOT_ANGLE_DEGREE) - /** * The backward move for the MOVE_FSM. */ @@ -125,9 +83,4 @@ #define PG_START_ZONE_LENGTH 500 #define PG_START_ZONE_WIDTH 500 -/** - * The bottom of the table is not usable. - */ -#define PG_BOTTOM_UNUSABLE_ZONE 150 - #endif // playground_h diff --git a/digital/io/src/servo.avr.c b/digital/io/src/servo.avr.c index 6689f06a..de50c819 100644 --- a/digital/io/src/servo.avr.c +++ b/digital/io/src/servo.avr.c @@ -56,7 +56,7 @@ * We want a time of 20ms (20/1000). * See @a servo_init to know the prescaler value of the timer/counter. */ -static const uint16_t servo_tic_cyle_ = AC_FREQ / 256 * 20 / 1000; +static const uint16_t servo_tic_cycle_ = AC_FREQ / 256 * 20 / 1000; /** @} */ @@ -141,7 +141,7 @@ SIGNAL (SIG_OVERFLOW2) overflow */ static int8_t servo_overflow_count = -1; /* Time spent by each servo motor at high state during a whole cycle */ - static uint16_t servo_high_time_cycle = servo_tic_cyle_; + static uint16_t servo_high_time_cycle = servo_tic_cycle_; /* State machine actions */ if (servo_updating_id_ >= 0) @@ -193,7 +193,7 @@ SIGNAL (SIG_OVERFLOW2) servo_updating_id_ = 0; /* Re-initialize the counter of time spent by each servo motor * at high state */ - servo_high_time_cycle = servo_tic_cyle_; + servo_high_time_cycle = servo_tic_cycle_; } } } diff --git a/digital/io/src/switch.h b/digital/io/src/switch.h index 2a2a28c4..ac8d7dbc 100644 --- a/digital/io/src/switch.h +++ b/digital/io/src/switch.h @@ -29,7 +29,7 @@ * @file Module to manage 'switchs'. For example, colors selector and jack. */ -#include "giboulee.h" /* team_color_e */ +#include "bot.h" #include "common.h" /** diff --git a/digital/io/src/test/common.host.c b/digital/io/src/test/common.host.c deleted file mode 100644 index c6586e58..00000000 --- a/digital/io/src/test/common.host.c +++ /dev/null @@ -1,154 +0,0 @@ -/* common.c */ -/* {{{ - * - * Copyright (C) 2008 Dufour Jérémy - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ - -#include "common.h" -#include "../giboulee.h" - -#include - -static uint16_t asserv_arm_position = 0; - -/* Yerk export */ -enum team_color_e bot_color = RED_TEAM; - -/* Define functions for debug */ -void -trap_setup_path_to_box (uint8_t box_id) -{ - printf ("[trap] Configure trap doors to open %d.\n", box_id); -} - -void -trap_close_rear_panel (void) -{ - printf ("[trap] Closing rear panel.\n"); -} - -void -trap_open_rear_panel (void) -{ - printf ("[trap] Opening rear panel.\n"); -} - -void -asserv_move_linearly (int32_t distance) -{ - printf ("[asserv] Make the bot move linearly of %d mm.\n", distance); -} - -void -asserv_move_arm (uint16_t position, uint8_t speed) -{ - asserv_arm_position += position; - printf ("[asserv] Move arm at %d (speed: %d).\n", - asserv_arm_position, speed); -} - -void -asserv_close_input_hole (void) -{ - printf ("[asserv] Put the arm in front of the input hole.\n"); - asserv_move_arm (asserv_arm_position % - BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED); -} - -uint16_t -asserv_get_arm_position (void) -{ - return asserv_arm_position; -} - -void -asserv_arm_set_position_reached (uint16_t position) -{ - printf ("[asserv] Arm notifier at position %d (we are at %d).\n", - position, asserv_arm_position); -} - -void -asserv_go_to_distributor (void) -{ - printf ("[asserv] Go to distributor.\n"); -} - -void -asserv_get_position (void *undef) -{ - printf ("[asserv] Asking position of the bot.\n"); -} - -void -asserv_goto (uint32_t x, uint32_t y) -{ - printf ("[asserv] Move the bot to (%d; %d).\n", x, y); -} - -void -asserv_goto_angle (int16_t angle) -{ - printf ("[asserv] Move the bot to face %X.\n", angle); -} - -void -asserv_set_x_position (int32_t x) -{ - printf ("[asserv] Set X position to %d.\n", x); -} - -void -asserv_set_y_position (int32_t y) -{ - printf ("[asserv] Set Y position to %d.\n", y); -} - -void -asserv_set_angle_position (int16_t a) -{ - printf ("[asserv] Set angle position to %d.\n", a); -} - -void -asserv_set_position (int32_t x, int32_t y, int16_t a) -{ - printf ("[asserv] Set bot position to (%d, %d, %d).\n", x, y, a); -} - -void -asserv_go_to_the_wall (void) -{ - printf ("[asserv] Go to the wall.\n"); -} - -void -chrono_init (void) -{ - printf ("[chrono] Chrono init.\n"); -} - -void -gutter_start (void) -{ - printf ("[FSM:gutter] Start the gutter FSM\n"); -} diff --git a/digital/io/src/test/gutter/Makefile b/digital/io/src/test/gutter/Makefile deleted file mode 100644 index fb829dc8..00000000 --- a/digital/io/src/test/gutter/Makefile +++ /dev/null @@ -1,17 +0,0 @@ -BASE= ../../../../avr -HOST_PROGS = gutter -IOBASE = ../.. -gutter_SOURCES = main.c ../common.host.c $(IOBASE)/fsm.c \ - $(IOBASE)/gutter_cb.c $(IOBASE)/gutter_fsm.c $(IOBASE)/gutter.c \ - $(IOBASE)/getsamples_cb.c $(IOBASE)/getsamples_fsm.c $(IOBASE)/getsamples.c \ - $(IOBASE)/top.c $(IOBASE)/top_cb.c $(IOBASE)/top_fsm.c \ - $(IOBASE)/move.c $(IOBASE)/move_cb.c $(IOBASE)/move_fsm.c \ - $(IOBASE)/simu.host.c - -MODULES = -CONFIGFILE = $(IOBASE)/avrconfig.h -# -O2 : speed -# -Os : size -OPTIMIZE = -O2 - -include $(BASE)/make/Makefile.gen diff --git a/digital/io/src/test/gutter/main.c b/digital/io/src/test/gutter/main.c deleted file mode 100644 index 39acaf28..00000000 --- a/digital/io/src/test/gutter/main.c +++ /dev/null @@ -1,49 +0,0 @@ -/* main.c */ -/* {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "../../fsm.h" -#include "../../playground.h" - -#include - -int -main (void) -{ - fsm_init (&gutter_fsm); - - fsm_handle_event (&gutter_fsm, GUTTER_EVENT_start); - - /* Move (angle here) command succeed */ - fsm_handle_event (&gutter_fsm, GUTTER_EVENT_bot_move_succeed); - - /* Move (go to the wall) command succeed */ - fsm_handle_event (&gutter_fsm, GUTTER_EVENT_bot_move_succeed); - - /* We have wait enough time for the all the balls to falls */ - fsm_handle_event (&gutter_fsm, GUTTER_EVENT_wait_finished); - - return 0; -} - diff --git a/digital/io/src/test/move/Makefile b/digital/io/src/test/move/Makefile deleted file mode 100644 index 4b301734..00000000 --- a/digital/io/src/test/move/Makefile +++ /dev/null @@ -1,13 +0,0 @@ -BASE= ../../../../avr -HOST_PROGS = fsm -IOBASE = ../.. -fsm_SOURCES = main.c $(IOBASE)/fsm.c \ - $(IOBASE)/move_cb.c $(IOBASE)/move_fsm.c \ - $(IOBASE)/move.c -MODULES = utils -CONFIGFILE = -# -O2 : speed -# -Os : size -OPTIMIZE = -O2 - -include $(BASE)/make/Makefile.gen diff --git a/digital/io/src/test/move/main.c b/digital/io/src/test/move/main.c deleted file mode 100644 index 25ca08b7..00000000 --- a/digital/io/src/test/move/main.c +++ /dev/null @@ -1,135 +0,0 @@ -/* main.c */ -/* {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" -#include "../../fsm.h" -#include "../../asserv.h" -#include "../../move.h" - -#include - -/** - * Status structure maintains by the update command. - */ -typedef struct asserv_struct_s -{ - /** Status flags. */ - uint8_t status; - /** Sequence number. */ - uint8_t seq; - /** Bot position. */ - asserv_position_t position; - /** Arm position. */ - uint16_t arm_position; -} asserv_struct_s; - -/** - * Status variable. - */ -asserv_struct_s asserv_status; - - -void -move_print_test (fsm_t *move) -{ - printf ("Machine state "); - - switch (move->state_current) - { - case MOVE_STATE_IDLE: - printf ("IDLE"); - break; - case MOVE_STATE_DESIRED_POSITION: - printf ("DESIRED POSITION"); - break; - case MOVE_STATE_MOVE_ON_RIGHT: - printf ("MOVE ON RIGHT"); - break; - case MOVE_STATE_MOVE_ON_LEFT: - printf ("MOVE ON LEFT"); - break; - default: - printf ("STATE_NB"); - } - printf ("\n"); -} - -int -main (void) -{ - move_data.position_x = asserv_status.position.x = 1500; - move_data.position_y = asserv_status.position.y = 1050; - - fsm_init (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_start); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_blocked); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_blocked); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_blocked); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_blocked); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_reached); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_blocked); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_reached); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_blocked); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_reached); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_reached); - move_print_test (&move_fsm); - - return 0; -} - -void -asserv_goto (uint32_t x, uint32_t y) -{ - printf ("x : %d\n", x); - printf ("y : %d\n", y); -} - -void -asserv_get_position (asserv_position_t *pos) -{ - pos->x = asserv_status.position.x; - pos->y = asserv_status.position.y; -} - diff --git a/digital/io/src/test/testgetsamples/Makefile b/digital/io/src/test/testgetsamples/Makefile deleted file mode 100644 index bbe76734..00000000 --- a/digital/io/src/test/testgetsamples/Makefile +++ /dev/null @@ -1,15 +0,0 @@ -BASE= ../../../../avr -HOST_PROGS = getsamples -IOBASE = ../.. -getsamples_SOURCES = main.c ../common.host.c $(IOBASE)/fsm.c $(IOBASE)/getsamples.c \ - $(IOBASE)/getsamples_cb.c $(IOBASE)/getsamples_fsm.c \ - $(IOBASE)/top.c $(IOBASE)/top_cb.c $(IOBASE)/top_fsm.c \ - $(IOBASE)/move.c $(IOBASE)/move_cb.c $(IOBASE)/move_fsm.c \ - $(IOBASE)/simu.host.c -MODULES = -CONFIGFILE = $(IOBASE)/avrconfig.h -# -O2 : speed -# -Os : size -OPTIMIZE = -O2 - -include $(BASE)/make/Makefile.gen diff --git a/digital/io/src/test/testgetsamples/main.c b/digital/io/src/test/testgetsamples/main.c deleted file mode 100644 index f083ec8d..00000000 --- a/digital/io/src/test/testgetsamples/main.c +++ /dev/null @@ -1,89 +0,0 @@ -/* main.c */ -/* {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - * - * }}} */ -#include "common.h" - -#include "../../fsm.h" -#include "../../getsamples.h" -#include "../../playground.h" - -#include "modules/utils/utils.h" - -#include "io.h" - -#include - -extern struct getsamples_data_t getsamples_data_; - -int -main (void) -{ - /* Configure the get sample FSM */ - /* Go to our distributor */ - int16_t approach_angle = PG_DISTRIBUTOR_SAMPLE_OUR_A; - uint8_t sample_bitfield = 0; - /* We want to put the sample into the 0, 2 and 4 box */ - sample_bitfield |= _BV(0); - sample_bitfield |= _BV(2); - sample_bitfield |= _BV(4); - - /* Print initial state */ - /* Configure and initialize the get sample FSM */ - getsamples_start (approach_angle, sample_bitfield); - /* Print first state */ - - /* We are facing the distributor */ - fsm_handle_event (&getsamples_fsm, - GETSAMPLES_EVENT_bot_move_succeed); - - /* We have open the input hole */ - fsm_handle_event (&getsamples_fsm, - GETSAMPLES_EVENT_arm_move_succeed); - - /* The bot is now in contact with to the distributor */ - fsm_handle_event (&getsamples_fsm, - GETSAMPLES_EVENT_bot_move_succeed); - - /* Get the sample one by one */ - do - { - fsm_handle_event (&getsamples_fsm, - GETSAMPLES_EVENT_arm_pass_noted_position); - } while (getsamples_data_.sample_bitfield); - - /* We need to do it one time again to make the bot move away from the - * distributor */ - fsm_handle_event (&getsamples_fsm, - GETSAMPLES_EVENT_arm_pass_noted_position); - - /* We go away from the gutter */ - fsm_handle_event (&getsamples_fsm, - GETSAMPLES_EVENT_bot_move_succeed); - - /* We close the input hole */ - fsm_handle_event (&getsamples_fsm, - GETSAMPLES_EVENT_arm_move_succeed); - - return 0; -} diff --git a/digital/io/src/top.c b/digital/io/src/top.c index c330067a..caf64de1 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -24,234 +24,9 @@ * }}} */ #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) -{ - /* 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; - else if (restart && current_position != 0) - current_position--; - /* 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); - } - 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 -#define DISTRIBUTOR_PROTECTION 360 - /* Table of distributor, first index is clean, second is front. */ - static const asserv_position_t distributor[DISTRIBUTOR_COUNT][2] = - { - /* First distributor. */ - { { 2711, 0 + DISTRIBUTOR_PROTECTION, 270 }, { 2711, DISTRIBUTOR_PROTECTION, 270 } }, - /* Second distributor. */ - { { PG_WIDTH - DISTRIBUTOR_PROTECTION, 600, 270 }, { PG_WIDTH - DISTRIBUTOR_PROTECTION, 800, 0 } }, - /* Third distributor. */ - { { PG_WIDTH - DISTRIBUTOR_PROTECTION, 1100, 270 }, { PG_WIDTH - DISTRIBUTOR_PROTECTION, 1300, 0 } }, - }; - /* Maximum number of remaining puck by distributor. */ - static uint8_t distributor_puck_count[DISTRIBUTOR_COUNT] = { 5, 5, 5 }; - /* Distributor cost: - * 7 for a distributor suposed to be not present. - * 7 for a empty distributor. - * 1 for a visited distributor. - * -1 for every puck in distributor. - */ - static int8_t distributor_cost[DISTRIBUTOR_COUNT]; - /* Current distributor. */ - static uint8_t current_position = DISTRIBUTOR_COUNT - 1; - /* 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 empty. */ - if (distributor_puck_count[current_position] == 0) - distributor_cost[current_position] += 7; - /* Supose the other one is not present. */ - if (current_position != 0) - distributor_cost[current_position == 1 ? 2 : 1] += 7; - } - /* Any time, add cost. */ - distributor_cost[current_position]++; - /* Update taken pucks. */ - previous_total_puck_taken = top_total_puck_taken; - /* Choose best distributor. */ - uint8_t best = 0; - uint8_t i; - for (i = 1; i < DISTRIBUTOR_COUNT; i++) - { - if (distributor_cost[i] - distributor_puck_count[i] < - distributor_cost[best] - distributor_puck_count[best]) - best = i; - } - /* Take the best one. */ - current_position = best; - - /* 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) -{ - 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++) - { - /* 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 = 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.fsm b/digital/io/src/top.fsm index 02fb854c..8203dfdb 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -7,42 +7,6 @@ States: waiting for the beginning of the top FSM. WAIT_INIT_TO_FINISH waiting for the end of init. - GET_PUCK_FROM_THE_GROUND [timeout=1575] - get some pucks from the ground. - GET_PUCK_FROM_DISTRIBUTOR [timeout=2250] - get some pucks from the distributor. - STOP_TO_GO_TO_UNLOAD_AREA - stop the move FSM before starting a new one. - STOP_TO_GET_PUCK_FROM_DISTRIBUTOR - stop the move FSM before starting a new one. - GO_TO_UNLOAD_AREA - when going to unload area. - FUCK_UNLOAD_AREA - when going backward in position for unload. - UNLOAD_PUCKS [timeout=1125] - when unloading all pucks from the bot. - ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD [timeout=1125] - wait for the elevator to be ready for a bot movement. - GO_AWAY_TO_RETRY_UNLOAD - when unload failed, go away from the unload area to try a new one. - TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD - try again to go away to retry unloading pucks. - FUCK_THE_DISTRIBUTOR - fuck the distributor. - WAIT_FOR_PUCKS [timeout=1575] - wait until no more pucks or already full. - GO_AWAY_FROM_UNLOAD_AREA - go away from the unload area to turn freely. - TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA - try again to go away from the unload area. - CLEAN_FRONT_OF_DISTRIBUTOR - clean the front of the distributor to have no problem to fuck it. - GO_AWAY_FROM_DISTRIBUTOR - to correctly leave a distributor, we need to do a linear move. - TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR - try again to go away from the distributor. - PREPARE_ELEVATOR [timeout=2250] - prepare the elevator for unload. Events: start @@ -63,183 +27,12 @@ Events: timeout in cycle count. asserv_last_cmd_ack last command sent to the asserv board has been acquitted. - bot_is_full_of_pucks - the bot has four pucks inside. - empty_distributor - the cylinder has tried to get some pucks but none has been seen. - elevator_order_done - the elevator has executed my order (go up, open door, close door). IDLE: start -> WAIT_INIT_TO_FINISH nothing to do. WAIT_INIT_TO_FINISH: - init_match_is_started -> GET_PUCK_FROM_THE_GROUND - the match start, try to get some puck from the ground. + init_match_is_started -> . + the match start, place something interresting here -GET_PUCK_FROM_THE_GROUND: - move_fsm_succeed: already_six_pucks_or_no_next_position_and_no_puck -> GET_PUCK_FROM_DISTRIBUTOR - close cylinder. - get the next distributor position and launch move FSM to go there. - move_fsm_succeed: already_six_pucks_or_no_next_position_and_some_puck -> GO_TO_UNLOAD_AREA - close cylinder. - get the next distributor position and launch move FSM to go there. - move_fsm_succeed: next_position_exists -> . - go to the next position using move FSM. - move_fsm_failed: some_pucks -> GO_TO_UNLOAD_AREA - close cylinder. - we have failed to do a move, unload pucks. - move_fsm_failed: no_puck -> GET_PUCK_FROM_DISTRIBUTOR - close cylinder. - get the next distributor position and launch move FSM to go there. - bot_is_full_of_pucks -> STOP_TO_GO_TO_UNLOAD_AREA - stop move FSM. - state_timeout: no_puck -> STOP_TO_GET_PUCK_FROM_DISTRIBUTOR - close cylinder. - too much time lost to get puck from the ground, stop move FSM. - state_timeout: some_pucks -> STOP_TO_GO_TO_UNLOAD_AREA - close cylinder. - too much time lost to get puck from the ground, stop move FSM. - -STOP_TO_GET_PUCK_FROM_DISTRIBUTOR: - move_fsm_stopped -> GET_PUCK_FROM_DISTRIBUTOR - get next distributor and go there. - -STOP_TO_GO_TO_UNLOAD_AREA: - move_fsm_stopped -> GO_TO_UNLOAD_AREA - compute an unload area. - ask move FSM to go there using backward move only. - move_fsm_succeed -> GO_TO_UNLOAD_AREA - do the same as bot_move_stopped. - bot_move_failed -> GO_TO_UNLOAD_AREA - do the same as bot_move_stopped. - -GO_TO_UNLOAD_AREA: - move_fsm_succeed -> PREPARE_ELEVATOR - prepare the elevator. -# It can be dangerous to loop here with no condition to cancel this loop. - move_fsm_failed -> . - compute a new unload area. - ask move FSM to go to the position using backward only. - -PREPARE_ELEVATOR: - elevator_order_done -> FUCK_UNLOAD_AREA - make the bot do a fuck the wall in backward until blocked. - state_timeout -> GO_TO_UNLOAD_AREA - compute a new unload area and retry. - -FUCK_UNLOAD_AREA: - bot_move_succeed -> UNLOAD_PUCKS - unload all the pucks from the elevator. - bot_move_failed: try_again -> GO_AWAY_TO_RETRY_UNLOAD - move backward from the unload area using linear move. - bot_move_failed: no_more_try_again -> UNLOAD_PUCKS - move backward from the unload area using linear move. - -GO_AWAY_TO_RETRY_UNLOAD: - bot_move_succeed -> GO_TO_UNLOAD_AREA - compute a new unload area. - ask move FSM to go there using backward move only. - bot_move_failed: niceness -> TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD - decrement niceness. - move forward using linear move. - bot_move_failed: no_more_niceness -> GO_TO_UNLOAD_AREA - do the same as move succeed. - -TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD: - bot_move_succeed -> GO_AWAY_TO_RETRY_UNLOAD - move backward from the unload area using linear move. - bot_move_failed -> GO_AWAY_TO_RETRY_UNLOAD - do the same as succeed. - -GET_PUCK_FROM_DISTRIBUTOR: - move_fsm_succeed -> CLEAN_FRONT_OF_DISTRIBUTOR - really go to the front of the distributor (it clean front area). - move_fsm_failed: no_puck_or_still_time -> . - get the next distributor position and launch move FSM to go there. - move_fsm_failed: some_pucks_and_no_more_time -> GO_TO_UNLOAD_AREA - compute an unload area. - ask move FSM to go there. - state_timeout -> . - get the next distributor position and launch move FSM to go there. - -CLEAN_FRONT_OF_DISTRIBUTOR: - move_fsm_succeed -> FUCK_THE_DISTRIBUTOR - we are in front of the distributor: fuck it. - move_fsm_failed -> GET_PUCK_FROM_DISTRIBUTOR - get a new distributor and go there. - -FUCK_THE_DISTRIBUTOR: - bot_move_succeed -> WAIT_FOR_PUCKS - let's cylinder FSM works until finished. - bot_move_failed: no_puck_or_still_time -> GET_PUCK_FROM_DISTRIBUTOR - get a new distributor and go there. - bot_move_failed: some_pucks_and_no_more_time -> GO_TO_UNLOAD_AREA - compute an unload area. - ask move FSM to go there. - -WAIT_FOR_PUCKS: - bot_is_full_of_pucks -> GO_AWAY_FROM_DISTRIBUTOR - remember unload area. - empty_distributor: no_puck_or_still_time -> GO_AWAY_FROM_DISTRIBUTOR - remember distributor. - empty_distributor: some_pucks_and_no_more_time -> GO_AWAY_FROM_DISTRIBUTOR - remember unload area. - state_timeout: no_puck_or_still_time -> GO_AWAY_FROM_DISTRIBUTOR - remember distributor. - state_timeout: some_pucks_and_no_more_time -> GO_AWAY_FROM_DISTRIBUTOR - remember unload area. - -GO_AWAY_FROM_DISTRIBUTOR: - bot_move_succeed: go_to_distributor -> GET_PUCK_FROM_DISTRIBUTOR - get a new distributor position and go there. - bot_move_succeed: go_to_unload -> GO_TO_UNLOAD_AREA - compute an unload area. - ask the move FSM to go there. - bot_move_failed: niceness -> TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR - decrement niceness. - move forward using linear move. - bot_move_failed: no_more_niceness_and_go_to_distributor -> GET_PUCK_FROM_DISTRIBUTOR - get a new distributor position and go there. - bot_move_failed: no_more_niceness_and_go_to_unload -> GO_TO_UNLOAD_AREA - compute an unload area. - ask the move FSM to go there. - -TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR: - bot_move_succeed -> GO_AWAY_FROM_DISTRIBUTOR - move backward using linear move. - bot_move_failed -> GO_AWAY_FROM_DISTRIBUTOR - do the same as succeed. - -UNLOAD_PUCKS: - elevator_order_done -> GO_AWAY_FROM_UNLOAD_AREA - linear move. - state_timeout -> ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD - we have failed, ask the elevator to put itself in a state where bot movement - will not damage something. - -GO_AWAY_FROM_UNLOAD_AREA: - bot_move_succeed: more_than_six_pucks_or_no_next_position -> GET_PUCK_FROM_DISTRIBUTOR - get the next distributor position and launch move FSM to go there. - bot_move_succeed: next_position_for_pucks_on_ground_exists -> GET_PUCK_FROM_THE_GROUND - go to the next position using move FSM. - bot_move_failed: niceness -> TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA - decrement niceness. - move forward using linear move. - bot_move_failed: no_more_niceness_and_more_than_six_pucks_or_no_next_position -> GET_PUCK_FROM_DISTRIBUTOR - go to the next position using move FSM. - bot_move_failed: no_more_niceness_and_next_position_for_pucks_on_ground_exists -> GET_PUCK_FROM_THE_GROUND - go to the next position using move FSM. - -TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA: - bot_move_succeed -> GO_AWAY_FROM_UNLOAD_AREA - move backward using linear move. - bot_move_failed -> GO_AWAY_FROM_UNLOAD_AREA - do the same as succeed. - -ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD: - elevator_order_done -> GO_AWAY_TO_RETRY_UNLOAD - move backward from the unload area using linear move. - state_timeout -> GO_AWAY_TO_RETRY_UNLOAD - do the same as elevator_ready. diff --git a/digital/io/src/top.h b/digital/io/src/top.h index 986035e8..71155342 100644 --- a/digital/io/src/top.h +++ b/digital/io/src/top.h @@ -25,50 +25,10 @@ * * }}} */ -#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. - * @param position the next position computed. - * @param restart set it to 1 when you restart a procedure. - * @return 0 if there is no more position. - */ -uint8_t -top_get_next_position_to_get_puck_on_the_ground (asserv_position_t *position, - uint8_t restart); - -/** - * Get the next position to get pucks from the distributor. - * @param clean_position the position to clean the distributor. - * @param front_position the position in front of the distributor to fuck it. - */ -void -top_get_next_position_to_get_distributor (asserv_position_t *clean_position, - asserv_position_t *front_position); - -/** - * Get the next position to get an unload position. - * @param position the next unloading position. - */ -void -top_get_next_position_to_unload_puck (asserv_position_t *position); - -/** - * Count number of pucks we have taken during a match. - */ -extern uint8_t top_total_puck_taken; - -/** - * Number of pucks inside the bot. - */ -extern uint8_t top_puck_inside_bot; - #endif /* top_h */ diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 35345858..bf24c7ee 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -1,7 +1,7 @@ /* top_cb.c */ /* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ * - * Copyright (C) 2009 Dufour Jérémy + * Copyright (C) 2010 Nicolas Schodet * * APBTeam: * Web: http://apbteam.org/ @@ -32,16 +32,6 @@ #include "chrono.h" #include "playground.h" #include "asserv.h" -#include "cylinder.h" -#include "elevator.h" - -/** - * Internal data. - */ -asserv_position_t front_position; -uint8_t remember_distributor = 0; -#define TOP_UNLOAD_RETRY_COUNT 1 -uint8_t unload_retry_count = TOP_UNLOAD_RETRY_COUNT; /** * Time limit. @@ -61,879 +51,12 @@ top__IDLE__start (void) /* * WAIT_INIT_TO_FINISH =init_match_is_started=> - * => GET_PUCK_FROM_THE_GROUND - * the match start, try to get some puck from the ground. + * => WAIT_INIT_TO_FINISH + * the match start, place something interresting here */ fsm_branch_t top__WAIT_INIT_TO_FINISH__init_match_is_started (void) { - /* Get next position. */ - asserv_position_t position; - top_get_next_position_to_get_puck_on_the_ground (&position, 0); - /* Go there. */ - move_start (position, 0); return top_next (WAIT_INIT_TO_FINISH, init_match_is_started); } -/* - * GET_PUCK_FROM_THE_GROUND =move_fsm_succeed=> - * already_six_pucks_or_no_next_position_and_no_puck => GET_PUCK_FROM_DISTRIBUTOR - * close cylinder. - * get the next distributor position and launch move FSM to go there. - * already_six_pucks_or_no_next_position_and_some_puck => GO_TO_UNLOAD_AREA - * close cylinder. - * get the next distributor position and launch move FSM to go there. - * next_position_exists => GET_PUCK_FROM_THE_GROUND - * go to the next position using move FSM. - */ -fsm_branch_t -top__GET_PUCK_FROM_THE_GROUND__move_fsm_succeed (void) -{ - /* Get next position. */ - asserv_position_t position; - if (!top_get_next_position_to_get_puck_on_the_ground (&position, 0) - || top_total_puck_taken >= 6) - { - /* Ensure cylinder is close. */ - cylinder_close_order = 1; - if (top_puck_inside_bot) - { - /* Flush cylinder. */ - cylinder_flush_order = 1; - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, already_six_pucks_or_no_next_position_and_some_puck); - } - else - { - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, already_six_pucks_or_no_next_position_and_no_puck); - } - } - else - { - /* Go there. */ - move_start (position, 0); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, next_position_exists); - } -} - -/* - * GET_PUCK_FROM_THE_GROUND =move_fsm_failed=> - * some_pucks => GO_TO_UNLOAD_AREA - * close cylinder. - * we have failed to do a move, unload pucks. - * no_puck => GET_PUCK_FROM_DISTRIBUTOR - * close cylinder. - * get the next distributor position and launch move FSM to go there. - */ -fsm_branch_t -top__GET_PUCK_FROM_THE_GROUND__move_fsm_failed (void) -{ - /* Ensure cylinder is close. */ - cylinder_close_order = 1; - asserv_position_t position; - if (top_puck_inside_bot) - { - /* Flush cylinder. */ - cylinder_flush_order = 1; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_failed, some_pucks); - } - else - { - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_failed, no_puck); - } -} - -/* - * GET_PUCK_FROM_THE_GROUND =bot_is_full_of_pucks=> - * => STOP_TO_GO_TO_UNLOAD_AREA - * stop move FSM. - */ -fsm_branch_t -top__GET_PUCK_FROM_THE_GROUND__bot_is_full_of_pucks (void) -{ - /* Ensure cylinder is close. */ - cylinder_close_order = 1; - /* Stop move FSM. */ - move_stop (); - return top_next (GET_PUCK_FROM_THE_GROUND, bot_is_full_of_pucks); -} - -/* - * GET_PUCK_FROM_THE_GROUND =state_timeout=> - * no_puck => STOP_TO_GET_PUCK_FROM_DISTRIBUTOR - * close cylinder. - * too much time lost to get puck from the ground, stop move FSM. - * some_pucks => STOP_TO_GO_TO_UNLOAD_AREA - * close cylinder. - * too much time lost to get puck from the ground, stop move FSM. - */ -fsm_branch_t -top__GET_PUCK_FROM_THE_GROUND__state_timeout (void) -{ - /* Ensure cylinder is close. */ - cylinder_close_order = 1; - if (top_puck_inside_bot) - { - /* Stop move FSM. */ - move_stop (); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, state_timeout, some_pucks); - } - else - { - /* Stop move FSM. */ - move_stop (); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, state_timeout, no_puck); - } -} - -/* - * GET_PUCK_FROM_DISTRIBUTOR =move_fsm_succeed=> - * => CLEAN_FRONT_OF_DISTRIBUTOR - * really go to the front of the distributor (it clean front area). - */ -fsm_branch_t -top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_succeed (void) -{ - /* Go in the front of the distributor. */ - move_start (front_position, ASSERV_REVERT_OK); - return top_next (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_succeed); -} - -/* - * GET_PUCK_FROM_DISTRIBUTOR =move_fsm_failed=> - * no_puck_or_still_time => GET_PUCK_FROM_DISTRIBUTOR - * get the next distributor position and launch move FSM to go there. - * some_pucks_and_no_more_time => GO_TO_UNLOAD_AREA - * compute an unload area. - * ask move FSM to go there. - */ -fsm_branch_t -top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_failed (void) -{ - if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT) - { - /* Close cylinder. */ - cylinder_close_order = 1; - /* Get next position. */ - asserv_position_t position; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, no_puck_or_still_time); - } - else - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Flush cylinder. */ - cylinder_flush_order = 1; - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, some_pucks_and_no_more_time); - } -} - -/* - * GET_PUCK_FROM_DISTRIBUTOR =state_timeout=> - * => GET_PUCK_FROM_DISTRIBUTOR - * get the next distributor position and launch move FSM to go there. - */ -fsm_branch_t -top__GET_PUCK_FROM_DISTRIBUTOR__state_timeout (void) -{ - /* Close cylinder. */ - cylinder_close_order = 1; - /* Get next position. */ - asserv_position_t position; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next (GET_PUCK_FROM_DISTRIBUTOR, state_timeout); -} - -/* - * STOP_TO_GO_TO_UNLOAD_AREA =move_fsm_stopped=> - * => GO_TO_UNLOAD_AREA - * compute an unload area. - * ask move FSM to go there using backward move only. - */ -fsm_branch_t -top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (void) -{ - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Flush cylinder. */ - cylinder_flush_order = 1; - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next (STOP_TO_GO_TO_UNLOAD_AREA, move_fsm_stopped); -} - -/* - * STOP_TO_GO_TO_UNLOAD_AREA =move_fsm_succeed=> - * => GO_TO_UNLOAD_AREA - * do the same as bot_move_stopped. - */ -fsm_branch_t -top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_succeed (void) -{ - /* Yerk. */ - top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (); - return top_next (STOP_TO_GO_TO_UNLOAD_AREA, move_fsm_succeed); -} - -/* - * STOP_TO_GO_TO_UNLOAD_AREA =bot_move_failed=> - * => GO_TO_UNLOAD_AREA - * do the same as bot_move_stopped. - */ -fsm_branch_t -top__STOP_TO_GO_TO_UNLOAD_AREA__bot_move_failed (void) -{ - /* Yerk. */ - top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (); - return top_next (STOP_TO_GO_TO_UNLOAD_AREA, bot_move_failed); -} - -/* - * STOP_TO_GET_PUCK_FROM_DISTRIBUTOR =move_fsm_stopped=> - * => GET_PUCK_FROM_DISTRIBUTOR - * get next distributor and go there. - */ -fsm_branch_t -top__STOP_TO_GET_PUCK_FROM_DISTRIBUTOR__move_fsm_stopped (void) -{ - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Get next position. */ - asserv_position_t position; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next (STOP_TO_GET_PUCK_FROM_DISTRIBUTOR, move_fsm_stopped); -} - -/* - * GO_TO_UNLOAD_AREA =move_fsm_succeed=> - * => PREPARE_ELEVATOR - * prepare the elevator. - */ -fsm_branch_t -top__GO_TO_UNLOAD_AREA__move_fsm_succeed (void) -{ - /* Prepare elevator. */ - elvt_prepare(3); - return top_next (GO_TO_UNLOAD_AREA, move_fsm_succeed); -} - -/* - * GO_TO_UNLOAD_AREA =move_fsm_failed=> - * => GO_TO_UNLOAD_AREA - * compute a new unload area. - * ask move FSM to go to the position using backward only. - */ -fsm_branch_t -top__GO_TO_UNLOAD_AREA__move_fsm_failed (void) -{ - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Flush cylinder. */ - cylinder_flush_order = 1; - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next (GO_TO_UNLOAD_AREA, move_fsm_failed); -} - -/* - * FUCK_UNLOAD_AREA =bot_move_succeed=> - * => UNLOAD_PUCKS - * unload all the pucks from the elevator. - */ -fsm_branch_t -top__FUCK_UNLOAD_AREA__bot_move_succeed (void) -{ - /* Unload elevator. */ - elvt_open(3); - return top_next (FUCK_UNLOAD_AREA, bot_move_succeed); -} - -/* - * FUCK_UNLOAD_AREA =bot_move_failed=> - * try_again => GO_AWAY_TO_RETRY_UNLOAD - * move backward from the unload area using linear move. - * no_more_try_again => UNLOAD_PUCKS - * move backward from the unload area using linear move. - */ -fsm_branch_t -top__FUCK_UNLOAD_AREA__bot_move_failed (void) -{ - if (unload_retry_count--) - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next_branch (FUCK_UNLOAD_AREA, bot_move_failed, try_again); - } - else - { - elvt_open_degraded (3); - return top_next_branch (FUCK_UNLOAD_AREA, bot_move_failed, no_more_try_again); - } -} - -/* - * UNLOAD_PUCKS =elevator_order_done=> - * => GO_AWAY_FROM_UNLOAD_AREA - * linear move. - */ -fsm_branch_t -top__UNLOAD_PUCKS__elevator_order_done (void) -{ - /* Close elevator. */ - elvt_close(); - /* Reset counter. */ - unload_retry_count = TOP_UNLOAD_RETRY_COUNT; - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next (UNLOAD_PUCKS, elevator_order_done); -} - -/* - * UNLOAD_PUCKS =state_timeout=> - * => ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD - * we have failed, ask the elevator to put itself in a state where bot movement - * will not damage something. - */ -fsm_branch_t -top__UNLOAD_PUCKS__state_timeout (void) -{ - /* Elevator close. */ - elvt_close(); - return top_next (UNLOAD_PUCKS, state_timeout); -} - -/* - * ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD =elevator_order_done=> - * => GO_AWAY_TO_RETRY_UNLOAD - * move backward from the unload area using linear move. - */ -fsm_branch_t -top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_order_done (void) -{ - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next (ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD, elevator_order_done); -} - -/* - * ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD =state_timeout=> - * => GO_AWAY_TO_RETRY_UNLOAD - * do the same as elevator_ready. - */ -fsm_branch_t -top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__state_timeout (void) -{ - /* Yerk. */ - top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_order_done (); - return top_next (ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD, state_timeout); -} - -/* - * GO_AWAY_TO_RETRY_UNLOAD =bot_move_succeed=> - * => GO_TO_UNLOAD_AREA - * compute a new unload area. - * ask move FSM to go there using backward move only. - */ -fsm_branch_t -top__GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void) -{ - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next (GO_AWAY_TO_RETRY_UNLOAD, bot_move_succeed); -} - -/* - * GO_AWAY_TO_RETRY_UNLOAD =bot_move_failed=> - * niceness => TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD - * decrement niceness. - * move forward using linear move. - * no_more_niceness => GO_TO_UNLOAD_AREA - * do the same as move succeed. - */ -fsm_branch_t -top__GO_AWAY_TO_RETRY_UNLOAD__bot_move_failed (void) -{ - /* TODO: niceness. */ - if (1) - { - /* Move backward. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next_branch (GO_AWAY_TO_RETRY_UNLOAD, bot_move_failed, niceness); - } - else - { - return top_next_branch (GO_AWAY_TO_RETRY_UNLOAD, bot_move_failed, no_more_niceness); - } -} - -/* - * TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD =bot_move_succeed=> - * => GO_AWAY_TO_RETRY_UNLOAD - * move backward from the unload area using linear move. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void) -{ - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next (TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD, bot_move_succeed); -} - -/* - * TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD =bot_move_failed=> - * => GO_AWAY_TO_RETRY_UNLOAD - * do the same as succeed. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_failed (void) -{ - /* Yerk. */ - top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (); - return top_next (TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD, bot_move_failed); -} - -/* - * FUCK_THE_DISTRIBUTOR =bot_move_succeed=> - * => WAIT_FOR_PUCKS - * let's cylinder FSM works until finished. - */ -fsm_branch_t -top__FUCK_THE_DISTRIBUTOR__bot_move_succeed (void) -{ - /* TODO: tell it to cylinder. */ - cylinder_distributor_fucked = 1; - return top_next (FUCK_THE_DISTRIBUTOR, bot_move_succeed); -} - -/* - * FUCK_THE_DISTRIBUTOR =bot_move_failed=> - * no_puck_or_still_time => GET_PUCK_FROM_DISTRIBUTOR - * get a new distributor and go there. - * some_pucks_and_no_more_time => GO_TO_UNLOAD_AREA - * compute an unload area. - * ask move FSM to go there. - */ -fsm_branch_t -top__FUCK_THE_DISTRIBUTOR__bot_move_failed (void) -{ - /* Close cylinder. */ - cylinder_close_order = 1; - if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT) - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Get next position. */ - asserv_position_t position; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (FUCK_THE_DISTRIBUTOR, bot_move_failed, - no_puck_or_still_time); - } - else - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - cylinder_flush_order = 1; - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (FUCK_THE_DISTRIBUTOR, bot_move_failed, - some_pucks_and_no_more_time); - } -} - -/* - * WAIT_FOR_PUCKS =bot_is_full_of_pucks=> - * => GO_AWAY_FROM_DISTRIBUTOR - * remember unload area. - */ -fsm_branch_t -top__WAIT_FOR_PUCKS__bot_is_full_of_pucks (void) -{ - remember_distributor = 0; - /* Backward move. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next (WAIT_FOR_PUCKS, bot_is_full_of_pucks); -} - -/* - * WAIT_FOR_PUCKS =empty_distributor=> - * no_puck_or_still_time => GO_AWAY_FROM_DISTRIBUTOR - * remember distributor. - * some_pucks_and_no_more_time => GO_AWAY_FROM_DISTRIBUTOR - * remember unload area. - */ -fsm_branch_t -top__WAIT_FOR_PUCKS__empty_distributor (void) -{ - /* Rest empty distributor. */ - cylinder_distributor_empty = 0; - if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT) - { - remember_distributor = 1; - /* Backward move. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, no_puck_or_still_time); - } - else - { - remember_distributor = 0; - /* Backward move. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, some_pucks_and_no_more_time); - } -} - -/* - * WAIT_FOR_PUCKS =state_timeout=> - * no_puck_or_still_time => GO_AWAY_FROM_DISTRIBUTOR - * remember distributor. - * some_pucks_and_no_more_time => GO_AWAY_FROM_DISTRIBUTOR - * remember unload area. - */ -fsm_branch_t -top__WAIT_FOR_PUCKS__state_timeout (void) -{ - if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT) - { - remember_distributor = 1; - /* Backward move. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next_branch (WAIT_FOR_PUCKS, state_timeout, no_puck_or_still_time); - } - else - { - remember_distributor = 0; - /* Backward move. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next_branch (WAIT_FOR_PUCKS, state_timeout, some_pucks_and_no_more_time); - } -} - -/* - * GO_AWAY_FROM_UNLOAD_AREA =bot_move_succeed=> - * more_than_six_pucks_or_no_next_position => GET_PUCK_FROM_DISTRIBUTOR - * get the next distributor position and launch move FSM to go there. - * next_position_for_pucks_on_ground_exists => GET_PUCK_FROM_THE_GROUND - * go to the next position using move FSM. - */ -fsm_branch_t -top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void) -{ - /* Get next position. */ - asserv_position_t position; - if (1 || !top_get_next_position_to_get_puck_on_the_ground (&position, 1) - || top_total_puck_taken >= 6) - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed, - more_than_six_pucks_or_no_next_position); - } - else - { - cylinder_close_order = 0; - /* Go there. */ - move_start (position, 0); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed, - next_position_for_pucks_on_ground_exists); - } -} - -/* - * GO_AWAY_FROM_UNLOAD_AREA =bot_move_failed=> - * niceness => TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA - * decrement niceness. - * move forward using linear move. - * no_more_niceness_and_more_than_six_pucks_or_no_next_position => GET_PUCK_FROM_DISTRIBUTOR - * go to the next position using move FSM. - * no_more_niceness_and_next_position_for_pucks_on_ground_exists => GET_PUCK_FROM_THE_GROUND - * go to the next position using move FSM. - */ -fsm_branch_t -top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_failed (void) -{ - /* TODO: niceness. */ - if (1) - { - /* Move backward. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed, niceness); - } - else - { - asserv_position_t position; - if (!top_get_next_position_to_get_puck_on_the_ground (&position, 1) - || top_total_puck_taken >= 6) - { - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed, - no_more_niceness_and_more_than_six_pucks_or_no_next_position); - } - else - { - cylinder_close_order = 0; - /* Go there. */ - move_start (position, 0); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed, - no_more_niceness_and_next_position_for_pucks_on_ground_exists); - } - } -} - -/* - * TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA =bot_move_succeed=> - * => GO_AWAY_FROM_UNLOAD_AREA - * move backward using linear move. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void) -{ - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next (TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed); -} - -/* - * TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA =bot_move_failed=> - * => GO_AWAY_FROM_UNLOAD_AREA - * do the same as succeed. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_failed (void) -{ - /* Yerk. */ - top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (); - return top_next (TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed); -} - -/* - * CLEAN_FRONT_OF_DISTRIBUTOR =move_fsm_succeed=> - * => FUCK_THE_DISTRIBUTOR - * we are in front of the distributor: fuck it. - */ -fsm_branch_t -top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_succeed (void) -{ - /* Open cylinder. */ - cylinder_close_order = 0; - /* Fuck the distributor. */ - asserv_go_to_distributor (); - return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_succeed); -} - -/* - * CLEAN_FRONT_OF_DISTRIBUTOR =move_fsm_failed=> - * => GET_PUCK_FROM_DISTRIBUTOR - * get a new distributor and go there. - */ -fsm_branch_t -top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_failed (void) -{ - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Get next position. */ - asserv_position_t position; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_failed); -} - -/* - * GO_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=> - * go_to_distributor => GET_PUCK_FROM_DISTRIBUTOR - * get a new distributor position and go there. - * go_to_unload => GO_TO_UNLOAD_AREA - * compute an unload area. - * ask the move FSM to go there. - */ -fsm_branch_t -top__GO_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) -{ - if (remember_distributor) - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Get next position. */ - asserv_position_t position; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_succeed, go_to_distributor); - } - else - { - - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - cylinder_flush_order = 1; - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_succeed, go_to_unload); - } -} - -/* - * GO_AWAY_FROM_DISTRIBUTOR =bot_move_failed=> - * niceness => TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR - * decrement niceness. - * move forward using linear move. - * no_more_niceness_and_go_to_distributor => GET_PUCK_FROM_DISTRIBUTOR - * get a new distributor position and go there. - * no_more_niceness_and_go_to_unload => GO_TO_UNLOAD_AREA - * compute an unload area. - * ask the move FSM to go there. - */ -fsm_branch_t -top__GO_AWAY_FROM_DISTRIBUTOR__bot_move_failed (void) -{ - /* TODO: niceness. */ - if (1) - { - /* Linear move. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_failed, niceness); - } - else - { - if (remember_distributor) - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Get next position. */ - asserv_position_t position; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_failed, no_more_niceness_and_go_to_distributor); - } - else - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - cylinder_flush_order = 1; - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_failed, no_more_niceness_and_go_to_unload); - } - } -} - -/* - * TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=> - * => GO_AWAY_FROM_DISTRIBUTOR - * move backward using linear move. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) -{ - /* Backward move. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next (TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR, bot_move_succeed); -} - -/* - * TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR =bot_move_failed=> - * => GO_AWAY_FROM_DISTRIBUTOR - * do the same as succeed. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR__bot_move_failed (void) -{ - /* Backward move. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next (TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR, bot_move_failed); -} - -/* - * PREPARE_ELEVATOR =elevator_order_done=> - * => FUCK_UNLOAD_AREA - * make the bot do a fuck the wall in backward until blocked. - */ -fsm_branch_t -top__PREPARE_ELEVATOR__elevator_order_done (void) -{ - /* Find the unload area. */ - asserv_go_to_the_wall (1); - return top_next (PREPARE_ELEVATOR, elevator_order_done); -} - -/* - * PREPARE_ELEVATOR =state_timeout=> - * => GO_TO_UNLOAD_AREA - * compute a new unload area and retry. - */ -fsm_branch_t -top__PREPARE_ELEVATOR__state_timeout (void) -{ - /* Get next position. */ - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next (PREPARE_ELEVATOR, state_timeout); -} diff --git a/digital/io/src/trace.trc b/digital/io/src/trace.trc index 5604e8e2..226c388b 100644 --- a/digital/io/src/trace.trc +++ b/digital/io/src/trace.trc @@ -7,4 +7,3 @@ EVENT fsm__handle_timeout fsm 1 "FSM[%c]: handle timeout." EVENT move__go_to xs 2 ys 2 as 2 xd 2 yd 2 ad 2 "Move: (%d, %d, %x) -> (%d, %d, %x)." EVENT move__obstacle x 2 y 2 "Move: obstacle a (%d, %d)." EVENT move__sharp s 2 s 2 s 2 s 2 s 2 "Sharps values: %x %x %x %x %x." -EVENT fsm__nbpucks s 1 s 1 s 1 s 1 s 1 "nb_pucks: top_all:%d top_inside:%d cylinder:%d filterbridge:%d elevator %d" diff --git a/digital/io/tools/test_simu.py b/digital/io/tools/test_simu.py new file mode 100644 index 00000000..081d8ec7 --- /dev/null +++ b/digital/io/tools/test_simu.py @@ -0,0 +1,124 @@ +# io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ +# +# Copyright (C) 2008 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT org +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# }}} +import math + +import mex.hub +import utils.forked + +import asserv +import asserv.init +import io +import io.init +from proto.popen_io import PopenIO + +import simu.model.table_eurobot2010 as table_model +import simu.view.table_eurobot2010 as table + +import simu.model.round_obstacle as obstacle_model +import simu.view.round_obstacle as obstacle_view + +import simu.robots.marcel.link.bag as robot_link +import simu.robots.marcel.model.bag as robot_model +import simu.robots.marcel.view.bag as robot_view + +from simu.inter.inter_node import InterNode +from Tkinter import * + +class TestSimu (InterNode): + """Interface, with simulated programs.""" + + robot_start_pos = { + False: (300, 2100 - 305, math.radians (-270)), + True: (3000 - 300, 2100 - 305, math.radians (-270)) + } + + def __init__ (self, asserv_cmd, io_cmd): + # Hub. + self.hub = mex.hub.Hub (min_clients = 2) + self.forked_hub = utils.forked.Forked (self.hub.wait) + # InterNode. + InterNode.__init__ (self) + def time (): + return self.node.date / self.node.tick + # Asserv. + self.asserv = asserv.Proto (PopenIO (asserv_cmd), time, + **asserv.init.host) + self.asserv.async = True + self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read) + # Io. + self.io = io.Proto (PopenIO (io_cmd), time, **io.init.host) + self.io.async = True + self.tk.createfilehandler (self.io, READABLE, self.io_read) + # Add table. + self.table_model = table_model.Table () + self.table = table.Table (self.table_view, self.table_model) + self.obstacle = obstacle_model.RoundObstacle (150) + self.table_model.obstacles.append (self.obstacle) + self.obstacle_view = obstacle_view.RoundObstacle (self.table, + self.obstacle) + self.table_view.bind ('<2>', self.place_obstacle) + # Add robot. + self.robot_link = robot_link.Bag (self.node) + self.robot_model = robot_model.Bag (self.node, self.table_model, + self.robot_link) + self.robot_view = robot_view.Bag (self.table, self.actuator_view, + self.sensor_frame, self.robot_model) + # Color switch. + self.robot_model.color_switch.register (self.change_color) + + def close (self): + self.forked_hub.kill () + import time + time.sleep (1) + self.asserv.close () + self.io.close () + + def asserv_read (self, file, mask): + self.asserv.proto.read () + self.asserv.proto.sync () + + def io_read (self, file, mask): + self.io.proto.read () + self.io.proto.sync () + + def step (self): + """Overide step to handle retransmissions, could be made cleaner using + simulated time.""" + InterNode.step (self) + self.asserv.proto.sync () + self.io.proto.sync () + + def change_color (self, *dummy): + i = self.robot_model.color_switch.state + self.asserv.set_simu_pos (*self.robot_start_pos[i]); + + def place_obstacle (self, ev): + pos = self.table_view.screen_coord ((ev.x, ev.y)) + self.obstacle.pos = pos + self.obstacle.notify () + +if __name__ == '__main__': + app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m9', 'marcel'), + ('../src/io.host')) + app.mainloop () diff --git a/digital/io/tools/test_simu_aquajim.py b/digital/io/tools/test_simu_aquajim.py deleted file mode 100644 index 44953a42..00000000 --- a/digital/io/tools/test_simu_aquajim.py +++ /dev/null @@ -1,126 +0,0 @@ -# io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ -# -# Copyright (C) 2008 Nicolas Schodet -# -# APBTeam: -# Web: http://apbteam.org/ -# Email: team AT apbteam DOT org -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. -# -# }}} -import math - -import mex.hub -import utils.forked - -import asserv -import asserv.init -import io -import io.init -from proto.popen_io import PopenIO - -import simu.model.table_eurobot2009 as table_model -import simu.view.table_eurobot2009 as table - -import simu.model.round_obstacle as obstacle_model -import simu.view.round_obstacle as obstacle_view - -import simu.robots.aquajim.link.bag as robot_link -import simu.robots.aquajim.model.bag as robot_model -import simu.robots.aquajim.view.bag as robot_view - -from simu.inter.inter_node import InterNode -from Tkinter import * - -class TestSimu (InterNode): - """Interface, with simulated programs.""" - - robot_start_pos = { - False: (300, 2100 - 305, math.radians (-270)), - True: (3000 - 300, 2100 - 305, math.radians (-270)) - } - - def __init__ (self, asserv_cmd, io_cmd): - # Hub. - self.hub = mex.hub.Hub (min_clients = 2) - self.forked_hub = utils.forked.Forked (self.hub.wait) - # InterNode. - InterNode.__init__ (self) - def time (): - return self.node.date / self.node.tick - # Asserv. - self.asserv = asserv.Proto (PopenIO (asserv_cmd), time, - **asserv.init.host) - self.asserv.async = True - self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read) - # Io. - self.io = io.Proto (PopenIO (io_cmd), time, **io.init.host) - self.io.async = True - self.tk.createfilehandler (self.io, READABLE, self.io_read) - # Add table. - self.table_model = table_model.Table () - self.table = table.Table (self.table_view, self.table_model) - self.obstacle = obstacle_model.RoundObstacle (150) - self.table_model.obstacles.append (self.obstacle) - self.obstacle_view = obstacle_view.RoundObstacle (self.table, - self.obstacle) - self.table_view.bind ('<2>', self.place_obstacle) - # Add robot. - self.robot_link = robot_link.Bag (self.node) - self.robot_model = robot_model.Bag (self.node, self.table_model, - self.robot_link) - self.robot_view = robot_view.Bag (self.table, self.actuator_view, - self.sensor_frame, self.robot_model) - # Unused ADC. - self.robot_link.io.adc[5].value = 0 - # Color switch. - self.robot_model.color_switch.register (self.change_color) - - def close (self): - self.forked_hub.kill () - import time - time.sleep (1) - self.asserv.close () - self.io.close () - - def asserv_read (self, file, mask): - self.asserv.proto.read () - self.asserv.proto.sync () - - def io_read (self, file, mask): - self.io.proto.read () - self.io.proto.sync () - - def step (self): - """Overide step to handle retransmissions, could be made cleaner using - simulated time.""" - InterNode.step (self) - self.asserv.proto.sync () - self.io.proto.sync () - - def change_color (self, *dummy): - i = self.robot_model.color_switch.state - self.asserv.set_simu_pos (*self.robot_start_pos[i]); - - def place_obstacle (self, ev): - pos = self.table_view.screen_coord ((ev.x, ev.y)) - self.obstacle.pos = pos - self.obstacle.notify () - -if __name__ == '__main__': - app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m9', 'aquajim'), - ('../src/io.host')) - app.mainloop () diff --git a/digital/io/tools/test_simu_control.py b/digital/io/tools/test_simu_control.py index a3c5fd49..b3123492 100644 --- a/digital/io/tools/test_simu_control.py +++ b/digital/io/tools/test_simu_control.py @@ -21,7 +21,7 @@ # Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. # # }}} -from test_simu_aquajim import TestSimu +from test_simu import TestSimu from Tkinter import * import math @@ -36,25 +36,6 @@ class TestSimuControl (TestSimu): self.control_frame = Frame (self) self.control_frame.pack (side = 'left', before = self.table_view, fill = 'y') - self.cylinder_button = Button (self.control_frame, text = 'Cylinder', - command = self.cylinder_command) - self.cylinder_button.pack () - self.door_var = IntVar () - self.door_button = Checkbutton (self.control_frame, text = 'Door', - indicatoron = False, - variable = self.door_var, command = self.door_command) - self.door_button.pack () - self.finger_var = IntVar () - self.finger_button = Checkbutton (self.control_frame, text = 'Finger', - indicatoron = False, - variable = self.finger_var, command = self.finger_command) - self.finger_button.pack () - self.elevator_door_var = IntVar () - self.elevator_door_button = Checkbutton (self.control_frame, - text = 'Elevator Door', indicatoron = False, - variable = self.elevator_door_var, - command = self.elevator_door_command) - self.elevator_door_button.pack () self.table_view.bind ('<1>', self.move) self.table_view.bind ('<3>', self.orient) @@ -69,25 +50,10 @@ class TestSimuControl (TestSimu): a = math.atan2 (y - robot_pos[1], x - robot_pos[0]) self.asserv.goto_angle (a) - def cylinder_command (self): - self.asserv.speed_pos ('a0', 5333 / 3) - - def door_command (self): - door_pos = self.door_var.get () - self.io.servo_pos (0, door_pos) - - def finger_command (self): - finger_pos = self.finger_var.get () - self.io.servo_pos (1, finger_pos) - - def elevator_door_command (self): - elevator_door_pos = self.elevator_door_var.get () - self.io.pwm_set (elevator_door_pos == 1 and 512 or -512, 225) - def change_color (self, *dummy): pass if __name__ == '__main__': app = TestSimuControl (('../../asserv/src/asserv/asserv.host', '-m9', - 'aquajim'), ('../src/io.host')) + 'marcel'), ('../src/io.host')) app.mainloop () diff --git a/digital/io/tools/test_simu_giboulee.py b/digital/io/tools/test_simu_giboulee.py deleted file mode 100644 index eacaccf8..00000000 --- a/digital/io/tools/test_simu_giboulee.py +++ /dev/null @@ -1,124 +0,0 @@ -# io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ -# -# Copyright (C) 2008 Nicolas Schodet -# -# APBTeam: -# Web: http://apbteam.org/ -# Email: team AT apbteam DOT org -# -# This program is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2 of the License, or -# (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with this program; if not, write to the Free Software -# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. -# -# }}} -import math - -import mex.hub -import utils.forked - -import asserv -import asserv.init -import io -import io.init -from proto.popen_io import PopenIO - -import simu.model.table as table_model -import simu.view.table_eurobot2008 as table - -import simu.model.round_obstacle as obstacle_model -import simu.view.round_obstacle as obstacle_view - -import simu.robots.giboulee.link.bag as robot_link -import simu.robots.giboulee.model.bag as robot_model -import simu.robots.giboulee.view.bag as robot_view - -from simu.inter.inter_node import InterNode -from Tkinter import * - -class TestSimu (InterNode): - """Interface, with simulated programs.""" - - robot_start_pos = { - False: (200, 2100 - 70, math.radians (-90)), - True: (3000 - 200, 2100 - 70, math.radians (-90)) - } - - def __init__ (self, asserv_cmd, io_cmd): - # Hub. - self.hub = mex.hub.Hub (min_clients = 2) - self.forked_hub = utils.forked.Forked (self.hub.wait) - # InterNode. - InterNode.__init__ (self) - def time (): - return self.node.date / self.node.tick - # Asserv. - self.asserv = asserv.Proto (PopenIO (asserv_cmd), time, - **asserv.init.host) - self.asserv.async = True - self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read) - # Io. - self.io = io.Proto (PopenIO (io_cmd), time, **io.init.host) - self.io.async = True - self.tk.createfilehandler (self.io, READABLE, self.io_read) - # Add table. - self.table_model = table_model.Table () - self.table = table.Table (self.table_view, self.table_model) - self.obstacle = obstacle_model.RoundObstacle (150) - self.table_model.obstacles.append (self.obstacle) - self.obstacle_view = obstacle_view.RoundObstacle (self.table, - self.obstacle) - self.table_view.bind ('<2>', self.place_obstacle) - # Add robot. - self.robot_link = robot_link.Bag (self.node) - self.robot_model = robot_model.Bag (self.node, self.table_model, - self.robot_link) - self.robot_view = robot_view.Bag (self.table, self.actuator_view, - self.sensor_frame, self.robot_model) - # Color switch. - self.robot_model.color_switch.register (self.change_color) - - def close (self): - self.forked_hub.kill () - import time - time.sleep (1) - self.asserv.close () - self.io.close () - - def asserv_read (self, file, mask): - self.asserv.proto.read () - self.asserv.proto.sync () - - def io_read (self, file, mask): - self.io.proto.read () - self.io.proto.sync () - - def step (self): - """Overide step to handle retransmissions, could be made cleaner using - simulated time.""" - InterNode.step (self) - self.asserv.proto.sync () - self.io.proto.sync () - - def change_color (self, *dummy): - i = self.robot_model.color_switch.state - self.asserv.set_simu_pos (*self.robot_start_pos[i]); - - def place_obstacle (self, ev): - pos = self.table_view.screen_coord ((ev.x, ev.y)) - self.obstacle.pos = pos - self.obstacle.notify () - -if __name__ == '__main__': - app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m', 'giboulee'), - ('../src/io.host')) - app.mainloop () -- cgit v1.2.3