From a87f059b7acdc9f00b2102249a647a51f8389de4 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Mon, 14 Apr 2008 12:00:59 +0200 Subject: * digital/io/src * asserv - improve the API of the arm functions to ease usage ; - add a new function to put the arm to the position to close the input hole. * get sample FSM - keep only the needed and real events ; - correctly names the states ; - improve the function to select where to put the samples ; * general - add headers for some standard configuration defines. * trap - add another entry to the enum to know its length. --- digital/io/src/asserv.c | 37 ++++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) (limited to 'digital/io/src/asserv.c') diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 66f407a8..4302ce10 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -28,6 +28,7 @@ #include "modules/twi/twi.h" /* twi_* */ #include "modules/utils/byte.h" /* v*_to_v* */ +#include "giboulee.h" /* BOT_* */ #include "io.h" /** @@ -121,6 +122,21 @@ asserv_twi_send_command (uint8_t command, uint8_t length); */ static inline uint8_t asserv_twi_send (uint8_t length); +/** + * Move the arm. + * A complete rotation correspond to 5000 steps. + * @param position desired goal position (in step). + * @param speed speed of the movement. + */ +void +asserv_move_arm_absolute (uint16_t position, uint8_t speed); + +/** + * Current position of the arm. + * We need to maintain it by ourself as it is more accurate than the one sent + * by the asserv board. + */ +static uint16_t asserv_arm_current_position; /* Update TWI module until request (send or receive) is finished. */ static inline void @@ -338,7 +354,7 @@ asserv_go_to_distributor (void) /* Move the arm. */ void -asserv_move_arm (uint16_t position, uint8_t speed) +asserv_move_arm_absolute (uint16_t position, uint8_t speed) { /* Put position and speed as parameters */ asserv_twi_buffer_param[0] = v16_to_v8 (position, 1); @@ -348,6 +364,25 @@ asserv_move_arm (uint16_t position, uint8_t speed) asserv_twi_send_command ('b', 3); } +/* Move the arm to a certain number of steps. */ +void +asserv_move_arm (int16_t offset, uint8_t speed) +{ + /* Compute the new desired position with the desired offset */ + asserv_arm_current_position += offset; + /* Move the arm to the desired position */ + asserv_move_arm_absolute (asserv_arm_current_position, speed); +} + +/* Move the arm to close the input hole. */ +void +asserv_close_input_hole (void) +{ + /* Move the arm */ + asserv_move_arm (asserv_arm_current_position % + BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED); +} + /* Set current X position. */ void asserv_set_x_position (int32_t x) -- cgit v1.2.3