summaryrefslogtreecommitdiff
path: root/digital/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv/src')
-rw-r--r--digital/asserv/src/asserv/traj.c26
-rw-r--r--digital/asserv/src/asserv/traj.h3
-rw-r--r--digital/asserv/src/asserv/twi_proto.c4
3 files changed, 30 insertions, 3 deletions
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
@@ -40,6 +40,9 @@ void
traj_update (void);
void
+traj_stop_start (void);
+
+void
traj_speed_start (void);
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.