From a0f115ece1275600e8e97f28bdd4c21eeb1650a5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 19 May 2012 05:44:10 +0200 Subject: digital/asserv: add twi command to set acceleration --- digital/ai/src/twi_master/asserv.c | 13 +++++++++++++ digital/ai/src/twi_master/asserv.h | 4 ++++ digital/asserv/src/asserv/twi_proto.c | 9 +++++++++ 3 files changed, 26 insertions(+) diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c index ad36e00b..cff3587b 100644 --- a/digital/ai/src/twi_master/asserv.c +++ b/digital/ai/src/twi_master/asserv.c @@ -419,6 +419,19 @@ asserv_set_speed (uint16_t linear_high, uint16_t angular_high, twi_master_send_buffer (10); } +void +asserv_set_acc (uint16_t linear, uint16_t angular) +{ + uint8_t *buffer = twi_master_get_buffer (TWI_MASTER_ID_ASSERV); + buffer[0] = 'p'; + buffer[1] = 'a'; + buffer[2] = v16_to_v8 (linear, 1); + buffer[3] = v16_to_v8 (linear, 0); + buffer[4] = v16_to_v8 (angular, 1); + buffer[5] = v16_to_v8 (angular, 0); + twi_master_send_buffer (6); +} + void asserv_set_position (int32_t x, int32_t y, int16_t angle) { diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h index 1cbcb187..ac2ba146 100644 --- a/digital/ai/src/twi_master/asserv.h +++ b/digital/ai/src/twi_master/asserv.h @@ -271,6 +271,10 @@ void asserv_set_speed (uint16_t linear_high, uint16_t angular_high, uint16_t linear_low, uint16_t angular_low); +/** Set acceleration. */ +void +asserv_set_acc (uint16_t linear, uint16_t angular); + /** * Set the complete position of the bot. * This is an helpful function preventing you from calling multiples other diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index c9e8a1f4..cd79bf3e 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -313,6 +313,15 @@ twi_proto_params (u8 *buf, u8 size) cs_main.speed_alpha.slow = v8_to_v16 (buf[6], buf[7]); eat = 4; break; + case 'a': + /* Set acceleration. + * - w: theta acc. + * - w: alpha acc. */ + if (size < 4) + return 1; + cs_main.speed_theta.acc_f = v8_to_v16 (buf[0], buf[1]); + cs_main.speed_alpha.acc_f = v8_to_v16 (buf[2], buf[3]); + break; default: return 1; } -- cgit v1.2.3