From f753bc39682f980feb49b7ff1bb9e9594f986e6b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 19 May 2011 23:24:08 +0200 Subject: digital/asserv: add push the wall traj mode, closes #167 --- digital/asserv/src/asserv/twi_proto.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) (limited to 'digital/asserv/src/asserv/twi_proto.c') diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index ff15c82f..fcf0a9cc 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -168,6 +168,24 @@ twi_proto_callback (u8 *buf, u8 size) * - b: 0: forward, 1: backward. */ traj_ftw_start (buf[2], 0); break; + case c ('G', 9): + /* Push the wall. + * - b: 0: forward, 1: backward. + * - 3b: init_x. + * - 3b: init_y. + * - w: init_a. */ + { + int32_t angle; + if (buf[9] == 0xff && buf[10] == 0xff) + angle = -1; + else + angle = v8_to_v32 (0, buf[9], buf[10], 0); + traj_ptw_start (buf[2], + v8_to_v32 (buf[3], buf[4], buf[5], 0xff), + v8_to_v32 (buf[6], buf[7], buf[8], 0xff), + angle, 0); + } + break; case c ('g', 2): /* Go to the wall using center sensor with delay. * - b: 0: forward, 1: backward. -- cgit v1.2.3