From 8a6cac0a41be34d36122ab5271295b0ee83fddac Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 2 May 2011 18:52:14 +0200 Subject: digital/{ai,io}: move TWI master files to ai directory --- digital/ai/src/twi_master/asserv.c | 449 ++++++++++++++++++++++++++++++++ digital/ai/src/twi_master/asserv.h | 297 ++++++++++++++++++++++ digital/ai/src/twi_master/mimot.c | 183 +++++++++++++ digital/ai/src/twi_master/mimot.h | 95 +++++++ digital/ai/src/twi_master/twi_master.c | 219 ++++++++++++++++ digital/ai/src/twi_master/twi_master.h | 68 +++++ digital/io/src/Makefile | 5 + digital/io/src/asserv.c | 452 --------------------------------- digital/io/src/asserv.h | 304 ---------------------- digital/io/src/mimot.c | 183 ------------- digital/io/src/mimot.h | 95 ------- digital/io/src/twi_master.c | 219 ---------------- digital/io/src/twi_master.h | 68 ----- 13 files changed, 1316 insertions(+), 1321 deletions(-) create mode 100644 digital/ai/src/twi_master/asserv.c create mode 100644 digital/ai/src/twi_master/asserv.h create mode 100644 digital/ai/src/twi_master/mimot.c create mode 100644 digital/ai/src/twi_master/mimot.h create mode 100644 digital/ai/src/twi_master/twi_master.c create mode 100644 digital/ai/src/twi_master/twi_master.h delete mode 100644 digital/io/src/asserv.c delete mode 100644 digital/io/src/asserv.h delete mode 100644 digital/io/src/mimot.c delete mode 100644 digital/io/src/mimot.h delete mode 100644 digital/io/src/twi_master.c delete mode 100644 digital/io/src/twi_master.h (limited to 'digital') 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 */ diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 5dfea34b..ceb7c206 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -11,6 +11,7 @@ io_SOURCES = main.c servo.avr.c eeprom.avr.c pwm.c \ # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \ trace flash spi +AI_MODULES = twi_master # Configuration file. CONFIGFILE = avrconfig.h # IO board use an ATMega128. @@ -19,6 +20,10 @@ AVR_MCU = atmega128 OPTIMIZE = -O2 HOST_LIBS = -lm +vpath %.c $(AI_MODULES:%=../../ai/src/%) +vpath %.h $(AI_MODULES:%=../../ai/src/%) +INCLUDES += -I. $(AI_MODULES:%=-I../../ai/src/%) + # FSM. FSM = ai FSM_SOURCES := top init move loader hola diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c deleted file mode 100644 index e9adcbbc..00000000 --- a/digital/io/src/asserv.c +++ /dev/null @@ -1,452 +0,0 @@ -/* asserv.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * 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" /* v*_to_v* */ -#include "modules/math/fixed/fixed.h" -#include "bot.h" -#include "io.h" - -/** - * @defgroup AsservPrivate Asserv module private variables and functions - * declarations and definitions - * @{ - */ - -/** - * 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; - -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_set_scale (uint32_t scale) -{ - asserv_scale = scale; - asserv_scale_inv = fixed_div_f824 (1L << 24, scale); -} - -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/io/src/asserv.h b/digital/io/src/asserv.h deleted file mode 100644 index a92d95f7..00000000 --- a/digital/io/src/asserv.h +++ /dev/null @@ -1,304 +0,0 @@ -#ifndef asserv_h -#define asserv_h -/* asserv.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * 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 from io using the TWI protocol. - * 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 - */ - -/** 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 motor0 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 in degrees - */ -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); - -/** Set scale. - * @param scale number of millimeter per step (f8.24). - */ -void -asserv_set_scale (uint32_t scale); - -/** 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/io/src/mimot.c b/digital/io/src/mimot.c deleted file mode 100644 index ba09a3f5..00000000 --- a/digital/io/src/mimot.c +++ /dev/null @@ -1,183 +0,0 @@ -/* mimot.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 "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/io/src/mimot.h b/digital/io/src/mimot.h deleted file mode 100644 index 98709162..00000000 --- a/digital/io/src/mimot.h +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef mimot_h -#define mimot_h -/* mimot.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. - * - * }}} */ -#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/io/src/twi_master.c b/digital/io/src/twi_master.c deleted file mode 100644 index cb600da8..00000000 --- a/digital/io/src/twi_master.c +++ /dev/null @@ -1,219 +0,0 @@ -/* 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 "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/io/src/twi_master.h b/digital/io/src/twi_master.h deleted file mode 100644 index e7cb9d86..00000000 --- a/digital/io/src/twi_master.h +++ /dev/null @@ -1,68 +0,0 @@ -#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_buffer (uint8_t length); - -#endif /* twi_master_h */ -- cgit v1.2.3