From c4d20dd9ffa1c479de2858dd1b77ff014d1073dd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 20 Apr 2008 19:17:07 +0200 Subject: * digital/asserv/src/asserv: - added separate setting of position control for theta/alpha. - disable angular position control for go to the dispenser. --- digital/asserv/src/asserv/main.c | 14 ++++++++------ digital/asserv/src/asserv/pos.c | 6 ++++-- digital/asserv/src/asserv/state.h | 9 ++++++++- digital/asserv/src/asserv/traj.c | 16 ++++++---------- digital/asserv/src/asserv/twi_proto.c | 7 +++---- 5 files changed, 29 insertions(+), 23 deletions(-) (limited to 'digital/asserv/src/asserv') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 79969a6b..0a746149 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -253,6 +253,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - w: theta consign offset. * - w: alpha consign offset. */ state_main.mode = MODE_POS; + state_main.variant = 0; pos_theta.cons += v8_to_v16 (args[0], args[1]); pos_alpha.cons += v8_to_v16 (args[2], args[3]); break; @@ -260,11 +261,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Add to auxiliary position consign. * - w: consign offset. */ state_aux0.mode = MODE_POS; + state_aux0.variant = 0; pos_aux0.cons += v8_to_v16 (args[0], args[1]); break; case c ('s', 0): /* Stop (set zero speed). */ state_main.mode = MODE_SPEED; + state_main.variant = 0; speed_theta.use_pos = speed_alpha.use_pos = 0; speed_theta.cons = 0; speed_alpha.cons = 0; @@ -274,6 +277,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - b: theta speed. * - b: alpha speed. */ state_main.mode = MODE_SPEED; + state_main.variant = 0; speed_theta.use_pos = speed_alpha.use_pos = 0; speed_theta.cons = args[0] << 8; speed_alpha.cons = args[1] << 8; @@ -282,6 +286,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Set auxiliary speed. * - b: speed. */ state_aux0.mode = MODE_SPEED; + state_aux0.variant = 0; speed_aux0.use_pos = 0; speed_aux0.cons = args[0] << 8; break; @@ -292,13 +297,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - b: sequence number. */ if (args[8] == state_main.sequence) break; - state_main.mode = MODE_SPEED; speed_theta.use_pos = speed_alpha.use_pos = 1; speed_theta.pos_cons = pos_theta.cons; speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); speed_alpha.pos_cons = pos_alpha.cons; speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]); - state_start (&state_main, args[8]); + state_start (&state_main, MODE_SPEED, args[8]); break; case c ('s', 5): /* Set auxiliary speed controlled position consign. @@ -306,11 +310,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - b: sequence number. */ if (args[4] == state_aux0.sequence) break; - state_aux0.mode = MODE_SPEED; speed_aux0.use_pos = 1; speed_aux0.pos_cons = pos_aux0.cons; speed_aux0.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); - state_start (&state_aux0, args[4]); + state_start (&state_aux0, MODE_SPEED, args[4]); break; case c ('l', 5): /* Set linear speed controlled position consign. @@ -318,12 +321,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - b: sequence number. */ if (args[4] == state_main.sequence) break; - state_main.mode = MODE_SPEED; speed_theta.use_pos = speed_alpha.use_pos = 1; speed_theta.pos_cons = pos_theta.cons; speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]); speed_alpha.pos_cons = pos_alpha.cons; - state_start (&state_main, args[4]); + state_start (&state_main, MODE_SPEED, args[4]); break; case c ('a', 5): /* Set angular speed controlled position consign. diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index 02cde731..49c7f7eb 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -99,8 +99,10 @@ pos_update (void) int16_t pid_theta, pid_alpha; int32_t diff_theta, diff_alpha; /* Update current shaft positions. */ - pos_theta.cur += counter_left_diff + counter_right_diff; - pos_alpha.cur += counter_right_diff - counter_left_diff; + if (!(state_main.variant & 1)) + pos_theta.cur += counter_left_diff + counter_right_diff; + if (!(state_main.variant & 2)) + pos_alpha.cur += counter_right_diff - counter_left_diff; /* Compute PID. */ diff_theta = pos_theta.cons - pos_theta.cur; diff_alpha = pos_alpha.cons - pos_alpha.cur; diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h index 75f2fdb1..ebfa11dd 100644 --- a/digital/asserv/src/asserv/state.h +++ b/digital/asserv/src/asserv/state.h @@ -52,6 +52,11 @@ struct state_t { /** Current motor control mode. */ uint8_t mode; + /** Control mode variant. + * Used for main motors: + * - bit 0: disable theta position control. + * - bit 1: disable alpha position control. */ + uint8_t variant; /** Sequence number of the currently processed command, should be between * 1 and 127. When a command is received on the serial port it is ignored * if its sequence number is equal to the current sequence number. In @@ -79,8 +84,10 @@ extern struct state_t state_aux0; /** Start a new command execution. */ static inline void -state_start (struct state_t *motor, uint8_t sequence) +state_start (struct state_t *motor, uint8_t mode, uint8_t sequence) { + motor->mode = mode; + motor->variant = 0; motor->sequence = sequence; motor->finished = 0; motor->blocked = 0; diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index dc7c439b..243df1c8 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -81,12 +81,11 @@ traj_angle_offset_start (int32_t angle, uint8_t seq) int32_t a = fixed_mul_f824 (angle, 2 * M_PI * (1L << 24)); uint32_t f = postrack_footing; int32_t arc = fixed_mul_f824 (f, a); - state_main.mode = MODE_SPEED; speed_theta.use_pos = speed_alpha.use_pos = 1; speed_theta.pos_cons = pos_theta.cons; speed_alpha.pos_cons = pos_alpha.cons; speed_alpha.pos_cons += arc; - state_start (&state_main, seq); + state_start (&state_main, MODE_SPEED, seq); } /** Go to the wall mode. */ @@ -127,9 +126,8 @@ traj_ftw (void) void traj_ftw_start (uint8_t seq) { - state_main.mode = MODE_TRAJ; traj_mode = TRAJ_FTW; - state_start (&state_main, seq); + state_start (&state_main, MODE_TRAJ, seq); } /** Go to the dispenser mode. */ @@ -160,9 +158,9 @@ traj_gtd (void) void traj_gtd_start (uint8_t seq) { - state_main.mode = MODE_TRAJ; traj_mode = TRAJ_GTD; - state_start (&state_main, seq); + state_start (&state_main, MODE_TRAJ, seq); + state_main.variant = 2; } /** Go to position mode. */ @@ -207,14 +205,13 @@ traj_goto (void) void traj_goto_start (uint32_t x, uint32_t y, uint8_t seq) { - state_main.mode = MODE_TRAJ; traj_mode = TRAJ_GOTO; traj_goto_x = x; traj_goto_y = y; speed_theta.use_pos = speed_alpha.use_pos = 1; speed_theta.pos_cons = pos_theta.cons; speed_alpha.pos_cons = pos_alpha.cons; - state_start (&state_main, seq); + state_start (&state_main, MODE_TRAJ, seq); } /** Go to angle mode. */ @@ -245,13 +242,12 @@ traj_goto_angle (void) void traj_goto_angle_start (uint32_t a, uint8_t seq) { - state_main.mode = MODE_TRAJ; traj_mode = TRAJ_GOTO_ANGLE; traj_goto_a = a; speed_theta.use_pos = speed_alpha.use_pos = 1; speed_theta.pos_cons = pos_theta.cons; speed_alpha.pos_cons = pos_alpha.cons; - state_start (&state_main, seq); + state_start (&state_main, MODE_TRAJ, seq); } /* Compute new speed according the defined trajectory. */ diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index db6bac41..171a6895 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -112,6 +112,7 @@ twi_proto_callback (u8 *buf, u8 size) case c ('s', 0): /* Stop (set zero speed). */ state_main.mode = MODE_SPEED; + state_main.variant = 0; speed_theta.use_pos = speed_alpha.use_pos = 0; speed_theta.cons = 0; speed_alpha.cons = 0; @@ -119,12 +120,11 @@ twi_proto_callback (u8 *buf, u8 size) case c ('l', 3): /* Set linear speed controlled position consign. * - 3b: theta consign offset. */ - state_main.mode = MODE_SPEED; speed_theta.use_pos = speed_alpha.use_pos = 1; speed_theta.pos_cons = pos_theta.cons; speed_theta.pos_cons += v8_to_v32 (0, buf[2], buf[3], buf[4]); speed_alpha.pos_cons = pos_alpha.cons; - state_start (&state_main, 0); + state_start (&state_main, MODE_SPEED, 0); break; case c ('a', 2): /* Set angular speed controlled position consign. @@ -156,13 +156,12 @@ twi_proto_callback (u8 *buf, u8 size) /* Move the arm. * - w: new position. * - b: speed. */ - state_aux0.mode = MODE_SPEED; speed_aux0.use_pos = 1; speed_aux0.pos_cons = pos_aux0.cons; speed_aux0.pos_cons += v8_to_v32 (0, 0, buf[2], buf[3]); speed_aux0.max = buf[4]; speed_aux0.slow = buf[4]; - state_start (&state_aux0, 0); + state_start (&state_aux0, MODE_SPEED, 0); break; case c ('p', x): /* Set parameters. */ -- cgit v1.2.3