summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-02 10:08:38 +0200
committerNicolas Schodet2010-05-02 10:08:38 +0200
commitd261d97b1497f67503faa04b83a999848381a791 (patch)
tree9c6ee0af0148dd45b49c2da5d249dd3eefd0ca82
parent756ced5e37dcb240bcbfa4faf94153b38327d026 (diff)
digital/asserv/src/asserv: find zero by pushing on hardware limits
-rw-r--r--digital/asserv/src/asserv/aux.c38
-rw-r--r--digital/asserv/src/asserv/aux.h5
-rw-r--r--digital/asserv/src/asserv/main.c2
-rw-r--r--digital/asserv/src/asserv/twi_proto.c8
4 files changed, 47 insertions, 6 deletions
diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c
index 22df6a6d..a5a65a8d 100644
--- a/digital/asserv/src/asserv/aux.c
+++ b/digital/asserv/src/asserv/aux.c
@@ -33,6 +33,7 @@
#include "counter.h"
#include "pos.h"
#include "speed.h"
+#include "pwm.h"
#include "contacts.h"
@@ -54,6 +55,8 @@ enum
AUX_TRAJ_FIND_ZERO_NOT,
/* Find zero mode, turn until zero is seen. */
AUX_TRAJ_FIND_ZERO,
+ /* Find zero by forcing into limit. */
+ AUX_TRAJ_FIND_LIMIT,
/* Everything done. */
AUX_TRAJ_DONE,
};
@@ -64,11 +67,13 @@ aux_init (void)
{
aux[0].state = &state_aux[0];
aux[0].speed = &speed_aux[0];
+ aux[0].pwm = &pwm_aux[0];
aux[0].zero_pin = &IO_PIN (CONTACT_AUX0_ZERO_IO);
aux[0].zero_bv = IO_BV (CONTACT_AUX0_ZERO_IO);
- aux[0].handle_blocking = 1;
+ aux[0].handle_blocking = 0;
aux[1].state = &state_aux[1];
aux[1].speed = &speed_aux[1];
+ aux[1].pwm = &pwm_aux[1];
aux[1].zero_pin = &IO_PIN (CONTACT_AUX1_ZERO_IO);
aux[1].zero_bv = IO_BV (CONTACT_AUX1_ZERO_IO);
aux[1].handle_blocking = 0;
@@ -163,6 +168,34 @@ aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq)
state_start (aux->state, MODE_TRAJ, seq);
}
+/** Find limit mode. */
+void
+aux_traj_find_limit (struct aux_t *aux)
+{
+ /* If blocking, limit is found. */
+ if (aux->speed->pos->blocked_counter
+ > aux->speed->pos->blocked_counter_limit)
+ {
+ state_finish (aux->state);
+ pos_reset (aux->speed->pos);
+ aux->state->mode = MODE_PWM;
+ pwm_set (aux->pwm, 0);
+ aux->pos = 0;
+ aux->traj_mode = AUX_TRAJ_DONE;
+ }
+}
+
+/** Start find limit mode. */
+void
+aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq)
+{
+ aux->traj_mode = AUX_TRAJ_FIND_LIMIT;
+ aux->speed->use_pos = 0;
+ aux->speed->cons = speed << 8;
+ state_start (aux->state, MODE_TRAJ, seq);
+ aux->state->variant = 4;
+}
+
/** Update trajectories for one motor. */
static void
aux_traj_update_single (struct aux_t *aux)
@@ -179,6 +212,9 @@ aux_traj_update_single (struct aux_t *aux)
case AUX_TRAJ_FIND_ZERO:
aux_traj_find_zero (aux);
break;
+ case AUX_TRAJ_FIND_LIMIT:
+ aux_traj_find_limit (aux);
+ break;
case AUX_TRAJ_DONE:
break;
}
diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h
index 65456ec9..b541f3eb 100644
--- a/digital/asserv/src/asserv/aux.h
+++ b/digital/asserv/src/asserv/aux.h
@@ -32,6 +32,8 @@ struct aux_t
struct state_t *state;
/** Controlled speed. */
struct speed_t *speed;
+ /** Associated PWM. */
+ struct pwm_t *pwm;
/** Absolute position. */
int16_t pos;
/** Trajectory mode. */
@@ -63,6 +65,9 @@ void
aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq);
void
+aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq);
+
+void
aux_traj_update (void);
#endif /* aux_h */
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index b980899e..8660854e 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -460,7 +460,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
if (!auxp) { proto_send0 ('?'); return; }
if (args[2] == state->sequence)
break;
- aux_traj_find_zero_start (auxp, args[1], args[2]);
+ aux_traj_find_limit_start (auxp, args[1], args[2]);
break;
case c ('a', 3):
/* Set all acknoledge.
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index c4fa1f0d..3602a1b6 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -211,9 +211,9 @@ twi_proto_callback (u8 *buf, u8 size)
aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0);
break;
case c ('B', 1):
- /* Find the zero position of the arm.
+ /* Find the aux0 zero position.
* - b: speed. */
- aux_traj_find_zero_start (&aux[0], buf[2], 0);
+ aux_traj_find_limit_start (&aux[0], buf[2], 0);
break;
case c ('c', 3):
/* Move the aux1.
@@ -223,9 +223,9 @@ twi_proto_callback (u8 *buf, u8 size)
aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]), 0);
break;
case c ('C', 1):
- /* Find the zero position of the aux1.
+ /* Find the aux0 zero position.
* - b: speed. */
- aux_traj_find_zero_start (&aux[1], buf[2], 0);
+ aux_traj_find_limit_start (&aux[1], buf[2], 0);
break;
case c ('p', x):
/* Set parameters. */