summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-07 01:36:39 +0200
committerNicolas Schodet2013-04-07 15:57:24 +0200
commitc58213184b835602210229ede670c854c35959d5 (patch)
tree9935475ca7703264abe4227155b9d7d5f2bec04b /digital
parent5512364835b89c2e2599bd19b76c8b08236fa089 (diff)
digital/asserv/src/asserv: add follow external consign trajectory
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/traj.c53
-rw-r--r--digital/asserv/src/asserv/traj.h6
-rw-r--r--digital/asserv/src/asserv/twi_proto.c13
3 files changed, 71 insertions, 1 deletions
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index 7f1be791..713163fc 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -62,6 +62,8 @@ enum
TRAJ_GTD,
/* Push the wall. */
TRAJ_PTW,
+ /* Follow external consign. */
+ TRAJ_FOLLOW,
/* Go to position. */
TRAJ_GOTO,
/* Go to angle. */
@@ -105,6 +107,12 @@ static uint8_t traj_center_delay;
/** Initial values for x, y and angle, or -1. */
static int32_t traj_init_x, traj_init_y, traj_init_a;
+/** External consign for follow mode. */
+static int16_t traj_follow_consign;
+
+/** Number of update since the last external consign update. */
+static uint8_t traj_follow_consign_age;
+
/** Initialise computed factors. */
void
traj_init (void)
@@ -333,6 +341,48 @@ traj_gtd_start (void)
#endif /* CONTACT_CENTER_IO */
}
+/** Follow externanl consign mode. */
+static void
+traj_follow (void)
+{
+ /* Check real time condition. */
+ traj_follow_consign_age++;
+ if (traj_follow_consign_age > 10)
+ {
+ control_state_blocked (&cs_main.state);
+ traj_mode = TRAJ_DONE;
+ }
+ else
+ {
+ /* Compute new consign. */
+ speed_control_pos_offset_from_here (&cs_main.speed_alpha,
+ traj_follow_consign);
+ }
+}
+
+/** Start follow external consign mode. */
+void
+traj_follow_start (uint8_t backward)
+{
+ traj_mode = TRAJ_FOLLOW;
+ int16_t speed = cs_main.speed_theta.slow;
+ if (backward)
+ speed = -speed;
+ speed_control_set_speed (&cs_main.speed_theta, speed);
+ speed_control_pos_offset (&cs_main.speed_alpha, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL, 0);
+ traj_follow_consign = 0;
+ traj_follow_consign_age = 0;
+}
+
+/** Update external consign. */
+void
+traj_follow_update (int16_t consign)
+{
+ traj_follow_consign = consign;
+ traj_follow_consign_age = 0;
+}
+
/** Go to position mode. */
static void
traj_goto (void)
@@ -485,6 +535,9 @@ traj_update (void)
case TRAJ_GTD:
traj_gtd ();
break;
+ case TRAJ_FOLLOW:
+ traj_follow ();
+ break;
case TRAJ_GOTO:
traj_goto ();
break;
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index 3e36b681..3f7c2ebf 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -62,6 +62,12 @@ void
traj_gtd_start (void);
void
+traj_follow_start (uint8_t backward);
+
+void
+traj_follow_update (int16_t consign);
+
+void
traj_goto_start (uint32_t x, uint32_t y, uint8_t backward);
void
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 686f236b..3ebde309 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -120,7 +120,13 @@ twi_proto_callback (u8 *buf, u8 size)
size -= 1;
}
/* Handle sequence number. */
- if (buf[0] == 0 || buf[0] == twi_proto.seq)
+ if (buf[0] == 0)
+ {
+ /* Transient command. */
+ traj_follow_update (v8_to_v16 (buf[1], buf[2]));
+ return;
+ }
+ if (buf[0] == twi_proto.seq)
return;
#define c(cmd, size) (cmd)
switch (c (buf[1], 0))
@@ -189,6 +195,11 @@ twi_proto_callback (u8 *buf, u8 size)
/* Go to the dispenser. */
traj_gtd_start ();
break;
+ case c ('o', 1):
+ /* Follow external consign.
+ * - b: 0: forward, 1: backward. */
+ traj_follow_start (buf[2]);
+ break;
case c ('x', 7):
/* Go to position.
* - 3b: x position.