summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2008-03-25 23:04:07 +0100
committerNicolas Schodet2008-03-25 23:04:07 +0100
commit1ea652e0d5185aee5ca60182302d7bb2d4056b46 (patch)
treea7a7b538b534a55c8f796448ffa72512d659e297 /digital
parent5b4cd3acddcf2ef7ad5c383fb93a0c27718f4da3 (diff)
* digital/asserv/src/asserv:
- better blocked state handling, now deactivate all motor control on blocking.
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/main.c7
-rw-r--r--digital/asserv/src/asserv/pos.c13
-rw-r--r--digital/asserv/src/asserv/state.h10
3 files changed, 15 insertions, 15 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 38a2e9f8..e96ebf90 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -326,18 +326,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Set both acknoledge.
* - b: main ack sequence number.
* - b: auxiliary ack sequence number. */
- if (state_aux0.blocked && args[1] == state_aux0.finished)
- pos_reset (&pos_aux0);
state_acknowledge (&state_aux0, args[1]);
/* no break; */
case c ('a', 1):
/* Set main acknoledge.
* - b: main ack sequence number. */
- if (state_main.blocked && args[0] == state_main.finished)
- {
- pos_reset (&pos_theta);
- pos_reset (&pos_alpha);
- }
state_acknowledge (&state_main, args[0]);
break;
/* Stats.
diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
index 091335e2..6c317766 100644
--- a/digital/asserv/src/asserv/pos.c
+++ b/digital/asserv/src/asserv/pos.c
@@ -101,14 +101,15 @@ pos_update (void)
/* Compute PID. */
diff_theta = pos_theta.cons - pos_theta.cur;
diff_alpha = pos_alpha.cons - pos_alpha.cur;
- if (state_main.blocked
- || diff_theta < -pos_blocked || pos_blocked < diff_theta
+ if (diff_theta < -pos_blocked || pos_blocked < diff_theta
|| diff_alpha < -pos_blocked || pos_blocked < diff_alpha)
{
/* Blocked. */
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
+ state_blocked (&state_main);
pwm_left = 0;
pwm_right = 0;
- state_blocked (&state_main);
}
else
{
@@ -129,12 +130,12 @@ pos_update (void)
pos_aux0.cur += counter_aux0_diff;
/* Compute PID. */
diff = pos_aux0.cons - pos_aux0.cur;
- if (state_aux0.blocked
- || diff < -pos_blocked || pos_blocked < diff)
+ if (diff < -pos_blocked || pos_blocked < diff)
{
/* Blocked. */
- pwm_aux0 = 0;
+ pos_reset (&pos_aux0);
state_blocked (&state_aux0);
+ pwm_aux0 = 0;
}
else
{
diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h
index 49ae1e60..2a5e642c 100644
--- a/digital/asserv/src/asserv/state.h
+++ b/digital/asserv/src/asserv/state.h
@@ -83,6 +83,7 @@ state_start (struct state_t *motor, uint8_t sequence)
{
motor->sequence = sequence;
motor->finished = 0;
+ motor->blocked = 0;
}
/** Signal the current command completion. */
@@ -93,21 +94,26 @@ state_finish (struct state_t *motor)
motor->finished = 1;
}
-/** Signal the current command completion. */
+/** Signal the current command is blocked, disable motor control until a new
+ * command is given. */
static inline void
state_blocked (struct state_t *motor)
{
motor->sequence_finish = motor->sequence | 0x80;
motor->blocked = 1;
+ motor->mode = MODE_PWM;
}
-/** Acknowledge a command completion. */
+/** Acknowledge a command completion and blocked state. */
static inline void
state_acknowledge (struct state_t *motor, uint8_t sequence)
{
motor->sequence_ack = sequence;
if (sequence == motor->sequence_finish)
+ {
+ motor->finished = 0;
motor->blocked = 0;
+ }
}
#endif /* state_h */