summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/asserv.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/asserv.c')
-rw-r--r--digital/io/src/asserv.c255
1 files changed, 255 insertions, 0 deletions
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
new file mode 100644
index 00000000..d36deb91
--- /dev/null
+++ b/digital/io/src/asserv.c
@@ -0,0 +1,255 @@
+/* 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 "asserv.h"
+
+#include "modules/twi/twi.h" /* twi_* */
+#include "modules/utils/byte.h" /* v*_to_v* */
+
+/**
+ * Sequence number.
+ * It is used for the acknowledge of the command send to the asserv.
+ */
+static uint8_t asserv_twi_seq;
+
+/**
+ * Flag to know if last command has been proceed by the asserv board.
+ */
+static uint8_t asserv_twi_seq_ack;
+
+/**
+ * Shared buffer used to send commands to the asserv.
+ */
+static uint8_t asserv_twi_buffer[7];
+
+/**
+ * Pointer to access the buffer to put the parameters list.
+ */
+static uint8_t *asserv_twi_buffer_param = &asserv_twi_buffer[2];
+
+/**
+ * Update TWI module until request (send or receive) is finished.
+ * This functions is blocking.
+ */
+static inline void
+asserv_twi_update (void);
+
+/**
+ * Send a command to the asserv board using the TWI module.
+ * If the command you want to send have some parameters, you need to fill
+ * asserv_twi_buffer_param buffer.
+ * @param command the command to send.
+ * @param length the length of the parameters list (in byte).
+ * @return
+ * - 0 when there is no error
+ * - 1 if there is already a command running on the asserv (not acknowledged
+ * or not finished yet).
+ */
+static inline uint8_t
+asserv_twi_send_command (uint8_t command, uint8_t length);
+
+
+/**
+ * Settings list of the asserv program.
+ */
+enum asserv_setting_e
+{
+ POSITION_X,
+ POSITION_Y,
+ POSITION_ANGULAR,
+ SPEED
+};
+
+/* Initialize the asserv control module. */
+void
+asserv_init (void)
+{
+ /* Initialize TWI with my (io) address */
+ twi_init (AC_IO_TWI_ADDRESS);
+ /* We are at first command */
+ asserv_twi_seq = asserv_twi_seq_ack = 0;
+}
+
+/* Update TWI module until request (send or receive) is finished. */
+static inline void
+asserv_twi_update (void)
+{
+ while (!twi_ms_is_finished ())
+ ;
+}
+
+/* Send a command to the asserv board using the TWI module. */
+static inline uint8_t
+asserv_twi_send_command (uint8_t command, uint8_t length)
+{
+ /* Check we are not doing a command ? */
+ if (asserv_twi_seq != asserv_twi_seq_ack)
+ return 1;
+
+ /* Put the command into the buffer */
+ asserv_twi_buffer[0] = command;
+ /* Put the sequence number */
+ asserv_twi_buffer[1] = ++asserv_twi_seq;
+
+ /* Send command to the asserv */
+ if (twi_ms_send (AC_ASSERV_TWI_ADDRESS, asserv_twi_buffer, length + 2) != 0)
+ return -1;
+
+ /* Update until the command is sent */
+ asserv_twi_update ();
+ return 0;
+}
+
+/* Reset the asserv board. */
+void
+asserv_reset (void)
+{
+ /* Send the reset command to the asserv board */
+ asserv_twi_send_command ('z', 0);
+}
+
+/* Free the motors (stop controlling them). */
+void
+asserv_free_motor (void)
+{
+ /* Send the free motor command to the asserv board */
+ asserv_twi_send_command ('w', 0);
+}
+
+/* Stop the motor (and the bot). */
+void
+asserv_stop_motor (void)
+{
+ /* Send the stop motor command to the asserv board */
+ asserv_twi_send_command ('s', 0);
+}
+
+/* Move linearly. */
+void
+asserv_move_linearly (int32_t distance)
+{
+ /* Put distance as parameter */
+ asserv_twi_buffer_param[0] = v32_to_v8 (distance, 3);
+ asserv_twi_buffer_param[1] = v32_to_v8 (distance, 2);
+ asserv_twi_buffer_param[2] = v32_to_v8 (distance, 1);
+ /* Send the linear move command to the asserv board */
+ asserv_twi_send_command ('l', 3);
+}
+
+ /* Move angularly (turn). */
+void
+asserv_move_angularly (int16_t angle)
+{
+ /* Put angle as parameter */
+ asserv_twi_buffer_param[0] = v16_to_v8 (angle, 1);
+ asserv_twi_buffer_param[1] = v16_to_v8 (angle, 0);
+ /* Send the angular move command to the asserv board */
+ asserv_twi_send_command ('a', 2);
+}
+
+/* Go to the wall (moving backward). */
+void
+asserv_go_to_the_wall (void)
+{
+ /* Send the go the wall command to the asserv board */
+ asserv_twi_send_command ('f', 0);
+}
+
+/* Move forward to approach a gutter. */
+void
+asserv_go_to_gutter (void)
+{
+ /* Send the go the gutter command to the asserv board */
+ asserv_twi_send_command ('F', 0);
+}
+
+/* Move the arm. */
+void
+asserv_move_arm (uint16_t position, uint8_t speed)
+{
+ /* Put position and speed as parameters */
+ asserv_twi_buffer_param[0] = v16_to_v8 (position, 1);
+ asserv_twi_buffer_param[1] = v16_to_v8 (position, 0);
+ asserv_twi_buffer_param[2] = speed;
+ /* Send the move the arm command to the asserv board */
+ asserv_twi_send_command ('b', 3);
+}
+
+/* Set current X position. */
+void
+asserv_set_x_position (int32_t x)
+{
+ /* 'X' subcommand */
+ asserv_twi_buffer_param[0] = 'X';
+ /* Put x position as parameter */
+ asserv_twi_buffer_param[1] = v32_to_v8 (x, 3);
+ asserv_twi_buffer_param[2] = v32_to_v8 (x, 2);
+ asserv_twi_buffer_param[3] = v32_to_v8 (x, 1);
+ /* Send the set X position command to the asserv board */
+ asserv_twi_send_command ('p', 4);
+}
+
+/* Set current Y position. */
+void
+asserv_set_y_position (int32_t y)
+{
+ /* 'Y' subcommand */
+ asserv_twi_buffer_param[0] = 'Y';
+ /* Put y position as parameter */
+ asserv_twi_buffer_param[1] = v32_to_v8 (y, 3);
+ asserv_twi_buffer_param[2] = v32_to_v8 (y, 2);
+ asserv_twi_buffer_param[3] = v32_to_v8 (y, 1);
+ /* Send the set Y position command to the asserv board */
+ asserv_twi_send_command ('p', 4);
+}
+
+/* Set current angular position. */
+void
+asserv_set_angle_position (int16_t angle)
+{
+ /* 'A' subcommand */
+ asserv_twi_buffer_param[0] = 'A';
+ /* Put angle position as parameter */
+ asserv_twi_buffer_param[1] = v32_to_v8 (angle, 1);
+ asserv_twi_buffer_param[2] = v32_to_v8 (angle, 0);
+ /* Send the set angular position command to the asserv board */
+ asserv_twi_send_command ('p', 3);
+}
+
+/* Set speeds of movements. */
+void
+asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
+ uint8_t linear_low, uint8_t angular_low)
+{
+ /* 's' subcommand */
+ asserv_twi_buffer_param[0] = 's';
+ /* Put speeds as parameters */
+ asserv_twi_buffer_param[1] = linear_high;
+ asserv_twi_buffer_param[2] = angular_high;
+ asserv_twi_buffer_param[3] = linear_low;
+ asserv_twi_buffer_param[4] = angular_low;
+ /* Send the set speed of movements command to the asserv board */
+ asserv_twi_send_command ('p', 5);
+}