From f20cbaddb60aeda6e300ce4147eaee99fb8e2dfc Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Mon, 10 Mar 2008 15:53:42 +0100 Subject: * digital/io/src/asserv - implement functions to receive and handle the ''shared'' status buffer between asserv and io; - implement functions for the upper layer (io) to get the status of the asserv board; - comments improvements and corrections. --- digital/io/src/asserv.c | 132 +++++++++++++++++++++++++++++++++++++++------ digital/io/src/avrconfig.h | 2 + 2 files changed, 117 insertions(+), 17 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 2171e04c..a50a6ad3 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -27,17 +27,19 @@ #include "modules/twi/twi.h" /* twi_* */ #include "modules/utils/byte.h" /* v*_to_v* */ +#include "io.h" /** - * Sequence number. - * It is used for the acknowledge of the command send to the asserv. + * @defgroup AsservPrivate Asserv module private variables and functions + * declarations and definitions + * @{ */ -static uint8_t asserv_twi_seq; /** - * Flag to know if last command has been proceed by the asserv board. + * Sequence number. + * It is used for the acknowledge of the command sent to the asserv. */ -static uint8_t asserv_twi_seq_ack; +static uint8_t asserv_twi_seq; /** * Shared buffer used to send commands to the asserv. @@ -49,6 +51,26 @@ static uint8_t asserv_twi_buffer[7]; */ static uint8_t *asserv_twi_buffer_param = &asserv_twi_buffer[2]; +/** + * Status structure maintains by the update command. + */ +typedef struct asserv_struct_s +{ + /** Status flags. */ + uint8_t status; + /** Sequence number. */ + uint8_t seq; + /** Bot position. */ + asserv_position_t position; + /** Arm position. */ + uint16_t arm_position; +} asserv_struct_s; + +/** + * Status variable. + */ +asserv_struct_s asserv_status; + /** * Update TWI module until request (send or receive) is finished. * This functions is blocking. @@ -70,17 +92,6 @@ asserv_twi_update (void); static inline uint8_t asserv_twi_send_command (uint8_t command, uint8_t length); - -/* Initialize the asserv control module. */ -void -asserv_init (void) -{ - /* Initialize TWI with my (io) address */ - twi_init (AC_IO_TWI_ADDRESS); - /* We are at first command */ - asserv_twi_seq = asserv_twi_seq_ack = 0; -} - /* Update TWI module until request (send or receive) is finished. */ static inline void asserv_twi_update (void) @@ -94,7 +105,7 @@ static inline uint8_t asserv_twi_send_command (uint8_t command, uint8_t length) { /* Check we are not doing a command ? */ - if (asserv_twi_seq != asserv_twi_seq_ack) + if (!asserv_last_cmd_ack ()) return 1; /* Put the command into the buffer */ @@ -110,6 +121,93 @@ asserv_twi_send_command (uint8_t command, uint8_t length) asserv_twi_update (); return 0; } +/** @} */ + +/* Initialize the asserv control module. */ +void +asserv_init (void) +{ + /* Initialize TWI with my (io) address */ + twi_init (AC_IO_TWI_ADDRESS); + /* We are at first command */ + asserv_twi_seq = asserv_status.seq = 0; +} + +/* 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 status_buffer[AC_ASSERV_STATUS_LENGTH]; + /* Read data from the asserv card */ + twi_ms_read (AC_ASSERV_TWI_ADDRESS, status_buffer , AC_ASSERV_STATUS_LENGTH); + /* Update until done */ + asserv_twi_update (); + /* Parse received data and store them */ + asserv_status.status = status_buffer[0]; + asserv_status.seq = status_buffer[1]; + asserv_status.position.x = v8_to_v32 (status_buffer[2], status_buffer[3], + status_buffer[4], 0); + asserv_status.position.y = v8_to_v32 (status_buffer[5], status_buffer[6], + status_buffer[7], 0); + asserv_status.position.a = v8_to_v16 (status_buffer[8], status_buffer[9]); + asserv_status.arm_position = v8_to_v16 (status_buffer[10], status_buffer[11]); +} + +/* 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); +} + +/* Is last move class command has successfully ended? */ +asserv_status_e +asserv_move_cmd_status (void) +{ + /* Check Motor Finished flag */ + if (asserv_status.status & _BV (0)) + return success; + /* Check Motor Blocked flag */ + else if (asserv_status.status & _BV (1)) + return failure; + /* Otherwise, not finished nor failure */ + return none; +} + +/* Is last arm class command has successfully ended? */ +asserv_status_e +asserv_arm_cmd_status (void) +{ + /* Check Arm Finished flag */ + if (asserv_status.status & _BV (2)) + return success; + /* Check Arm Blocked flag */ + else if (asserv_status.status & _BV (3)) + return failure; + /* Otherwise, not finished nor failure */ + return none; +} + +/* Get the current position of the bot. */ +void +asserv_get_position (asserv_position_t current_position) +{ + /* Copy last received status buffer information to current position */ + current_position.x = asserv_status.position.x; + current_position.y = asserv_status.position.y; + current_position.a = asserv_status.position.a; +} + +/* Get the arm position. */ +uint16_t +asserv_get_arm_position (void) +{ + /* Return the position of the arm of the current status buffer */ + return asserv_status.arm_position; +} /* Reset the asserv board. */ void diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h index 4d72b213..e7555acb 100644 --- a/digital/io/src/avrconfig.h +++ b/digital/io/src/avrconfig.h @@ -94,5 +94,7 @@ #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. */ +#define AC_ASSERV_STATUS_LENGTH 12 #endif /* avrconfig_h */ -- cgit v1.2.3