From 4a78f478a8787f255b782f077b6931cbd51af1d3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 6 Apr 2008 23:22:14 +0200 Subject: * digital/asserv/src/asserv: - added go to the dispenser. --- digital/asserv/src/asserv/main.c | 7 +++++++ digital/asserv/src/asserv/traj.c | 38 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 3 +++ digital/asserv/src/asserv/twi_proto.c | 1 + 4 files changed, 49 insertions(+) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 25b0f024..3a0cf7cb 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -328,6 +328,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; traj_ftw_start (args[0]); break; + case c ('F', 1): + /* Go to the dispenser. + * - b: sequence number. */ + if (args[0] == state_main.sequence) + break; + traj_gtd_start (args[0]); + break; case c ('a', 2): /* Set both acknoledge. * - b: main ack sequence number. diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index d33c76c9..ce8a7585 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -46,6 +46,8 @@ enum { /* Go to the wall. */ TRAJ_FTW, + /* Go to the dispenser. */ + TRAJ_GTD, /* Everything done. */ TRAJ_DONE, }; @@ -111,6 +113,39 @@ traj_ftw_start (uint8_t seq) state_start (&state_main, seq); } +/** Go to the dispenser mode. */ +static void +traj_gtd (void) +{ + int16_t speed; + speed = speed_theta.slow; + speed *= 256; + speed_theta.use_pos = speed_alpha.use_pos = 0; + if (PINC & _BV (2)) + { + speed_theta.cons = speed; + speed_alpha.cons = 0; + } + else + { + speed_theta.cons = 0; + speed_alpha.cons = 0; + speed_theta.cur = 0; + speed_alpha.cur = 0; + state_finish (&state_main); + traj_mode = TRAJ_DONE; + } +} + +/** Start go to the dispenser mode. */ +void +traj_gtd_start (uint8_t seq) +{ + state_main.mode = MODE_TRAJ; + traj_mode = TRAJ_GTD; + state_start (&state_main, seq); +} + /* Compute new speed according the defined trajectory. */ void traj_update (void) @@ -120,6 +155,9 @@ traj_update (void) case TRAJ_FTW: traj_ftw (); break; + case TRAJ_GTD: + traj_gtd (); + break; case TRAJ_DONE: break; } diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index 283f2c49..d80460bf 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -36,4 +36,7 @@ traj_angle_offset_start (int32_t angle, uint8_t seq); void traj_ftw_start (uint8_t seq); +void +traj_gtd_start (uint8_t seq); + #endif /* traj_h */ diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index b7cfd22f..e977bf76 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -137,6 +137,7 @@ twi_proto_callback (u8 *buf, u8 size) break; case c ('F', 0): /* Go to the dispenser. */ + traj_gtd_start (0); break; case c ('b', 3): /* Move the arm. -- cgit v1.2.3