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 +++++++++++++++++++++++++++++++++++++ 1 file changed, 449 insertions(+) create mode 100644 digital/ai/src/twi_master/asserv.c (limited to 'digital/ai/src/twi_master/asserv.c') 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); +} + -- cgit v1.2.3