summaryrefslogtreecommitdiffhomepage
path: root/digital/io
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/doc/Makefile11
-rw-r--r--digital/io/doc/getSamplesFsm.dot11
-rw-r--r--digital/io/doc/loadBall.dot8
-rw-r--r--digital/io/doc/mainFsm.dot16
-rw-r--r--digital/io/doc/move.dot19
-rw-r--r--digital/io/doc/proto_asserv.txt36
-rw-r--r--digital/io/doc/stdContenerFsm.dot10
-rw-r--r--digital/io/src/Makefile2
-rw-r--r--digital/io/src/asserv.c157
-rw-r--r--digital/io/src/asserv.h92
-rw-r--r--digital/io/src/bot.h (renamed from digital/io/src/aquajim.h)52
-rw-r--r--digital/io/src/chrono.c2
-rw-r--r--digital/io/src/contact.c5
-rw-r--r--digital/io/src/contact.h6
-rw-r--r--digital/io/src/cylinder.c47
-rw-r--r--digital/io/src/cylinder.fsm170
-rw-r--r--digital/io/src/cylinder.h56
-rw-r--r--digital/io/src/cylinder_cb.c441
-rw-r--r--digital/io/src/eeprom_dump.txt4
-rw-r--r--digital/io/src/elevator.c119
-rw-r--r--digital/io/src/elevator.fsm125
-rw-r--r--digital/io/src/elevator.h91
-rw-r--r--digital/io/src/elevator_cb.c320
-rw-r--r--digital/io/src/filterbridge.c31
-rw-r--r--digital/io/src/filterbridge.fsm74
-rw-r--r--digital/io/src/filterbridge.h32
-rw-r--r--digital/io/src/filterbridge_cb.c149
-rw-r--r--digital/io/src/fsm.h3
-rw-r--r--digital/io/src/giboulee.h98
-rw-r--r--digital/io/src/init_cb.c9
-rw-r--r--digital/io/src/main.c204
-rw-r--r--digital/io/src/main.h14
-rw-r--r--digital/io/src/move_cb.c5
-rw-r--r--digital/io/src/playground.h49
-rw-r--r--digital/io/src/servo.avr.c6
-rw-r--r--digital/io/src/switch.h2
-rw-r--r--digital/io/src/test/common.host.c154
-rw-r--r--digital/io/src/test/gutter/Makefile17
-rw-r--r--digital/io/src/test/gutter/main.c49
-rw-r--r--digital/io/src/test/move/Makefile13
-rw-r--r--digital/io/src/test/move/main.c135
-rw-r--r--digital/io/src/test/testgetsamples/Makefile15
-rw-r--r--digital/io/src/test/testgetsamples/main.c89
-rw-r--r--digital/io/src/top.c225
-rw-r--r--digital/io/src/top.fsm211
-rw-r--r--digital/io/src/top.h40
-rw-r--r--digital/io/src/top_cb.c883
-rw-r--r--digital/io/src/trace.trc1
-rw-r--r--digital/io/tools/test_simu.py (renamed from digital/io/tools/test_simu_aquajim.py)14
-rw-r--r--digital/io/tools/test_simu_control.py38
-rw-r--r--digital/io/tools/test_simu_giboulee.py124
51 files changed, 176 insertions, 4308 deletions
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/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;
/**
@@ -133,11 +133,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/aquajim.h b/digital/io/src/bot.h
index 4468ac73..aff6ada0 100644
--- a/digital/io/src/aquajim.h
+++ b/digital/io/src/bot.h
@@ -1,9 +1,9 @@
-#ifndef aquajim_h
-#define aquajim_h
-/* aquajim.h */
+#ifndef bot_h
+#define bot_h
+/* bot.h */
/* 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/
@@ -26,7 +26,7 @@
* }}} */
/**
- * Some defines for the eurobot 2009 bot of APBTeam: Aqua Jim.
+ * Specific defines about the robot dimensions and competion rules.
*/
/**
@@ -35,21 +35,19 @@
#define MATCH_DURATION_MS 90000
/**
- * filterbridge servo
+ * How to compute a angle for the robot?
+ * One degree is 65536 / 360
*/
-
-#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
+#define BOT_ANGLE_DEGREE (65536 / 360)
/**
- * How to compute a angle for giboulee?
- * One degree is 65536 / 360
+ * The scaling factor, millimeter per step.
*/
-#define BOT_ANGLE_DEGREE (65536 / 360)
+#ifdef HOST
+# define BOT_SCALE 0.0395840674352314
+#else
+# define BOT_SCALE 0.0413530725332892
+#endif
/**
* The size of the bot.
@@ -57,4 +55,24 @@
#define BOT_LENGTH 300
#define BOT_WIDTH 310
-#endif /* aquajim_h */
+/**
+ * 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
@@ -49,23 +49,9 @@ 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.
@@ -73,48 +73,6 @@
#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.
*/
#define PG_MOVE_DISTANCE 300
@@ -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 <stdio.h>
-
-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 <stdio.h>
-
-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 <stdio.h>
-
-/**
- * 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 <stdio.h>
-
-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 <math.h>
-
-/* Reset to 0. */
-uint8_t top_total_puck_taken = 0;
-uint8_t top_puck_inside_bot = 0;
-
-/**
- * Number of unload position.
- */
-#define TOP_UNLOAD_POSITION_COUNT 8
-
-#define TOP_UNLOAD_DISTANCE_TO_CENTER (PG_LENGTH / 2 - 600)
-
-/**
- * Structure of a unload position with costs.
- */
-typedef struct top_unload_position_t
-{
- move_position_t position;
- uint8_t cost;
-} top_unload_position_t;
-
-top_unload_position_t top_unload_position[TOP_UNLOAD_POSITION_COUNT];
void
top_init (void)
{
- uint8_t i, cost;
- uint32_t angle;
- for (i = 0; i < TOP_UNLOAD_POSITION_COUNT; i++)
- {
- /* Compute predefined positions. */
- angle = 0x1000000 * i / TOP_UNLOAD_POSITION_COUNT;
- top_unload_position[i].position.x = PG_WIDTH / 2
- + fixed_mul_f824 (TOP_UNLOAD_DISTANCE_TO_CENTER,
- fixed_cos_f824 (angle));
- top_unload_position[i].position.y = PG_LENGTH / 2
- + fixed_mul_f824 (TOP_UNLOAD_DISTANCE_TO_CENTER,
- fixed_sin_f824 (angle));
- top_unload_position[i].position.a = angle >> 8;
- /* Initialize costs. */
- if (i < TOP_UNLOAD_POSITION_COUNT / 4
- || i > TOP_UNLOAD_POSITION_COUNT * 3 / 4)
- cost = bot_color ? 6 : 0;
- else if (i > TOP_UNLOAD_POSITION_COUNT / 4 && i <
- TOP_UNLOAD_POSITION_COUNT * 3 / 4)
- cost = bot_color ? 0 : 6;
- else
- cost = 0;
- top_unload_position[i].cost = cost;
- }
}
-
-uint8_t
-top_get_next_position_to_get_puck_on_the_ground (asserv_position_t *position,
- uint8_t restart)
-{
- /* 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 (&current_position);
- pos = (uint8_t) (atan2 (PG_LENGTH / 2 - (int16_t) current_position.y,
- PG_WIDTH / 2 - (int16_t) current_position.x)
- * (1.0 / (2 * M_PI)) * TOP_UNLOAD_DISTANCE_TO_CENTER
- + 1.0 / (TOP_UNLOAD_POSITION_COUNT * 2))
- % TOP_UNLOAD_POSITION_COUNT;
- /* Compute dynamic cost. Nearest position costs 0, near half circle costs
- * 1, far half circle costs 2. */
- for (i = 0; i < TOP_UNLOAD_POSITION_COUNT; i++)
- {
- /* 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_aquajim.py b/digital/io/tools/test_simu.py
index 44953a42..081d8ec7 100644
--- a/digital/io/tools/test_simu_aquajim.py
+++ b/digital/io/tools/test_simu.py
@@ -32,15 +32,15 @@ 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.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.aquajim.link.bag as robot_link
-import simu.robots.aquajim.model.bag as robot_model
-import simu.robots.aquajim.view.bag as robot_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 *
@@ -84,8 +84,6 @@ class TestSimu (InterNode):
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)
@@ -121,6 +119,6 @@ class TestSimu (InterNode):
self.obstacle.notify ()
if __name__ == '__main__':
- app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m9', 'aquajim'),
+ app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m9', 'marcel'),
('../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 ()