From f3e2181e6e41225255e7fd654cd4989eac9e19e3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 1 May 2010 01:11:09 +0200 Subject: digital/io/src: handle several TWI command for several slaves, closes #122 --- digital/io/src/Makefile | 3 +- digital/io/src/asserv.c | 533 ++++++++++++-------------------------------- digital/io/src/asserv.h | 83 ++----- digital/io/src/avrconfig.h | 7 - digital/io/src/chrono.c | 30 +-- digital/io/src/main.c | 22 +- digital/io/src/trace.trc | 3 - digital/io/src/twi_master.c | 215 ++++++++++++++++++ digital/io/src/twi_master.h | 68 ++++++ 9 files changed, 464 insertions(+), 500 deletions(-) create mode 100644 digital/io/src/twi_master.c create mode 100644 digital/io/src/twi_master.h (limited to 'digital') 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. */ @@ -130,62 +106,12 @@ 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 @@ -164,6 +129,13 @@ asserv_get_motor1_position (void); 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 */ -- cgit v1.2.3