summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-20 19:17:07 +0200
committerNicolas Schodet2008-04-20 19:17:07 +0200
commitc4d20dd9ffa1c479de2858dd1b77ff014d1073dd (patch)
treeee30f90e46f60c15ab69514f6889671c134f745d /digital/asserv
parent1d6b510b0394562c6e9d52990986b4ddbd0263c5 (diff)
* digital/asserv/src/asserv:
- added separate setting of position control for theta/alpha. - disable angular position control for go to the dispenser.
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/main.c14
-rw-r--r--digital/asserv/src/asserv/pos.c6
-rw-r--r--digital/asserv/src/asserv/state.h9
-rw-r--r--digital/asserv/src/asserv/traj.c16
-rw-r--r--digital/asserv/src/asserv/twi_proto.c7
5 files changed, 29 insertions, 23 deletions
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. */