From c58213184b835602210229ede670c854c35959d5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 7 Apr 2013 01:36:39 +0200 Subject: digital/asserv/src/asserv: add follow external consign trajectory --- digital/asserv/src/asserv/traj.c | 53 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 6 ++++ digital/asserv/src/asserv/twi_proto.c | 13 ++++++++- 3 files changed, 71 insertions(+), 1 deletion(-) (limited to 'digital/asserv') 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 @@ -61,6 +61,12 @@ traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, 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); 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. -- cgit v1.2.3