From 0a60b83874961ceca69420eda184b78f090ffb16 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 17 May 2012 11:48:05 +0200 Subject: digital/asserv/src/asserv: add stop traj mode --- digital/asserv/src/asserv/traj.c | 26 ++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 3 +++ digital/asserv/src/asserv/twi_proto.c | 4 +--- 3 files changed, 30 insertions(+), 3 deletions(-) (limited to 'digital/asserv/src') diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 48802fcb..7f1be791 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -52,6 +52,8 @@ /** Traj mode enum. */ enum { + /* Detect end of stop command. */ + TRAJ_STOP, /* Detect end of speed controled position control. */ TRAJ_SPEED, /* Go to the wall. */ @@ -110,6 +112,27 @@ traj_init (void) traj_set_angle_limit (traj_angle_limit); } +/** Wait for stop mode. */ +void +traj_stop (void) +{ + if (cs_main.speed_theta.cur_f == 0 && cs_main.speed_alpha.cur_f == 0) + { + control_state_finished (&cs_main.state); + traj_mode = TRAJ_DONE; + } +} + +/** Start stop mode. */ +void +traj_stop_start (void) +{ + speed_control_set_speed (&cs_main.speed_theta, 0); + speed_control_set_speed (&cs_main.speed_alpha, 0); + control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL, 0); + traj_mode = TRAJ_STOP; +} + /** Wait for zero speed mode. */ void traj_speed (void) @@ -447,6 +470,9 @@ traj_update (void) { switch (traj_mode) { + case TRAJ_STOP: + traj_stop (); + break; case TRAJ_SPEED: traj_speed (); break; diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index a2f36cda..3e36b681 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -39,6 +39,9 @@ traj_init (void); void traj_update (void); +void +traj_stop_start (void); + void traj_speed_start (void); diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 7c649e1c..c9e8a1f4 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -138,9 +138,7 @@ twi_proto_callback (u8 *buf, u8 size) break; case c ('s', 0): /* Stop (set zero speed). */ - speed_control_set_speed (&cs_main.speed_theta, 0); - speed_control_set_speed (&cs_main.speed_alpha, 0); - control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0); + traj_stop_start (); break; case c ('l', 3): /* Set linear speed controlled position consign. -- cgit v1.2.3