summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-04 02:25:01 +0200
committerNicolas Schodet2010-05-04 02:25:01 +0200
commit7566b7f7da95df90235d3b35617b2bc0aeab980f (patch)
tree69d927049303de687fd44bb3335dfb1606006163 /digital
parent50b8abe7f025cd2a4a498c24da37826a6e2b585a (diff)
digital/mimot/src/dirty: find zero by pushing on hardware limits
See r1070.
Diffstat (limited to 'digital')
-rw-r--r--digital/mimot/src/dirty/aux.c36
-rw-r--r--digital/mimot/src/dirty/aux.h5
-rw-r--r--digital/mimot/src/dirty/main.c2
-rw-r--r--digital/mimot/src/dirty/twi_proto.c4
4 files changed, 44 insertions, 3 deletions
diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c
index d262cba0..a5a65a8d 100644
--- a/digital/mimot/src/dirty/aux.c
+++ b/digital/mimot/src/dirty/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 = 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/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h
index 65456ec9..b541f3eb 100644
--- a/digital/mimot/src/dirty/aux.h
+++ b/digital/mimot/src/dirty/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/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c
index 9b674461..d826dd98 100644
--- a/digital/mimot/src/dirty/main.c
+++ b/digital/mimot/src/dirty/main.c
@@ -283,7 +283,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', 2):
/* Set all acknoledge.
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
index 614536e6..37c1b64f 100644
--- a/digital/mimot/src/dirty/twi_proto.c
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -125,7 +125,7 @@ twi_proto_callback (u8 *buf, u8 size)
case c ('B', 1):
/* Find the zero position of the aux0.
* - 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.
@@ -137,7 +137,7 @@ twi_proto_callback (u8 *buf, u8 size)
case c ('C', 1):
/* Find the zero position of the aux1.
* - 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. */