summaryrefslogtreecommitdiff
path: root/digital/io/src/asserv.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-02 18:52:14 +0200
committerNicolas Schodet2011-05-02 23:00:49 +0200
commit8a6cac0a41be34d36122ab5271295b0ee83fddac (patch)
treeca779a03b2264f6777b8a10dc01aa84698496c29 /digital/io/src/asserv.c
parent4909f63bea2598489a5895f9bbe1e2622093caa1 (diff)
digital/{ai,io}: move TWI master files to ai directory
Diffstat (limited to 'digital/io/src/asserv.c')
-rw-r--r--digital/io/src/asserv.c452
1 files changed, 0 insertions, 452 deletions
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);
-}
-