summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/twi_proto.c30
-rw-r--r--digital/io/doc/proto_asserv.txt41
-rw-r--r--digital/io/src/asserv.c55
-rw-r--r--digital/io/src/asserv.h27
-rw-r--r--digital/io/src/avrconfig.h2
-rw-r--r--digital/io/src/main.c8
6 files changed, 135 insertions, 28 deletions
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index f1baecb6..d93cfa18 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -70,11 +70,14 @@ twi_proto_update (void)
while (twi_sl_poll (buf, sizeof (buf)))
twi_proto_callback (buf, sizeof (buf));
/* Update status. */
- u8 status[12];
- status[0] = (speed_theta.cur < 0 ? (1 << 5) : 0)
- | (speed_theta.cur > 0 ? (1 << 4) : 0)
- | (state_aux[0].blocked << 3)
- | (state_aux[0].finished << 2)
+ u8 status[14];
+ status[0] = 0
+ | (state_aux[1].blocked << 7)
+ | (state_aux[1].finished << 6)
+ | (state_aux[0].blocked << 5)
+ | (state_aux[0].finished << 4)
+ | (speed_theta.cur < 0 ? (1 << 3) : 0)
+ | (speed_theta.cur > 0 ? (1 << 2) : 0)
| (state_main.blocked << 1)
| (state_main.finished << 0);
status[1] = twi_proto.seq;
@@ -88,6 +91,8 @@ twi_proto_update (void)
status[9] = v32_to_v8 (postrack_a, 1);
status[10] = v16_to_v8 (aux[0].pos, 1);
status[11] = v16_to_v8 (aux[0].pos, 0);
+ status[12] = v16_to_v8 (aux[1].pos, 1);
+ status[13] = v16_to_v8 (aux[1].pos, 0);
twi_sl_update (status, sizeof (status));
}
@@ -187,7 +192,7 @@ twi_proto_callback (u8 *buf, u8 size)
1, 0);
break;
case c ('b', 3):
- /* Move the arm.
+ /* Move the aux0.
* - w: new position.
* - b: speed. */
speed_aux[0].max = buf[4];
@@ -199,6 +204,19 @@ twi_proto_callback (u8 *buf, u8 size)
speed_aux[0].max = buf[2];
aux_traj_find_zero_start (&aux[0], 0);
break;
+ case c ('c', 3):
+ /* Move the aux1.
+ * - w: new position.
+ * - b: speed. */
+ speed_aux[1].max = buf[4];
+ aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]), 0);
+ break;
+ case c ('C', 1):
+ /* Find the zero position of the aux1.
+ * - b: speed. */
+ speed_aux[1].max = buf[2];
+ aux_traj_find_zero_start (&aux[1], 0);
+ break;
case c ('p', x):
/* Set parameters. */
if (twi_proto_params (&buf[2], size - 2) != 0)
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
@@ -113,6 +113,15 @@ 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.
*/
typedef struct asserv_position_t
@@ -140,6 +149,13 @@ 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
* - 0 we are not moving;
@@ -230,6 +246,17 @@ 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.
* @param x X position.
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;