From 473d501d1ead422ecc9680cb20e46a04a6356ed8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 14 May 2009 23:22:02 +0200 Subject: * digital/io, digital/asserv: - give asserv input port in TWI status. --- digital/asserv/src/asserv/twi_proto.c | 33 +++++++++++++++++++-------------- digital/io/doc/proto_asserv.txt | 8 ++++---- digital/io/src/asserv.c | 21 ++++++++++++--------- digital/io/src/avrconfig.h | 2 +- 4 files changed, 36 insertions(+), 28 deletions(-) (limited to 'digital') diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 735430a2..6168f245 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -40,6 +40,10 @@ #include "traj.h" #include "aux.h" +#ifdef HOST +# include "simu.host.h" +#endif + struct twi_proto_t { u8 seq; @@ -70,7 +74,7 @@ twi_proto_update (void) while (twi_sl_poll (buf, sizeof (buf))) twi_proto_callback (buf, sizeof (buf)); /* Update status. */ - u8 status[14]; + u8 status[15]; status[0] = 0 | (state_aux[1].blocked << 7) | (state_aux[1].finished << 6) @@ -80,19 +84,20 @@ twi_proto_update (void) | (speed_theta.cur > 0 ? (1 << 2) : 0) | (state_main.blocked << 1) | (state_main.finished << 0); - status[1] = twi_proto.seq; - status[2] = v32_to_v8 (postrack_x, 3); - status[3] = v32_to_v8 (postrack_x, 2); - status[4] = v32_to_v8 (postrack_x, 1); - status[5] = v32_to_v8 (postrack_y, 3); - status[6] = v32_to_v8 (postrack_y, 2); - status[7] = v32_to_v8 (postrack_y, 1); - status[8] = v32_to_v8 (postrack_a, 2); - 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); + status[1] = PINC; + status[2] = twi_proto.seq; + status[3] = v32_to_v8 (postrack_x, 3); + status[4] = v32_to_v8 (postrack_x, 2); + status[5] = v32_to_v8 (postrack_x, 1); + status[6] = v32_to_v8 (postrack_y, 3); + status[7] = v32_to_v8 (postrack_y, 2); + status[8] = v32_to_v8 (postrack_y, 1); + status[9] = v32_to_v8 (postrack_a, 2); + status[10] = v32_to_v8 (postrack_a, 1); + status[11] = v16_to_v8 (aux[0].pos, 1); + status[12] = v16_to_v8 (aux[0].pos, 0); + status[13] = v16_to_v8 (aux[1].pos, 1); + status[14] = v16_to_v8 (aux[1].pos, 0); twi_sl_update (status, sizeof (status)); } diff --git a/digital/io/doc/proto_asserv.txt b/digital/io/doc/proto_asserv.txt index e6e43c5b..ada584f9 100644 --- a/digital/io/doc/proto_asserv.txt +++ b/digital/io/doc/proto_asserv.txt @@ -147,10 +147,10 @@ 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 12 13 <-- byte position - +--------+--------+- - - -+- - - -+-------+---------+----------+ - | status | seq | X pos | Y pos | angle | arm pos | elevator | - +--------+--------+- - - -+- - - -+-------+---------+----------+ + 0 1 2 3 .. 5 6 .. 8 9 10 11 12 13 14 <-- byte position + +--------+------+-----+- - - -+- - - -+-------+---------+----------+ + | status | PINC | seq | X pos | Y pos | angle | arm pos | elevator | + +--------+------+-----+- - - -+- - - -+-------+---------+----------+ Status value diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 2bb6b515..b35c436c 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -91,6 +91,8 @@ typedef struct asserv_struct_s { /** Status flags. */ uint8_t status; + /** Asserv board input port. */ + uint8_t input_port; /** Sequence number. */ uint8_t seq; /** Bot position. */ @@ -242,19 +244,20 @@ asserv_update_status (void) /* Status buffer used to receive data from the asserv */ static uint8_t status_buffer[AC_ASSERV_STATUS_LENGTH]; /* Read data from the asserv card */ - twi_ms_read (AC_ASSERV_TWI_ADDRESS, status_buffer , AC_ASSERV_STATUS_LENGTH); + twi_ms_read (AC_ASSERV_TWI_ADDRESS, status_buffer, AC_ASSERV_STATUS_LENGTH); /* Update until done */ asserv_twi_update (); /* Parse received data and store them */ asserv_status.status = status_buffer[0]; - asserv_status.seq = status_buffer[1]; - asserv_status.position.x = ((int32_t) v8_to_v32 (status_buffer[2], status_buffer[3], - status_buffer[4], 0)) >> 8; - asserv_status.position.y = ((int32_t) v8_to_v32 (status_buffer[5], status_buffer[6], - 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]); + asserv_status.input_port = status_buffer[1]; + asserv_status.seq = status_buffer[2]; + asserv_status.position.x = ((int32_t) v8_to_v32 (status_buffer[3], status_buffer[4], + status_buffer[5], 0)) >> 8; + asserv_status.position.y = ((int32_t) v8_to_v32 (status_buffer[6], status_buffer[7], + status_buffer[8], 0)) >> 8; + asserv_status.position.a = v8_to_v16 (status_buffer[9], status_buffer[10]); + asserv_status.arm_position = v8_to_v16 (status_buffer[11], status_buffer[12]); + asserv_status.elevator_position = v8_to_v16 (status_buffer[13], status_buffer[14]); } /* Is last command sent to the asserv board is being executed? */ diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h index f70cc766..4b676c96 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 14 +#define AC_ASSERV_STATUS_LENGTH 15 #endif /* avrconfig_h */ -- cgit v1.2.3