From 6abdc9299138fe3b1e4f84605492a9c634b3d088 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 3 May 2009 12:49:07 +0200 Subject: * digital/io, digital/asserv: - added TWI commands for the elevator. --- digital/io/doc/proto_asserv.txt | 41 +++++++++++++++++++----------- digital/io/src/asserv.c | 55 +++++++++++++++++++++++++++++++++++------ digital/io/src/asserv.h | 27 ++++++++++++++++++++ digital/io/src/avrconfig.h | 2 +- digital/io/src/main.c | 8 ++++++ 5 files changed, 111 insertions(+), 22 deletions(-) (limited to 'digital/io') diff --git a/digital/io/doc/proto_asserv.txt b/digital/io/doc/proto_asserv.txt index dd78003c..391950dd 100644 --- a/digital/io/doc/proto_asserv.txt +++ b/digital/io/doc/proto_asserv.txt @@ -92,10 +92,20 @@ This table describe the list of supported commands by the *asserv* card: +---------+-------+-----------------+------------------------------------------+ | 'b' | Arm | - position (2b) | Move the arm to a desired position | | | | - speed (1b) | at a specific speed | -| | | | - position: 5000 steps by round | +| | | | - position: 250 * 4 * 80 / 15 steps by | +| | | | round | | | | | - speed: in step by cycle (4.44ms) | +---------+-------+-----------------+------------------------------------------+ -| 'B' | arm | None | Find the zero position of the arm | +| 'B' | Arm | - speed (1b) | Find the zero position of the arm | +| | | | - speed: in step by cycle (4.44ms) | ++---------+-------+-----------------+------------------------------------------+ +| 'c' | Elvt | - position (2b) | Move the elevator to a desired position | +| | | - speed (1b) | at a specific speed | +| | | | - position: 72.34 steps by mm | +| | | | - speed: in step by cycle (4.44ms) | ++---------+-------+-----------------+------------------------------------------+ +| 'C' | Elvt | - speed (1b) | Find the zero position of the elevator | +| | | | - speed: in step by cycle (4.44ms) | +---------+-------+-----------------+------------------------------------------+ | 'p' | Other | See next table | Change setting values | +---------+-------+-----------------+------------------------------------------+ @@ -132,14 +142,14 @@ card. It is used to know if the last command sent by the *io* card is currently executed, is finished or has failed (i.e. we are blocked) and also to retreive -the current position (X, Y, alpha) of the bot and its arm. +the current position (X, Y, alpha) of the bot, its arm and elevator. The buffer is a static 12 bytes length one:: - 0 1 2 .. 4 5 .. 7 8 9 10 11 <-- byte position - +--------+--------+- - - -+- - - -+- - - - -+- - - - -+ - | status | seq | X pos | Y pos | angle | arm pos | - +--------+--------+- - - -+- - - -+- - - - -+- - - - -+ + 0 1 2 .. 4 5 .. 7 8 9 10 11 12 13 <-- byte position + +--------+--------+- - - -+- - - -+-------+---------+----------+ + | status | seq | X pos | Y pos | angle | arm pos | elevator | + +--------+--------+- - - -+- - - -+-------+---------+----------+ Status value @@ -154,16 +164,19 @@ Here is a list of the bits used and their definitions: 1. Movement finished with failure: the bot is blocked (MB); - 2. Arm movement finished with success (AF); + 2. Movement is going forward: the current linear speed is greater than 0 + (FWD); + + 3. Movement is going backward: the current linear speed is smaller than 0 + (BCK); + + 4. Arm movement finished with success (AF); - 3. Arm movement finished with failure: it should not happen, but we are not - sure yet (AB). + 5. Arm movement finished with failure: it should not happen (AB); - 4. Movement is going forward: the current linear speed is greater than 0 - (FWD) ; + 6. Elevator movement finished with success (EF); - 5. Movement is going backward: the current linear speed is smaller than 0 - (BCK) ; + 7. Elevator movement finished with failure: it should not happen (EB). The others bits are unused. diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 408f8cbe..a3175176 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -50,14 +50,18 @@ enum asserv_status_flag_e asserv_status_flag_move_succeed = 0, /** Bot movement finished with failure: the bot is blocked. */ asserv_status_flag_move_failed = 1, - /** Arm movement finished with success. */ - asserv_status_flag_arm_succeed = 2, - /** Arm movement finished with failure (can not happen?). */ - asserv_status_flag_arm_failed = 3, /** Bot is moving forward (linear speed greater than 0). */ - asserv_status_flag_move_forward = 4, + asserv_status_flag_move_forward = 2, /** Bot is moving backward (linear speed smaller than 0). */ - asserv_status_flag_move_backward = 5, + asserv_status_flag_move_backward = 3, + /** Arm movement finished with success. */ + asserv_status_flag_arm_succeed = 4, + /** Arm movement finished with failure (can not happen). */ + asserv_status_flag_arm_failed = 5, + /** Elevator movement finished with success. */ + asserv_status_flag_elevator_succeed = 6, + /** Elevator movement finished with failure (can not happen). */ + asserv_status_flag_elevator_failed = 7, }; typedef enum asserv_status_flag_e asserv_status_flag_e; @@ -93,6 +97,8 @@ typedef struct asserv_struct_s asserv_position_t position; /** Arm position. */ uint16_t arm_position; + /** Elevator position. */ + uint16_t elevator_position; } asserv_struct_s; /** @@ -157,7 +163,7 @@ asserv_twi_send (uint8_t length); /** * Move the arm. - * A complete rotation correspond to 5000 steps. + * A complete rotation correspond to 5333 steps. * @param position desired goal position (in step). * @param speed speed of the movement. */ @@ -248,6 +254,7 @@ asserv_update_status (void) status_buffer[7], 0)) >> 8; asserv_status.position.a = v8_to_v16 (status_buffer[8], status_buffer[9]); asserv_status.arm_position = v8_to_v16 (status_buffer[10], status_buffer[11]); + asserv_status.elevator_position = v8_to_v16 (status_buffer[12], status_buffer[13]); } /* Is last command sent to the asserv board is being executed? */ @@ -304,6 +311,20 @@ asserv_arm_cmd_status (void) return none; } +/* Is last elevator class command has successfully ended? */ +asserv_status_e +asserv_elevator_cmd_status (void) +{ + /* Check Elevator Finished flag */ + if (asserv_status.status & _BV (asserv_status_flag_elevator_succeed)) + return success; + /* Check Elevator Blocked flag */ + else if (asserv_status.status & _BV (asserv_status_flag_elevator_failed)) + return failure; + /* Otherwise, not finished nor failure */ + return none; +} + /* Get the current position of the bot. */ void asserv_get_position (asserv_position_t *current_position) @@ -327,6 +348,14 @@ asserv_get_arm_position (void) return asserv_status.arm_position; } +/* Get the elevator position. */ +uint16_t +asserv_get_elevator_position (void) +{ + /* Return the position of the elevator of the current status buffer */ + return asserv_status.elevator_position; +} + /* Are we moving forward/backward? */ uint8_t asserv_get_moving_direction (void) @@ -460,6 +489,18 @@ asserv_move_arm (int16_t offset, uint8_t speed) asserv_move_arm_absolute (asserv_arm_current_position, speed); } +/* Move the elevator. */ +void +asserv_move_elevator_absolute (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 elevator command to the asserv board */ + asserv_twi_send_command ('c', 3); +} + /* Set current X position. */ void asserv_set_x_position (int32_t x) diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index c1fdae74..07950244 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -112,6 +112,15 @@ asserv_move_cmd_status (void); asserv_status_e asserv_arm_cmd_status (void); +/** + * Is last elevator class command has successfully ended? + * This function is used to know the status of the last elevator command. It + * looks at the status register. + * @return the status of the last move class command. + */ +asserv_status_e +asserv_elevator_cmd_status (void); + /** * Structure for storing a position for the bot. */ @@ -139,6 +148,13 @@ asserv_get_position (asserv_position_t *current_position); uint16_t asserv_get_arm_position (void); +/** + * Get the elevator position. + * @return the position of the elevator (in steps). + */ +uint16_t +asserv_get_elevator_position (void); + /** * Are we moving forward/backward? * @return @@ -229,6 +245,17 @@ asserv_go_to_distributor (void); void asserv_move_arm (int16_t offset, uint8_t speed); +/** + * Move the elevator. + * Elevator 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_elevator_absolute (uint16_t position, uint8_t speed); + /** * Set current X position. * Other class command. diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h index 0c0c5430..f70cc766 100644 --- a/digital/io/src/avrconfig.h +++ b/digital/io/src/avrconfig.h @@ -105,6 +105,6 @@ /** TWI address of the asserv board. */ #define AC_ASSERV_TWI_ADDRESS 4 /** Length of the status buffer maintains by the asserv board. */ -#define AC_ASSERV_STATUS_LENGTH 12 +#define AC_ASSERV_STATUS_LENGTH 14 #endif /* avrconfig_h */ diff --git a/digital/io/src/main.c b/digital/io/src/main.c index e2435516..9f9874a4 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -582,6 +582,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) */ asserv_move_arm (v8_to_v16 (args[1], args[2]), args[3]); break; + case 'c': + /* Move the elevator + * - 2b: position ; + * - 1b: speed. + */ + asserv_move_elevator_absolute (v8_to_v16 (args[1], args[2]), + args[3]); + break; } } break; -- cgit v1.2.3