summaryrefslogtreecommitdiff
path: root/digital/ai
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-02 18:52:14 +0200
committerNicolas Schodet2011-05-02 23:00:49 +0200
commit8a6cac0a41be34d36122ab5271295b0ee83fddac (patch)
treeca779a03b2264f6777b8a10dc01aa84698496c29 /digital/ai
parent4909f63bea2598489a5895f9bbe1e2622093caa1 (diff)
digital/{ai,io}: move TWI master files to ai directory
Diffstat (limited to 'digital/ai')
-rw-r--r--digital/ai/src/twi_master/asserv.c449
-rw-r--r--digital/ai/src/twi_master/asserv.h297
-rw-r--r--digital/ai/src/twi_master/mimot.c183
-rw-r--r--digital/ai/src/twi_master/mimot.h95
-rw-r--r--digital/ai/src/twi_master/twi_master.c219
-rw-r--r--digital/ai/src/twi_master/twi_master.h68
6 files changed, 1311 insertions, 0 deletions
diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c
new file mode 100644
index 00000000..d066737b
--- /dev/null
+++ b/digital/ai/src/twi_master/asserv.c
@@ -0,0 +1,449 @@
+/* asserv.c */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "asserv.h"
+
+#include "twi_master.h"
+
+#include "modules/utils/byte.h"
+#include "modules/math/fixed/fixed.h"
+#include "bot.h"
+#include "io.h"
+
+/**
+ * Flag bit position value for the status byte of the asserv.
+ */
+enum asserv_status_flag_e
+{
+ /** Bot movement finished with success. */
+ asserv_status_flag_move_succeed = 0,
+ /** Bot movement finished with failure: the bot is blocked. */
+ asserv_status_flag_move_failed = 1,
+ /** Bot is moving forward (linear speed greater than 0). */
+ asserv_status_flag_move_forward = 2,
+ /** Bot is moving backward (linear speed smaller than 0). */
+ asserv_status_flag_move_backward = 3,
+ /** Motor0 movement finished with success. */
+ asserv_status_flag_motor0_succeed = 4,
+ /** Motor0 movement finished with failure. */
+ asserv_status_flag_motor0_failed = 5,
+ /** Motor1 movement finished with success. */
+ asserv_status_flag_motor1_succeed = 6,
+ /** Motor1 movement finished with failure. */
+ asserv_status_flag_motor1_failed = 7,
+};
+typedef enum asserv_status_flag_e asserv_status_flag_e;
+
+/** Scaling factor. */
+static uint32_t asserv_scale;
+
+/** Scaling factor inverse. */
+static uint32_t asserv_scale_inv;
+
+/** Last moving direction. */
+static uint8_t asserv_last_moving_direction;
+
+/**
+ * Structure for storing a position for the bot using asserv units.
+ */
+typedef struct asserv_position_t
+{
+ /** X position. */
+ uint32_t x;
+ /** Y position. */
+ uint32_t y;
+ /** Angle. */
+ uint16_t a;
+} asserv_position_t;
+
+/**
+ * Status structure maintains by the update command.
+ */
+typedef struct asserv_struct_s
+{
+ /** Status flags. */
+ uint8_t status;
+ /** Asserv board input port. */
+ uint8_t input_port;
+ /** Bot position. */
+ asserv_position_t position;
+ /** Motor0 position. */
+ uint16_t motor0_position;
+ /** Motor1 position. */
+ uint16_t motor1_position;
+} asserv_struct_s;
+
+/**
+ * Status variable.
+ */
+asserv_struct_s asserv_status;
+
+/** Set scale.
+ * @param scale number of millimeter per step (f8.24).
+ */
+static void
+asserv_set_scale (uint32_t scale)
+{
+ asserv_scale = scale;
+ asserv_scale_inv = fixed_div_f824 (1L << 24, scale);
+}
+
+void
+asserv_init (void)
+{
+ asserv_set_scale (BOT_SCALE * (1L << 24));
+}
+
+void
+asserv_status_cb (uint8_t *status)
+{
+ /* Parse received data and store them. */
+ asserv_status.status = status[0];
+ asserv_status.input_port = status[1];
+ asserv_status.position.x = v8_to_v32 (0, status[3], status[4], status[5]);
+ asserv_status.position.y = v8_to_v32 (0, status[6], status[7], status[8]);
+ asserv_status.position.a = v8_to_v16 (status[9], status[10]);
+ asserv_status.motor0_position = v8_to_v16 (status[11], status[12]);
+ asserv_status.motor1_position = v8_to_v16 (status[13], status[14]);
+ /* Update moving direction. */
+ if (asserv_get_moving_direction () != 0)
+ asserv_last_moving_direction = asserv_get_moving_direction ();
+}
+
+asserv_status_e
+asserv_move_cmd_status (void)
+{
+ /* Check Motor Finished flag */
+ if (asserv_status.status & _BV (asserv_status_flag_move_succeed))
+ return success;
+ /* Check Motor Blocked flag */
+ else if (asserv_status.status & _BV (asserv_status_flag_move_failed))
+ return failure;
+ /* Otherwise, not finished nor failure */
+ return none;
+}
+
+asserv_status_e
+asserv_motor0_cmd_status (void)
+{
+ /* Check Motor0 Finished flag */
+ if (asserv_status.status & _BV (asserv_status_flag_motor0_succeed))
+ return success;
+ /* Check Motor0 Blocked flag */
+ else if (asserv_status.status & _BV (asserv_status_flag_motor0_failed))
+ return failure;
+ /* Otherwise, not finished nor failure */
+ return none;
+}
+
+asserv_status_e
+asserv_motor1_cmd_status (void)
+{
+ /* Check Motor1 Finished flag */
+ if (asserv_status.status & _BV (asserv_status_flag_motor1_succeed))
+ return success;
+ /* Check Motor1 Blocked flag */
+ else if (asserv_status.status & _BV (asserv_status_flag_motor1_failed))
+ return failure;
+ /* Otherwise, not finished nor failure */
+ return none;
+}
+
+void
+asserv_get_position (position_t *current_position)
+{
+ assert (current_position);
+ /* Copy last received status buffer information to current position */
+ current_position->v.x = fixed_mul_f824 (asserv_status.position.x,
+ asserv_scale);
+ current_position->v.y = fixed_mul_f824 (asserv_status.position.y,
+ asserv_scale);
+ current_position->a = asserv_status.position.a;
+}
+
+uint16_t
+asserv_get_motor0_position (void)
+{
+ /* Return the position of the motor0 of the current status buffer */
+ return asserv_status.motor0_position;
+}
+
+uint16_t
+asserv_get_motor1_position (void)
+{
+ /* Return the position of the motor1 of the current status buffer */
+ return asserv_status.motor1_position;
+}
+
+uint8_t
+asserv_get_moving_direction (void)
+{
+ /* Foward move? */
+ if (asserv_status.status & _BV (asserv_status_flag_move_forward))
+ return 1;
+ /* Backward move? */
+ if (asserv_status.status & _BV (asserv_status_flag_move_backward))
+ return 2;
+ /* Not moving */
+ return 0;
+}
+
+uint8_t
+asserv_get_last_moving_direction (void)
+{
+ return asserv_last_moving_direction;
+}
+
+void
+asserv_reset (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'z';
+ twi_master_send_buffer (1);
+}
+
+void
+asserv_free_motor (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'w';
+ twi_master_send_buffer (1);
+}
+
+void
+asserv_stop_motor (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 's';
+ twi_master_send_buffer (1);
+}
+
+void
+asserv_move_linearly (int32_t distance)
+{
+ distance = fixed_mul_f824 (distance, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'l';
+ buffer[1] = v32_to_v8 (distance, 2);
+ buffer[2] = v32_to_v8 (distance, 1);
+ buffer[3] = v32_to_v8 (distance, 0);
+ twi_master_send_buffer (4);
+}
+
+void
+asserv_move_angularly (int16_t angle)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'a';
+ buffer[1] = v16_to_v8 (angle, 1);
+ buffer[2] = v16_to_v8 (angle, 0);
+ twi_master_send_buffer (3);
+}
+
+void
+asserv_goto_angle (int16_t angle)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'y';
+ buffer[1] = v16_to_v8 (angle, 1);
+ buffer[2] = v16_to_v8 (angle, 0);
+ twi_master_send_buffer (3);
+}
+
+void
+asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward)
+{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ y = fixed_mul_f824 (y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'X';
+ buffer[1] = v32_to_v8 (x, 2);
+ buffer[2] = v32_to_v8 (x, 1);
+ buffer[3] = v32_to_v8 (x, 0);
+ buffer[4] = v32_to_v8 (y, 2);
+ buffer[5] = v32_to_v8 (y, 1);
+ buffer[6] = v32_to_v8 (y, 0);
+ buffer[7] = v16_to_v8 (a, 1);
+ buffer[8] = v16_to_v8 (a, 0);
+ buffer[9] = backward;
+ twi_master_send_buffer (10);
+}
+
+void
+asserv_go_to_the_wall (uint8_t backward)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'f';
+ buffer[1] = backward;
+ twi_master_send_buffer (2);
+}
+
+void
+asserv_move_motor0_absolute (uint16_t position, uint8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'b';
+ buffer[1] = v16_to_v8 (position, 1);
+ buffer[2] = v16_to_v8 (position, 0);
+ buffer[3] = speed;
+ twi_master_send_buffer (4);
+}
+
+void
+asserv_move_motor1_absolute (uint16_t position, uint8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'c';
+ buffer[1] = v16_to_v8 (position, 1);
+ buffer[2] = v16_to_v8 (position, 0);
+ buffer[3] = speed;
+ twi_master_send_buffer (4);
+}
+
+void
+asserv_set_x_position (int32_t x)
+{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'X';
+ buffer[2] = v32_to_v8 (x, 2);
+ buffer[3] = v32_to_v8 (x, 1);
+ buffer[4] = v32_to_v8 (x, 0);
+ twi_master_send_buffer (5);
+}
+
+void
+asserv_set_y_position (int32_t y)
+{
+ y = fixed_mul_f824 (y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'Y';
+ buffer[2] = v32_to_v8 (y, 2);
+ buffer[3] = v32_to_v8 (y, 1);
+ buffer[4] = v32_to_v8 (y, 0);
+ twi_master_send_buffer (5);
+}
+
+void
+asserv_set_angle_position (int16_t angle)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'A';
+ buffer[2] = v32_to_v8 (angle, 1);
+ buffer[3] = v32_to_v8 (angle, 0);
+ twi_master_send_buffer (4);
+}
+
+void
+asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
+ uint8_t linear_low, uint8_t angular_low)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 's';
+ buffer[2] = linear_high;
+ buffer[3] = angular_high;
+ buffer[4] = linear_low;
+ buffer[5] = angular_low;
+ twi_master_send_buffer (6);
+}
+
+void
+asserv_set_position (int32_t x, int32_t y, int16_t angle)
+{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ y = fixed_mul_f824 (y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'X';
+ buffer[2] = v32_to_v8 (x, 2);
+ buffer[3] = v32_to_v8 (x, 1);
+ buffer[4] = v32_to_v8 (x, 0);
+ buffer[5] = 'Y';
+ buffer[6] = v32_to_v8 (y, 2);
+ buffer[7] = v32_to_v8 (y, 1);
+ buffer[8] = v32_to_v8 (y, 0);
+ buffer[9] = 'A';
+ buffer[10] = v32_to_v8 (angle, 1);
+ buffer[11] = v32_to_v8 (angle, 0);
+ twi_master_send_buffer (12);
+}
+
+void
+asserv_goto (uint32_t x, uint32_t y, uint8_t backward)
+{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ y = fixed_mul_f824 (y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'x';
+ buffer[1] = v32_to_v8 (x, 2);
+ buffer[2] = v32_to_v8 (x, 1);
+ buffer[3] = v32_to_v8 (x, 0);
+ buffer[4] = v32_to_v8 (y, 2);
+ buffer[5] = v32_to_v8 (y, 1);
+ buffer[6] = v32_to_v8 (y, 0);
+ buffer[7] = backward;
+ twi_master_send_buffer (8);
+}
+
+void
+asserv_motor0_zero_position (int8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'B';
+ buffer[1] = speed;
+ twi_master_send_buffer (2);
+}
+
+void
+asserv_motor1_zero_position (int8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'C';
+ buffer[1] = speed;
+ twi_master_send_buffer (2);
+}
+
+void
+asserv_motor0_free (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'r';
+ buffer[1] = 0;
+ twi_master_send_buffer (2);
+}
+
+void
+asserv_motor1_free (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'r';
+ buffer[1] = 1;
+ twi_master_send_buffer (2);
+}
+
diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h
new file mode 100644
index 00000000..0d29ba96
--- /dev/null
+++ b/digital/ai/src/twi_master/asserv.h
@@ -0,0 +1,297 @@
+#ifndef asserv_h
+#define asserv_h
+/* asserv.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * 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 "defs.h"
+
+/**
+ * @file Control the asserv board using the TWI protocol.
+ * This files contains the 'public' functions to send commands to the asserv
+ * board using a protocol over TWI communication.
+ */
+
+/** Slave number in twi_master list. */
+#define ASSERV_SLAVE 0
+
+/** Asserv TWI address. */
+#define ASSERV_TWI_ADDRESS 4
+
+/** Length of the status buffer (not including CRC). */
+#define ASSERV_STATUS_LENGTH 15
+
+/** Use backward movements. */
+#define ASSERV_BACKWARD 1
+/** Authorise reverse the requested movement direction, may be or'ed with the
+ * previous define. */
+#define ASSERV_REVERT_OK 2
+
+/** Initialize the asserv control module. */
+void
+asserv_init (void);
+
+/** Called when a new status buffer is received, update the asserv
+ * information. */
+void
+asserv_status_cb (uint8_t *status);
+
+/**
+ * Status of a move or motor class command.
+ * It is return by status functions.
+ */
+typedef enum asserv_status_e
+{
+ /** No status is available. The command is not finished yet. */
+ none,
+ /** The command has succeed. */
+ success,
+ /** The command has failed. The bot or the motor is blocked */
+ failure
+} asserv_status_e;
+
+/**
+ * Is last move class command has successfully ended?
+ * This function is used to know the status of the last move command. It looks
+ * at the status register.
+ * @return the status of the last move class command.
+ */
+asserv_status_e
+asserv_move_cmd_status (void);
+
+/**
+ * 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_motor0_cmd_status (void);
+
+/**
+ * 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_motor1_cmd_status (void);
+
+/**
+ * Get the current position of the bot.
+ * @param current_position the current position to update.
+ */
+void
+asserv_get_position (position_t *current_position);
+
+/**
+ * Get the motor0 position.
+ * @return the position of the motor0 (in steps).
+ */
+uint16_t
+asserv_get_motor0_position (void);
+
+/**
+ * Get the motor1 position.
+ * @return the position of the motor1 (in steps).
+ */
+uint16_t
+asserv_get_motor1_position (void);
+
+/**
+ * Are we moving forward/backward?
+ * @return
+ * - 0 we are not moving;
+ * - 1 we are moving forward;
+ * - 2 we are moving backward.
+ */
+uint8_t
+asserv_get_moving_direction (void);
+
+/**
+ * Get the last moving direction of the bot.
+ * @return 1 is forward, 2 is backward.
+ */
+uint8_t
+asserv_get_last_moving_direction (void);
+
+/**
+ * Reset the asserv board.
+ * Other class command.
+ */
+void
+asserv_reset (void);
+
+/**
+ * Free the motors (stop controlling them).
+ * Other class command.
+ */
+void
+asserv_free_motor (void);
+
+/**
+ * Stop the motor (and the bot).
+ * Other class command.
+ */
+void
+asserv_stop_motor (void);
+
+/**
+ * Move linearly.
+ * Move class command.
+ * @param distance the distance to move (mm).
+ */
+void
+asserv_move_linearly (int32_t distance);
+
+/**
+ * Move angularly (turn).
+ * Move class command.
+ * @param angle the angle to turn.
+ */
+void
+asserv_move_angularly (int16_t angle);
+
+/**
+ * Make the bot turn of an absolute angle.
+ * The angle is absolute and not a difference with the current one.
+ * @param a the absolute angle
+ */
+void
+asserv_goto_angle (int16_t angle);
+
+/**
+ * Go to an absolute position and then an absolute angle.
+ * @param x the absolute position on the X axis.
+ * @param y the absolute position on the Y axis.
+ * @param a the absolute angle.
+ * @param backward 0 no backward, ASSERV_BACKWARD backward compulsary,
+ * ASSERV_REVERT_OK backward allowed.
+ */
+void
+asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward);
+
+/**
+ * Go to the wall (moving backward).
+ * Move class command.
+ */
+void
+asserv_go_to_the_wall (uint8_t backward);
+
+/**
+ * Move the motor0.
+ * Motor0 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_motor0_absolute (uint16_t position, uint8_t speed);
+
+/**
+ * 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_motor1_absolute (uint16_t position, uint8_t speed);
+
+/**
+ * Set current X position.
+ * Other class command.
+ * @param x X position.
+ */
+void
+asserv_set_x_position (int32_t x);
+
+/**
+ * Set current Y position.
+ * Other class command.
+ * @param y Y position.
+ */
+void
+asserv_set_y_position (int32_t y);
+
+/**
+ * Set current angular position.
+ * Other class command.
+ * @param angle angular position.
+ */
+void
+asserv_set_angle_position (int16_t angle);
+
+/**
+ * Set speeds of movements.
+ * Other class command.
+ * @param linear_high linear high speed
+ * @param angular_high angular high speed
+ * @param linear_low linear low speed
+ * @param angular_low angular low speed
+ */
+void
+asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
+ uint8_t linear_low, uint8_t angular_low);
+
+/**
+ * Set the complete position of the bot.
+ * This is an helpful function preventing you from calling multiples other
+ * ones.
+ * It calls other class commands.
+ * @param x X position
+ * @param y Y position
+ * @param angle angular position
+ */
+void
+asserv_set_position (int32_t x, int32_t y, int16_t angle);
+
+/**
+ * Go to an absolute position in (X, Y).
+ * @param x the x position on the table.
+ * @param y the y position on the table.
+ * @param backward 0 no backward, ASSERV_BACKWARD backward compulsary,
+ * ASSERV_REVERT_OK backward allowed.
+ */
+void
+asserv_goto (uint32_t x, uint32_t y, uint8_t backward);
+
+/** Reset the motor0 to the zero position. */
+void
+asserv_motor0_zero_position (int8_t speed);
+
+/** Reset the motor1 to the zero position. */
+void
+asserv_motor1_zero_position (int8_t speed);
+
+/** Set PWM to zero for motor0. */
+void
+asserv_motor0_free (void);
+
+/** Set PWM to zero for motor1. */
+void
+asserv_motor1_free (void);
+
+#endif /* asserv_h */
diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c
new file mode 100644
index 00000000..af6ff5f9
--- /dev/null
+++ b/digital/ai/src/twi_master/mimot.c
@@ -0,0 +1,183 @@
+/* mimot.c */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "mimot.h"
+
+#include "twi_master.h"
+
+#include "modules/utils/byte.h"
+#include "io.h"
+
+/** Flag bit positions in mimot status byte. */
+enum mimot_status_flag_t
+{
+ /** Motor0 movement finished with success. */
+ mimot_status_flag_motor0_succeed = 0,
+ /** Motor0 movement finished with failure. */
+ mimot_status_flag_motor0_failed = 1,
+ /** Motor1 movement finished with success. */
+ mimot_status_flag_motor1_succeed = 2,
+ /** Motor1 movement finished with failure. */
+ mimot_status_flag_motor1_failed = 3,
+};
+
+/** Status structure. */
+struct mimot_status_t
+{
+ /** Status flags. */
+ uint8_t status;
+ /** Mimot input port. */
+ uint8_t input_port;
+ /** Motor0 position. */
+ uint16_t motor0_position;
+ /** Motor1 position. */
+ uint16_t motor1_position;
+};
+
+/** Current mimot status. */
+struct mimot_status_t mimot_status;
+
+void
+mimot_init (void)
+{
+ /* Nothing to do. */
+}
+
+void
+mimot_status_cb (uint8_t *status)
+{
+ /* Parse received data and store them. */
+ mimot_status.status = status[0];
+ mimot_status.input_port = status[1];
+ mimot_status.motor0_position = v8_to_v16 (status[3], status[4]);
+ mimot_status.motor1_position = v8_to_v16 (status[5], status[6]);
+}
+
+asserv_status_e
+mimot_motor0_cmd_status (void)
+{
+ if (mimot_status.status & _BV (mimot_status_flag_motor0_succeed))
+ return success;
+ else if (mimot_status.status & _BV (mimot_status_flag_motor0_failed))
+ return failure;
+ else
+ return none;
+}
+
+asserv_status_e
+mimot_motor1_cmd_status (void)
+{
+ if (mimot_status.status & _BV (mimot_status_flag_motor1_succeed))
+ return success;
+ else if (mimot_status.status & _BV (mimot_status_flag_motor1_failed))
+ return failure;
+ else
+ return none;
+}
+
+uint16_t
+mimot_get_motor0_position (void)
+{
+ return mimot_status.motor0_position;
+}
+
+uint16_t
+mimot_get_motor1_position (void)
+{
+ return mimot_status.motor1_position;
+}
+
+void
+mimot_reset (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'z';
+ twi_master_send_buffer (1);
+}
+
+void
+mimot_move_motor0_absolute (uint16_t position, uint8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'b';
+ buffer[1] = v16_to_v8 (position, 1);
+ buffer[2] = v16_to_v8 (position, 0);
+ buffer[3] = speed;
+ twi_master_send_buffer (4);
+}
+
+void
+mimot_move_motor1_absolute (uint16_t position, uint8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'c';
+ buffer[1] = v16_to_v8 (position, 1);
+ buffer[2] = v16_to_v8 (position, 0);
+ buffer[3] = speed;
+ twi_master_send_buffer (4);
+}
+
+void
+mimot_motor0_zero_position (int8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'B';
+ buffer[1] = speed;
+ twi_master_send_buffer (2);
+}
+
+void
+mimot_motor1_zero_position (int8_t speed)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'C';
+ buffer[1] = speed;
+ twi_master_send_buffer (2);
+}
+
+void
+mimot_motor0_clamp (int8_t speed, int16_t pwm)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'l';
+ buffer[1] = 0;
+ buffer[2] = speed;
+ buffer[3] = v16_to_v8 (pwm, 1);
+ buffer[4] = v16_to_v8 (pwm, 0);
+ twi_master_send_buffer (5);
+}
+
+void
+mimot_motor1_clamp (int8_t speed, int16_t pwm)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'l';
+ buffer[1] = 1;
+ buffer[2] = speed;
+ buffer[3] = v16_to_v8 (pwm, 1);
+ buffer[4] = v16_to_v8 (pwm, 0);
+ twi_master_send_buffer (5);
+}
+
diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h
new file mode 100644
index 00000000..60811c10
--- /dev/null
+++ b/digital/ai/src/twi_master/mimot.h
@@ -0,0 +1,95 @@
+#ifndef mimot_h
+#define mimot_h
+/* mimot.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "asserv.h"
+
+/**
+ * Provide interface to mimot board using the TWI protocol.
+ */
+
+/** Slave number in the twi_master list. */
+#define MIMOT_SLAVE 1
+
+/** Mimot TWI address. */
+#define MIMOT_TWI_ADDRESS 6
+
+/** Length of status buffer (not including CRC). */
+#define MIMOT_STATUS_LENGTH 7
+
+/** Initialise module. */
+void
+mimot_init (void);
+
+/** Called when a new status buffer is received, update the mimot
+ * information. */
+void
+mimot_status_cb (uint8_t *status);
+
+/** Return motor0 last command status. */
+asserv_status_e
+mimot_motor0_cmd_status (void);
+
+/** Return motor1 last command status. */
+asserv_status_e
+mimot_motor1_cmd_status (void);
+
+/** Get motor0 position in steps. */
+uint16_t
+mimot_get_motor0_position (void);
+
+/** Get motor1 position in steps. */
+uint16_t
+mimot_get_motor1_position (void);
+
+/** Reset mimot board. */
+void
+mimot_reset (void);
+
+/** Move motor0 to absolute position in steps. */
+void
+mimot_move_motor0_absolute (uint16_t position, uint8_t speed);
+
+/** Move motor1 to absolute position in steps. */
+void
+mimot_move_motor1_absolute (uint16_t position, uint8_t speed);
+
+/** Reset motor0 to zero position. */
+void
+mimot_motor0_zero_position (int8_t speed);
+
+/** Reset motor1 to zero position. */
+void
+mimot_motor1_zero_position (int8_t speed);
+
+/** Clamp motor0. */
+void
+mimot_motor0_clamp (int8_t speed, int16_t pwm);
+
+/** Clamp motor1. */
+void
+mimot_motor1_clamp (int8_t speed, int16_t pwm);
+
+#endif /* mimot_h */
diff --git a/digital/ai/src/twi_master/twi_master.c b/digital/ai/src/twi_master/twi_master.c
new file mode 100644
index 00000000..90977fd3
--- /dev/null
+++ b/digital/ai/src/twi_master/twi_master.c
@@ -0,0 +1,219 @@
+/* twi_master.c */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "twi_master.h"
+
+#include "asserv.h"
+#include "mimot.h"
+
+#include "modules/twi/twi.h"
+#include "modules/utils/utils.h"
+#include "modules/utils/crc.h"
+
+/** Interval between retransmissions. */
+#define TWI_MASTER_RETRANSMIT_INTERVAL 10
+
+/** Index of the slave sequence number. */
+#define TWI_MASTER_STATUS_SEQ_INDEX 2
+
+/** Maximum command payload size. */
+#define TWI_MASTER_COMMAND_PAYLOAD_MAX 13
+
+/** Maximum status payload size. */
+#define TWI_MASTER_STATUS_PAYLOAD_MAX 15
+
+/** Maximum number of pending commands. */
+#define TWI_MASTER_PENDING_MAX 16
+
+/** Position of next free command. */
+#define TWI_MASTER_PENDING_TAIL \
+ ((twi_master.pending_head + twi_master.pending_nb) \
+ % TWI_MASTER_PENDING_MAX)
+
+/** Pending command structure. */
+struct twi_master_command_t
+{
+ /** Addressed slave index. */
+ uint8_t slave;
+ /** Command payload with space for CRC. */
+ uint8_t command[TWI_MASTER_COMMAND_PAYLOAD_MAX + 1];
+ /** Command length not including CRC. */
+ uint8_t length;
+};
+
+/** Global context. */
+struct twi_master_t
+{
+ /** Retransmission counter, retransmit when it reach zero. It is also set
+ * to zero after an acknowledge so that the next pending command is
+ * sent. */
+ uint8_t retransmit_counter;
+ /** Index of next pending command. */
+ uint8_t pending_head;
+ /** Number of pending commands. */
+ uint8_t pending_nb;
+ /** Table of pending commands. The next command is at pending_head.
+ * Next commands are stored in a circular buffer way. */
+ struct twi_master_command_t pending[TWI_MASTER_PENDING_MAX];
+} twi_master;
+
+/** Callback called when a slave status has been read.
+ * - status: status buffer (without CRC). */
+typedef void (*twi_master_slave_status_cb) (uint8_t *status);
+
+/** Information on a slave. */
+struct twi_master_slave_t
+{
+ /** Slave address. */
+ uint8_t address;
+ /** Last command sequence number. */
+ uint8_t seq;
+ /** Size of the status buffer not including CRC. */
+ uint8_t status_length;
+ /** Status callback. */
+ twi_master_slave_status_cb status_cb;
+};
+
+/** Information on all slaves. */
+static struct twi_master_slave_t twi_master_slaves[] = {
+ { ASSERV_TWI_ADDRESS, 0, ASSERV_STATUS_LENGTH, asserv_status_cb },
+ { MIMOT_TWI_ADDRESS, 0, MIMOT_STATUS_LENGTH, mimot_status_cb },
+};
+
+/** Send first pending message if available. */
+static void
+twi_master_send_head (void)
+{
+ if (twi_master.pending_nb)
+ {
+ struct twi_master_command_t *c =
+ &twi_master.pending[twi_master.pending_head];
+ /* Send command. */
+ twi_master_send (twi_master_slaves[c->slave].address, c->command,
+ c->length + 1);
+ twi_master_wait ();
+ /* Reset retransmission counter. */
+ twi_master.retransmit_counter = TWI_MASTER_RETRANSMIT_INTERVAL;
+ }
+}
+
+/** Update slave status, return non zero on success. */
+static uint8_t
+twi_master_update_status (uint8_t slave, uint8_t init)
+{
+ uint8_t buffer[TWI_MASTER_STATUS_PAYLOAD_MAX + 1];
+ /* Read status. */
+ twi_master_recv (twi_master_slaves[slave].address, buffer,
+ twi_master_slaves[slave].status_length + 1);
+ uint8_t ret = twi_master_wait ();
+ if (ret != twi_master_slaves[slave].status_length + 1)
+ return 0;
+ uint8_t crc = crc_compute (buffer + 1,
+ twi_master_slaves[slave].status_length);
+ if (crc != buffer[0])
+ return 0;
+ if (init)
+ twi_master_slaves[slave].seq =
+ buffer[1 + TWI_MASTER_STATUS_SEQ_INDEX];
+ /* Call user callback. */
+ twi_master_slaves[slave].status_cb (buffer + 1);
+ /* Update pending command list. */
+ if (twi_master.pending_nb)
+ {
+ struct twi_master_command_t *c =
+ &twi_master.pending[twi_master.pending_head];
+ if (slave == c->slave
+ && buffer[1 + TWI_MASTER_STATUS_SEQ_INDEX] == c->command[1])
+ {
+ /* Successfully acknowledged. */
+ twi_master.pending_nb--;
+ twi_master.pending_head = (twi_master.pending_head + 1)
+ % TWI_MASTER_PENDING_MAX;
+ /* Trigger next transmission. */
+ twi_master.retransmit_counter = 0;
+ }
+ }
+ /* Success. */
+ return 1;
+}
+
+void
+twi_master_init (void)
+{
+ uint8_t i;
+ /* Initialise hardware. */
+ twi_init (AC_IO_TWI_ADDRESS);
+ /* Get first status and reset sequence number. */
+ for (i = 0; i < UTILS_COUNT (twi_master_slaves); i++)
+ while (!twi_master_update_status (i, 1))
+ ;
+}
+
+uint8_t
+twi_master_sync (void)
+{
+ uint8_t i;
+ /* Update all slaves status. */
+ for (i = 0; i < UTILS_COUNT (twi_master_slaves); i++)
+ twi_master_update_status (i, 0);
+ /* If command is to be retransmitted (or transmitted for the first time
+ * after a acknowledge). */
+ if (twi_master.retransmit_counter == 0)
+ twi_master_send_head ();
+ else
+ twi_master.retransmit_counter--;
+ /* Synchronised if no pending command. */
+ return twi_master.pending_nb == 0;
+}
+
+uint8_t *
+twi_master_get_buffer (uint8_t slave)
+{
+ assert (twi_master.pending_nb < TWI_MASTER_PENDING_MAX);
+ struct twi_master_command_t *c =
+ &twi_master.pending[TWI_MASTER_PENDING_TAIL];
+ /* Store slave. */
+ c->slave = slave;
+ /* Skip CRC and sequence number. */
+ return &c->command[2];
+}
+
+void
+twi_master_send_buffer (uint8_t length)
+{
+ assert (length != 0);
+ struct twi_master_command_t *c =
+ &twi_master.pending[TWI_MASTER_PENDING_TAIL];
+ /* Fill sequence number, compute CRC, store length. */
+ c->command[1] = ++twi_master_slaves[c->slave].seq;
+ c->command[0] = crc_compute (&c->command[1], length + 1);
+ c->length = length + 1;
+ /* Add to the list of pending command. */
+ twi_master.pending_nb++;
+ /* Early transmission. */
+ if (twi_master.pending_nb == 1)
+ twi_master_send_head ();
+}
+
diff --git a/digital/ai/src/twi_master/twi_master.h b/digital/ai/src/twi_master/twi_master.h
new file mode 100644
index 00000000..e75dbfab
--- /dev/null
+++ b/digital/ai/src/twi_master/twi_master.h
@@ -0,0 +1,68 @@
+#ifndef twi_master_h
+#define twi_master_h
+/* twi_master.h */
+/* ai - Robot Artificial Intelligence. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+/**
+ * Handle communication as a master to several slaves.
+ *
+ * The communication protocol is always based on:
+ * - a message sent to the slave with a command to execute,
+ * - a status read from the slave containing the current slave state.
+ *
+ * The first byte of all messages is a CRC of the following bytes, called the
+ * message payload.
+ *
+ * The first byte of payload sent to slave is the command sequence number. It
+ * is used by the master to know if its command has been handled. The last
+ * handled command sequence number is available in the slave status (third
+ * byte).
+ *
+ * As long as the slave last command sequence number is not equal to the last
+ * sent sequence number, the master can not send any other command. If the
+ * slave do not acknowledge the command after a time out, the command is sent
+ * again.
+ *
+ * Several commands can be stored by this module, it will defer their
+ * transmission until the first command is acknowledged.
+ */
+
+/** Initialise module. */
+void
+twi_master_init (void);
+
+/** Synchronise all slaves, return non zero if synchronised. */
+uint8_t
+twi_master_sync (void);
+
+/** Get a buffer to send a command to the corresponding slave. */
+uint8_t *
+twi_master_get_buffer (uint8_t slave);
+
+/** Send previously got buffer with the given length. */
+void
+twi_master_send_buffer (uint8_t length);
+
+#endif /* twi_master_h */