From 7566b7f7da95df90235d3b35617b2bc0aeab980f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 4 May 2010 02:25:01 +0200 Subject: digital/mimot/src/dirty: find zero by pushing on hardware limits See r1070. --- digital/mimot/src/dirty/aux.c | 36 ++++++++++++++++++++++++++++++++++++ digital/mimot/src/dirty/aux.h | 5 +++++ digital/mimot/src/dirty/main.c | 2 +- digital/mimot/src/dirty/twi_proto.c | 4 ++-- 4 files changed, 44 insertions(+), 3 deletions(-) (limited to 'digital/mimot/src') 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. */ @@ -62,6 +64,9 @@ aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq); 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); 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. */ -- cgit v1.2.3