summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/Makefile3
-rw-r--r--digital/io/src/asserv.c533
-rw-r--r--digital/io/src/asserv.h83
-rw-r--r--digital/io/src/avrconfig.h7
-rw-r--r--digital/io/src/chrono.c30
-rw-r--r--digital/io/src/main.c22
-rw-r--r--digital/io/src/trace.trc3
-rw-r--r--digital/io/src/twi_master.c215
-rw-r--r--digital/io/src/twi_master.h68
9 files changed, 464 insertions, 500 deletions
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 424df2cb..8e51b416 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -3,8 +3,9 @@ BASE = ../../avr
# Name of the program to build.
PROGS = io
# Sources to compile.
-io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c pwm.c \
+io_SOURCES = main.c servo.avr.c eeprom.avr.c pwm.c \
switch.avr.c chrono.c main_timer.avr.c servo_pos.c \
+ twi_master.c asserv.c \
simu.host.c contact.c usdist.c radar.c \
path.c food.c
# Modules needed for IO.
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index 87c3baa4..c9e95f51 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -22,26 +22,15 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
-
#include "common.h"
#include "asserv.h"
-#include "modules/twi/twi.h" /* twi_* */
+#include "twi_master.h"
+
#include "modules/utils/byte.h" /* v*_to_v* */
#include "modules/math/fixed/fixed.h"
-#include "modules/utils/crc.h"
-#include "modules/trace/trace.h"
#include "bot.h"
#include "io.h"
-#include "trace_event.h"
-
-/** Scaling factor. */
-uint32_t asserv_scale;
-
-/**
- * Last moving direction.
- */
-uint8_t asserv_last_moving_direction = 0;
/**
* @defgroup AsservPrivate Asserv module private variables and functions
@@ -73,25 +62,14 @@ enum asserv_status_flag_e
};
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;
-/**
- * Sequence number.
- * It is used for the acknowledge of the command sent to the asserv.
- */
-static uint8_t asserv_twi_seq;
-static uint8_t trace_asserv_twi_seq;
-
-/**
- * Shared buffer used to send commands to the asserv.
- */
-static uint8_t asserv_twi_buffer[15];
-
-/**
- * Pointer to access the buffer to put the parameters list.
- */
-static uint8_t *asserv_twi_buffer_param = &asserv_twi_buffer[3];
+/** Last moving direction. */
+static uint8_t asserv_last_moving_direction;
/**
* Structure for storing a position for the bot using asserv units.
@@ -115,8 +93,6 @@ typedef struct asserv_struct_s
uint8_t status;
/** Asserv board input port. */
uint8_t input_port;
- /** Sequence number. */
- uint8_t seq;
/** Bot position. */
asserv_position_t position;
/** Motor0 position. */
@@ -131,61 +107,11 @@ typedef struct asserv_struct_s
asserv_struct_s asserv_status;
/**
- * Retransmit counter initial value.
- */
-#define ASSERV_RETRANSMIT_COUNT 10
-
-/**
- * Retransmit counter.
- */
-static uint8_t asserv_retransmit_counter;
-
-/**
- * Length of the last transmitted command.
- */
-static uint8_t asserv_retransmit_length;
-
-/**
- * Update TWI module until request (send or receive) is finished.
- * This functions is blocking.
- */
-static inline void
-asserv_twi_update (void);
-
-/**
- * Send a command to the asserv board using the TWI module.
- * If the command you want to send have some parameters, you need to fill
- * asserv_twi_buffer_param buffer.
- * @param command the command to send.
- * @param length the length of the parameters list (in byte).
- * @return
- * - 0 when there is no error
- * - 1 if there is already a command running on the asserv (not acknowledged
- * or not finished yet).
- */
-static inline uint8_t
-asserv_twi_send_command (uint8_t command, uint8_t length);
-
-/**
- * Send a prepared command to the asserv board using TWI module.
- * It will reset the counter retransmission value and store the length for
- * future retransmission.
- * In comparison with \a asserv_twi_send_command this function is internal and
- * used by \a asserv_twi_send_command.
- * @param length th length of the complete command with parameters.
- * @return
- * - 0 if no error occurred.
- * - 1 if TWI transmission failed.
- */
-static inline uint8_t
-asserv_twi_send (uint8_t length);
-
-/**
* Move the motor0.
* @param position desired goal position (in step).
* @param speed speed of the movement.
*/
-void
+static void
asserv_move_motor0_absolute (uint16_t position, uint8_t speed);
/**
@@ -195,140 +121,28 @@ asserv_move_motor0_absolute (uint16_t position, uint8_t speed);
*/
static uint16_t asserv_motor0_current_position;
-/* Update TWI module until request (send or receive) is finished. */
-static inline void
-asserv_twi_update (void)
-{
- while (!twi_ms_is_finished ())
- ;
-}
-
-/* Send a command to the asserv board using the TWI module. */
-static inline uint8_t
-asserv_twi_send_command (uint8_t command, uint8_t length)
-{
- /* Check we are not doing a command ? */
- assert (asserv_last_cmd_ack ());
-
- /* Put the sequence number */
- asserv_twi_buffer[1] = ++asserv_twi_seq;
- /* Put the command into the buffer */
- asserv_twi_buffer[2] = command;
-
- /* Compute CRC. */
- asserv_twi_buffer[0] = crc_compute (&asserv_twi_buffer[1], length + 2);
-
- /* Send the prepared command */
- return asserv_twi_send (length + 3);
-}
-
-/* Send a prepared command to the asserv board using TWI module. */
-static inline uint8_t
-asserv_twi_send (uint8_t length)
-{
- /* Send command to the asserv */
- if (twi_ms_send (AC_ASSERV_TWI_ADDRESS, asserv_twi_buffer, length) != 0)
- return 1;
-
- /* Sending a command. */
- TRACE (TRACE_ASSERV__SEND, asserv_twi_buffer[1], asserv_twi_buffer[2]);
-
- /* Update until the command is sent */
- asserv_twi_update ();
-
- /* Reset retransmit counter */
- asserv_retransmit_counter = ASSERV_RETRANSMIT_COUNT;
- /* Store length for retransmission */
- asserv_retransmit_length = length;
-
- return 0;
-}
-/** @} */
-
-/* Initialize the asserv control module. */
void
asserv_init (void)
{
- /* Initialize TWI with my (io) address */
- twi_init (AC_IO_TWI_ADDRESS);
- /* Get first status of the asserv board */
- asserv_update_status ();
- /* Reset sequence number */
- asserv_twi_seq = asserv_status.seq;
- trace_asserv_twi_seq = asserv_twi_seq + 1;
- /* Scaling factor. */
asserv_set_scale (BOT_SCALE * (1L << 24));
}
-/* Update the status of the asserv board seen from the io program. */
void
-asserv_update_status (void)
-{
- /* Status buffer used to receive data from the asserv */
- static uint8_t buffer[AC_ASSERV_STATUS_LENGTH + 1];
- uint8_t *status_buffer = &buffer[1];
-
- /* Read data from the asserv card */
- twi_ms_read (AC_ASSERV_TWI_ADDRESS, buffer, AC_ASSERV_STATUS_LENGTH + 1);
- /* Update until done */
- asserv_twi_update ();
- /* Check CRC. */
- uint8_t crc = crc_compute (status_buffer, AC_ASSERV_STATUS_LENGTH);
- if (crc != buffer[0])
- return;
-
- /* Parse received data and store them */
- asserv_status.status = status_buffer[0];
- asserv_status.input_port = status_buffer[1];
- asserv_status.seq = status_buffer[2];
- asserv_status.position.x = ((int32_t) v8_to_v32 (status_buffer[3], status_buffer[4],
- status_buffer[5], 0)) >> 8;
- 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.motor0_position = v8_to_v16 (status_buffer[11], status_buffer[12]);
- asserv_status.motor1_position = v8_to_v16 (status_buffer[13], status_buffer[14]);
+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 ();
-
- if (trace_asserv_twi_seq == asserv_status.seq)
- {
- /* Next ack. */
- trace_asserv_twi_seq++;
- TRACE (TRACE_ASSERV__LAST_STATUS_ACK, asserv_status.seq,
- asserv_status.status);
- }
-}
-
-/* Is last command sent to the asserv board is being executed? */
-uint8_t
-asserv_last_cmd_ack (void)
-{
- /* Compare last command sequence number and the one acknowledge by the
- * asserv */
- return (asserv_status.seq == asserv_twi_seq);
-}
-
-/**
- * Re-send command if not acknowledged.
- */
-uint8_t
-asserv_retransmit (void)
-{
- /* Check if we have reached the maximum number of check before
- * retransmission */
- if (--asserv_retransmit_counter == 0)
- {
- TRACE (TRACE_ASSERV__RETRANSMIT, asserv_twi_buffer[1]);
- /* Retransmit! */
- asserv_twi_send (asserv_retransmit_length);
- return 1;
- }
- return 0;
}
-/* Is last move class command has successfully ended? */
asserv_status_e
asserv_move_cmd_status (void)
{
@@ -342,7 +156,6 @@ asserv_move_cmd_status (void)
return none;
}
-/* Is last motor0 class command has successfully ended? */
asserv_status_e
asserv_motor0_cmd_status (void)
{
@@ -356,7 +169,6 @@ asserv_motor0_cmd_status (void)
return none;
}
-/* Is last motor1 class command has successfully ended? */
asserv_status_e
asserv_motor1_cmd_status (void)
{
@@ -370,22 +182,18 @@ asserv_motor1_cmd_status (void)
return none;
}
-/* Get the current position of the bot. */
void
asserv_get_position (position_t *current_position)
{
- if (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;
- }
+ 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;
}
-/* Get the motor0 position. */
uint16_t
asserv_get_motor0_position (void)
{
@@ -393,7 +201,6 @@ asserv_get_motor0_position (void)
return asserv_status.motor0_position;
}
-/* Get the motor1 position. */
uint16_t
asserv_get_motor1_position (void)
{
@@ -401,7 +208,6 @@ asserv_get_motor1_position (void)
return asserv_status.motor1_position;
}
-/* Are we moving forward/backward? */
uint8_t
asserv_get_moving_direction (void)
{
@@ -415,112 +221,108 @@ asserv_get_moving_direction (void)
return 0;
}
-/* Reset the asserv board. */
+uint8_t
+asserv_get_last_moving_direction (void)
+{
+ return asserv_last_moving_direction;
+}
+
void
asserv_reset (void)
{
- /* Send the reset command to the asserv board */
- asserv_twi_send_command ('z', 0);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'z';
+ twi_master_send (1);
}
-/* Free the motors (stop controlling them). */
void
asserv_free_motor (void)
{
- /* Send the free motor command to the asserv board */
- asserv_twi_send_command ('w', 0);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'w';
+ twi_master_send (1);
}
-/* Stop the motor (and the bot). */
void
asserv_stop_motor (void)
{
- /* Send the stop motor command to the asserv board */
- asserv_twi_send_command ('s', 0);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 's';
+ twi_master_send (1);
}
-/* Move linearly. */
void
asserv_move_linearly (int32_t distance)
{
distance = fixed_mul_f824 (distance, asserv_scale_inv);
- /* Put distance as parameter */
- asserv_twi_buffer_param[0] = v32_to_v8 (distance, 2);
- asserv_twi_buffer_param[1] = v32_to_v8 (distance, 1);
- asserv_twi_buffer_param[2] = v32_to_v8 (distance, 0);
- /* Send the linear move command to the asserv board */
- asserv_twi_send_command ('l', 3);
+ 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 (4);
}
-/* Move angularly (turn). */
void
asserv_move_angularly (int16_t angle)
{
- /* Put angle as parameter */
- asserv_twi_buffer_param[0] = v16_to_v8 (angle, 1);
- asserv_twi_buffer_param[1] = v16_to_v8 (angle, 0);
- /* Send the angular move command to the asserv board */
- asserv_twi_send_command ('a', 2);
+ 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 (3);
}
-/* Make the bot turn of an absolute angle. */
void
asserv_goto_angle (int16_t angle)
{
- /* Put angle as parameter */
- asserv_twi_buffer_param[0] = v16_to_v8 (angle, 1);
- asserv_twi_buffer_param[1] = v16_to_v8 (angle, 0);
- /* Sent the got to an absolute angle command to the asserv board */
- asserv_twi_send_command ('y', 2);
+ 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 (3);
}
-/* Go to an absolute position and then an absolute angle. */
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);
- /* Put X as parameter */
- asserv_twi_buffer_param[0] = v32_to_v8 (x, 2);
- asserv_twi_buffer_param[1] = v32_to_v8 (x, 1);
- asserv_twi_buffer_param[2] = v32_to_v8 (x, 0);
- /* Put distance as parameter */
- asserv_twi_buffer_param[3] = v32_to_v8 (y, 2);
- asserv_twi_buffer_param[4] = v32_to_v8 (y, 1);
- asserv_twi_buffer_param[5] = v32_to_v8 (y, 0);
- /* Put angle as parameter */
- asserv_twi_buffer_param[6] = v16_to_v8 (a, 1);
- asserv_twi_buffer_param[7] = v16_to_v8 (a, 0);
- /* No backward. */
- asserv_twi_buffer_param[8] = backward;
- /* Send the got to an absolute position and them absolute angle command to
- * the asserv board */
- asserv_twi_send_command ('X', 9);
+ 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 (10);
}
-/* Go to the wall. */
void
asserv_go_to_the_wall (uint8_t backward)
{
- /* Put direction as parameters */
- asserv_twi_buffer_param[0] = backward;
- /* Send the go the wall command to the asserv board */
- asserv_twi_send_command ('f', 1);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'f';
+ buffer[1] = backward;
+ twi_master_send (2);
}
/* Move the motor0. */
-void
+static void
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 motor0 command to the asserv board */
- asserv_twi_send_command ('b', 3);
+ 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 (4);
}
-/* Move the motor0 to a certain number of steps. */
void
asserv_move_motor0 (int16_t offset, uint8_t speed)
{
@@ -530,150 +332,106 @@ asserv_move_motor0 (int16_t offset, uint8_t speed)
asserv_move_motor0_absolute (asserv_motor0_current_position, speed);
}
-/* Move the motor1. */
void
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 motor1 command to the asserv board */
- asserv_twi_send_command ('c', 3);
+ 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 (4);
}
-/* Set current X position. */
void
asserv_set_x_position (int32_t x)
{
x = fixed_mul_f824 (x, asserv_scale_inv);
- /* 'X' subcommand */
- asserv_twi_buffer_param[0] = 'X';
- /* Put x position as parameter */
- asserv_twi_buffer_param[1] = v32_to_v8 (x, 2);
- asserv_twi_buffer_param[2] = v32_to_v8 (x, 1);
- asserv_twi_buffer_param[3] = v32_to_v8 (x, 0);
- asserv_twi_buffer_param[4] = 0;
- /* Send the set X position command to the asserv board */
- asserv_twi_send_command ('p', 5);
+ 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 (5);
}
-/* Set current Y position. */
void
asserv_set_y_position (int32_t y)
{
y = fixed_mul_f824 (y, asserv_scale_inv);
- /* 'Y' subcommand */
- asserv_twi_buffer_param[0] = 'Y';
- /* Put y position as parameter */
- asserv_twi_buffer_param[1] = v32_to_v8 (y, 2);
- asserv_twi_buffer_param[2] = v32_to_v8 (y, 1);
- asserv_twi_buffer_param[3] = v32_to_v8 (y, 0);
- asserv_twi_buffer_param[4] = 0;
- /* Send the set Y position command to the asserv board */
- asserv_twi_send_command ('p', 5);
+ 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 (5);
}
-/* Set current angular position. */
void
asserv_set_angle_position (int16_t angle)
{
- /* 'A' subcommand */
- asserv_twi_buffer_param[0] = 'A';
- /* Put angle position as parameter */
- asserv_twi_buffer_param[1] = v32_to_v8 (angle, 1);
- asserv_twi_buffer_param[2] = v32_to_v8 (angle, 0);
- asserv_twi_buffer_param[3] = 0;
- /* Send the set angular position command to the asserv board */
- asserv_twi_send_command ('p', 4);
+ 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 (4);
}
-/* Set speeds of movements. */
void
asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
uint8_t linear_low, uint8_t angular_low)
{
- /* 's' subcommand */
- asserv_twi_buffer_param[0] = 's';
- /* Put speeds as parameters */
- asserv_twi_buffer_param[1] = linear_high;
- asserv_twi_buffer_param[2] = angular_high;
- asserv_twi_buffer_param[3] = linear_low;
- asserv_twi_buffer_param[4] = angular_low;
- asserv_twi_buffer_param[5] = 0;
- /* Send the set speed of movements command to the asserv board */
- asserv_twi_send_command ('p', 6);
+ 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 (6);
}
-/* Set the complete position of the bot. */
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);
- /* 'X' subcommand */
- asserv_twi_buffer_param[0] = 'X';
- /* Put x position as parameter */
- asserv_twi_buffer_param[1] = v32_to_v8 (x, 2);
- asserv_twi_buffer_param[2] = v32_to_v8 (x, 1);
- asserv_twi_buffer_param[3] = v32_to_v8 (x, 0);
- /* 'Y' subcommand */
- asserv_twi_buffer_param[4] = 'Y';
- /* Put y position as parameter */
- asserv_twi_buffer_param[5] = v32_to_v8 (y, 2);
- asserv_twi_buffer_param[6] = v32_to_v8 (y, 1);
- asserv_twi_buffer_param[7] = v32_to_v8 (y, 0);
- /* 'A' subcommand */
- asserv_twi_buffer_param[8] = 'A';
- /* Put angle position as parameter */
- asserv_twi_buffer_param[9] = v32_to_v8 (angle, 1);
- asserv_twi_buffer_param[10] = v32_to_v8 (angle, 0);
- asserv_twi_buffer_param[11] = 0;
- /* Send the whole things in a one time shot */
- asserv_twi_send_command ('p', 12);
+ 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 (12);
}
-/* Go to an absolute position in (X, Y). */
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);
- /* Put X as parameter */
- asserv_twi_buffer_param[0] = v32_to_v8 (x, 2);
- asserv_twi_buffer_param[1] = v32_to_v8 (x, 1);
- asserv_twi_buffer_param[2] = v32_to_v8 (x, 0);
- /* Put Y as parameter */
- asserv_twi_buffer_param[3] = v32_to_v8 (y, 2);
- asserv_twi_buffer_param[4] = v32_to_v8 (y, 1);
- asserv_twi_buffer_param[5] = v32_to_v8 (y, 0);
- /* No backward. */
- asserv_twi_buffer_param[6] = backward;
- /* Send the got to an absolute position command to the asserv board */
- asserv_twi_send_command ('x', 7);
+ 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 (8);
}
-/* Go to an absolute position at (X, Y) with backward enabled. */
-void
-asserv_goto_back (uint32_t x, uint32_t y)
-{
- x = fixed_mul_f824 (x, asserv_scale_inv);
- y = fixed_mul_f824 (y, asserv_scale_inv);
- /* Put X as parameter */
- asserv_twi_buffer_param[0] = v32_to_v8 (x, 2);
- asserv_twi_buffer_param[1] = v32_to_v8 (x, 1);
- asserv_twi_buffer_param[2] = v32_to_v8 (x, 0);
- /* Put Y as parameter */
- asserv_twi_buffer_param[3] = v32_to_v8 (y, 2);
- asserv_twi_buffer_param[4] = v32_to_v8 (y, 1);
- asserv_twi_buffer_param[5] = v32_to_v8 (y, 0);
- /* Authorise backward movements. */
- asserv_twi_buffer_param[6] = ASSERV_REVERT_OK;
- /* Send the goto to an absolute position command to the asserv board */
- asserv_twi_send_command ('x', 7);
-}
-
-/* Set scale. */
void
asserv_set_scale (uint32_t scale)
{
@@ -684,19 +442,18 @@ asserv_set_scale (uint32_t scale)
void
asserv_motor0_zero_position (void)
{
- asserv_twi_buffer_param[0] = 0x05;
- asserv_twi_send_command ('B', 1);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'B';
+ buffer[1] = 0x05;
+ twi_master_send (2);
}
void
asserv_motor1_zero_position (void)
{
- asserv_twi_buffer_param[0] = -0x10;
- asserv_twi_send_command ('C', 1);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'C';
+ buffer[1] = -0x10;
+ twi_master_send (2);
}
-uint8_t
-asserv_get_last_moving_direction (void)
-{
- return asserv_last_moving_direction;
-}
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index be7e061a..9ea12e2b 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -31,18 +31,16 @@
* This files contains the 'public' functions to send commands to the asserv
* board using a protocol over TWI communication.
* @see trunk/digital/io/doc/proto_asserv.txt
- * @todo
- * - the function to send a new command to the asserv board is protected from
- * sending one when the previous is not finished yet. But the information is
- * not raised to the upper layer (io). It can be a bad idea to give this
- * information to the upper layer because it can be mis-interpreted or
- * ignored.
*/
-#define ASSERV_MOTOR0_SPEED_DEFAULT 0x0C
-#define ASSERV_MOTOR1_SPEED_DEFAULT 0x46
+/** Slave number in twi_master list. */
+#define ASSERV_SLAVE 0
-#define ASSERV_MOTOR0_STEP_BY_DEGREE 14.814814
+/** 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
@@ -50,50 +48,17 @@
* previous define. */
#define ASSERV_REVERT_OK 2
-/**
- * Initialize the asserv control module.
- * This functions does not initialize the asserv board, but the underling
- * communication protocol used to communicate with the asserv (TWI).
- */
+/** Initialize the asserv control module. */
void
asserv_init (void);
-/**
- * Update the status of the asserv board seen from the io program.
- * This command asks the status buffer of the asserv, receive it and do some
- * internal updates.
- * You need to call this command regularly in order to be able to send new
- * commands (this module will not let you send new command if the previous one
- * has not been yet received).
- */
+/** Called when a new status buffer is received, update the asserv
+ * information. */
void
-asserv_update_status (void);
-
-/**
- * Is last command sent to the asserv board is being executed?
- * This function is used to know if the last command sent to the asserv board
- * has been received and is currently executing.
- * @return
- * - 0 if the command has not started to be executed.
- * - 1 if the command is currently been executed.
- */
-uint8_t
-asserv_last_cmd_ack (void);
+asserv_status_cb (uint8_t *status);
/**
- * Re-send command if not acknowledged.
- * This function should be called when the command has not been acknowledged
- * by the asserv board. It will re send the last command when a certain number
- * of cycle has been reached without any acknowledge from the asserv.
- * @return
- * - 0 if the command was not received.
- * - 1 if the command was re-sent.
- */
-uint8_t
-asserv_retransmit (void);
-
-/**
- * Status of a move or motor0 class command.
+ * Status of a move or motor class command.
* It is return by status functions.
*/
typedef enum asserv_status_e
@@ -165,6 +130,13 @@ 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.
*/
@@ -308,16 +280,8 @@ asserv_set_position (int32_t x, int32_t y, int16_t angle);
void
asserv_goto (uint32_t x, uint32_t y, uint8_t backward);
-/**
- * Go to an absolute position at (X, Y) with backward enabled.
- * @param x the x position on the table.
- * @param y the y position on the table.
- */
-void
-asserv_goto_back (uint32_t x, uint32_t y);
-
/** Set scale.
- * @param scale vous avez qu'à deviner (f8.24).
+ * @param scale number of millimeter per step (f8.24).
*/
void
asserv_set_scale (uint32_t scale);
@@ -334,11 +298,4 @@ asserv_motor0_zero_position (void);
void
asserv_motor1_zero_position (void);
-/**
- * Get the last moving direction of the bot.
- * @return 1 is forward, 2 is backward.
- */
-uint8_t
-asserv_get_last_moving_direction (void);
-
#endif /* asserv_h */
diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h
index 9b79b4e1..a9be1f29 100644
--- a/digital/io/src/avrconfig.h
+++ b/digital/io/src/avrconfig.h
@@ -108,13 +108,6 @@
/* io - io/ai board. */
/** TWI address of the io board. */
#define AC_IO_TWI_ADDRESS 2
-/** TWI address of the asserv board. */
-#define AC_ASSERV_TWI_ADDRESS 4
-/**
- * Length of the status buffer maintains by the asserv board.
- * This length should not include the CRC byte!
- */
-#define AC_ASSERV_STATUS_LENGTH 15
/* spi - SPI module. */
/** Select driver: HARD, SOFT, or NONE. */
diff --git a/digital/io/src/chrono.c b/digital/io/src/chrono.c
index 631c7096..b73b627c 100644
--- a/digital/io/src/chrono.c
+++ b/digital/io/src/chrono.c
@@ -28,6 +28,7 @@
#include "bot.h"
#include "main_timer.h"
#include "asserv.h"
+#include "twi_master.h"
#include "modules/utils/utils.h"
#include "modules/host/mex.h"
@@ -56,7 +57,7 @@
#define CHRONO_LOOP_DURATION_MS 4
/**
- * Time to wait before resetting asserv board, in ms.
+ * Time to wait before resetting slaves board, in ms.
*/
#define CHRONO_WAIT_BEFORE_RESET_MS 1000
@@ -128,36 +129,15 @@ chrono_end_match (uint8_t block)
{
/* Make sure previous command has been acknowledged. If not, retransmit
* until acknowledged */
- while (asserv_last_cmd_ack () == 0)
- {
- /* Update status */
- asserv_update_status ();
- /* Manage retransmission */
- asserv_retransmit ();
- /* Wait a little */
+ while (!twi_master_sync ())
utils_delay_ms (CHRONO_LOOP_DURATION_MS);
- }
/* Make the bot stop moving */
asserv_stop_motor ();
/* Wait until complete */
- while (42)
- {
- /* Update the asserv board */
- asserv_update_status ();
- /* Stop acknowledged ? */
- if (asserv_last_cmd_ack () == 0)
- {
- /* Retransmit if needed */
- asserv_retransmit ();
- /* Wait a little */
- utils_delay_ms (CHRONO_LOOP_DURATION_MS);
- }
- else
- /* Exit loop */
- break;
- }
+ while (!twi_master_sync ())
+ utils_delay_ms (CHRONO_LOOP_DURATION_MS);
/* Wait CHRONO_WAIT_BEFORE_RESET ms before reseting */
utils_delay_ms (CHRONO_WAIT_BEFORE_RESET_MS);
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index fb359461..b4d558f1 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -40,7 +40,8 @@
#include "main_timer.h"
#include "simu.host.h"
-#include "asserv.h" /* Functions to control the asserv board */
+#include "asserv.h"
+#include "twi_master.h"
#include "eeprom.h" /* Parameters loaded/stored in the EEPROM */
#include "fsm.h" /* fsm_* */
#include "bot.h"
@@ -189,6 +190,8 @@ main_init (void)
utils_delay_ms (500);
/* Asserv communication */
asserv_init ();
+ /* TWI master. */
+ twi_master_init ();
/* Switch module */
switch_init ();
/* Path module */
@@ -265,16 +268,8 @@ main_loop (void)
simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0);
}
- /* Update TWI module to get new data from the asserv board */
- asserv_update_status ();
-
- /* Is last command has been acknowledged? */
- if (asserv_last_cmd_ack () == 0)
- {
- /* Called function to manage retransmission */
- asserv_retransmit ();
- }
- else
+ /* Are TWI slaves synchronised? */
+ if (twi_master_sync ())
{
/* First, update modules */
/* Update switch module */
@@ -549,8 +544,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - 4b: x;
* - 4b: y.
*/
- asserv_goto_back (v8_to_v32 (args[1], args[2], args[3], args[4]),
- v8_to_v32 (args[5], args[6], args[7], args[8]));
+ asserv_goto (v8_to_v32 (args[1], args[2], args[3], args[4]),
+ v8_to_v32 (args[5], args[6], args[7], args[8]),
+ ASSERV_REVERT_OK);
break;
}
}
diff --git a/digital/io/src/trace.trc b/digital/io/src/trace.trc
index 34be1c12..c1b46347 100644
--- a/digital/io/src/trace.trc
+++ b/digital/io/src/trace.trc
@@ -1,6 +1,3 @@
-EVENT asserv__retransmit seq 1 "Asserv[0x%02x]: retransmiting."
-EVENT asserv__send seq 1 cmd 1 "Asserv[0x%02x]: sending: !%c."
-EVENT asserv__last_status_ack seq 1 status 1 "Asserv[0x%02x]: status: %x."
EVENT main_timer__late "MainTimer: we are late."
EVENT fsm__handle_event from 1 event 1 to 1 "FSM: %i =%i=> %i."
EVENT move__go_to xs 2 ys 2 as 2 xd 2 yd 2 ad 2 "Move: (%d, %d, %x) -> (%d, %d, %x)."
diff --git a/digital/io/src/twi_master.c b/digital/io/src/twi_master.c
new file mode 100644
index 00000000..94947808
--- /dev/null
+++ b/digital/io/src/twi_master.c
@@ -0,0 +1,215 @@
+/* twi_master.c */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "twi_master.h"
+
+#include "asserv.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 },
+};
+
+/** 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_ms_send (twi_master_slaves[c->slave].address, c->command,
+ c->length + 1);
+ while (!twi_ms_is_finished ())
+ ;
+ /* 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_ms_read (twi_master_slaves[slave].address, buffer,
+ twi_master_slaves[slave].status_length + 1);
+ while (!twi_ms_is_finished ())
+ ;
+ 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];
+ /* Skip CRC and sequence number. */
+ return &c->command[2];
+}
+
+void
+twi_master_send (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/io/src/twi_master.h b/digital/io/src/twi_master.h
new file mode 100644
index 00000000..143e8727
--- /dev/null
+++ b/digital/io/src/twi_master.h
@@ -0,0 +1,68 @@
+#ifndef twi_master_h
+#define twi_master_h
+/* twi_master.h */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2010 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+/**
+ * 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 (uint8_t length);
+
+#endif /* twi_master_h */