From fcee99a4ff34939a6dbde8238e660f5e70f8d8cd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 6 Apr 2008 23:21:52 +0200 Subject: * digital/asserv/src/asserv: - added go to the wall. --- digital/asserv/src/asserv/main.c | 5 +---- digital/asserv/src/asserv/traj.c | 30 ++++++++++++++++++++++++------ digital/asserv/src/asserv/traj.h | 3 +++ digital/asserv/src/asserv/twi_proto.c | 1 + 4 files changed, 29 insertions(+), 10 deletions(-) (limited to 'digital/asserv/src') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 45ae321e..25b0f024 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -326,10 +326,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - b: sequence number. */ if (args[0] == state_main.sequence) break; - state_main.mode = MODE_TRAJ; - speed_theta.use_pos = speed_alpha.use_pos = 0; - traj_mode = 10; - state_start (&state_main, args[0]); + traj_ftw_start (args[0]); break; case c ('a', 2): /* Set both acknoledge. diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 3b8bc7d3..d33c76c9 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -41,9 +41,16 @@ /* This file provides high level trajectories commands. */ -/** Traj mode: - * 10, 11: go to the wall. - */ +/** Traj mode enum. */ +enum +{ + /* Go to the wall. */ + TRAJ_FTW, + /* Everything done. */ + TRAJ_DONE, +}; + +/** Traj mode. */ uint8_t traj_mode; /** Angle offset. Directly handled to speed layer. */ @@ -68,6 +75,7 @@ traj_ftw (void) int16_t speed; speed = speed_theta.slow; speed *= 256; + speed_theta.use_pos = speed_alpha.use_pos = 0; if (PINC & _BV (0) && PINC & _BV (1)) { speed_theta.cons = -speed; @@ -90,19 +98,29 @@ traj_ftw (void) speed_theta.cur = 0; speed_alpha.cur = 0; state_finish (&state_main); - traj_mode = 11; + traj_mode = TRAJ_DONE; } } +/** Start go to the wall mode. */ +void +traj_ftw_start (uint8_t seq) +{ + state_main.mode = MODE_TRAJ; + traj_mode = TRAJ_FTW; + state_start (&state_main, seq); +} + /* Compute new speed according the defined trajectory. */ void traj_update (void) { switch (traj_mode) { - case 10: + case TRAJ_FTW: traj_ftw (); - case 11: + break; + case TRAJ_DONE: break; } } diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index 2eaf9a0e..283f2c49 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -33,4 +33,7 @@ traj_update (void); void traj_angle_offset_start (int32_t angle, uint8_t seq); +void +traj_ftw_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 9b3d8f36..b7cfd22f 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -133,6 +133,7 @@ twi_proto_callback (u8 *buf, u8 size) break; case c ('f', 0): /* Go to the wall. */ + traj_ftw_start (0); break; case c ('F', 0): /* Go to the dispenser. */ -- cgit v1.2.3