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/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 ------- 7 files changed, 5 insertions(+), 1321 deletions(-) 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/io/src') 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