From 19e614348483e09ddb2acca5fcf905ec0c582625 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 21 Apr 2008 00:30:41 +0200 Subject: * digital/asserv/src/asserv: - added find zero auxiliary command. --- digital/asserv/src/asserv/aux.c | 78 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) (limited to 'digital/asserv/src/asserv/aux.c') diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c index 880496b4..562bcffd 100644 --- a/digital/asserv/src/asserv/aux.c +++ b/digital/asserv/src/asserv/aux.c @@ -25,15 +25,34 @@ #include "common.h" #include "aux.h" +#include "io.h" + #include "state.h" #include "counter.h" #include "pos.h" #include "speed.h" +#ifdef HOST +# include "simu.host.h" +#endif + /** Motor state. */ struct aux_t aux0; +/** Trajectory modes. */ +enum +{ + /* Find zero mode, first start at full speed to detect a arm... */ + AUX_TRAJ_FIND_ZERO_START, + /* ...then go on until it is not seen any more... */ + AUX_TRAJ_FIND_ZERO_SLOW, + /* ...finally, go backward until it is seen. */ + AUX_TRAJ_FIND_ZERO_BACK, + /* Everything done. */ + AUX_TRAJ_DONE, +}; + /** Update positions. */ void aux_pos_update (void) @@ -52,3 +71,62 @@ aux_traj_goto_start (uint16_t pos, uint8_t seq) state_start (&state_aux0, MODE_SPEED, seq); } +/** Find zero mode. */ +void +aux_traj_find_zero (void) +{ + switch (aux0.traj_mode) + { + case AUX_TRAJ_FIND_ZERO_START: + if (!(PINC & _BV (3))) + { + speed_aux0.cons = speed_aux0.max << 8; + aux0.traj_mode = AUX_TRAJ_FIND_ZERO_SLOW; + } + break; + case AUX_TRAJ_FIND_ZERO_SLOW: + if (PINC & _BV (3)) + { + speed_aux0.cons = -speed_aux0.slow << 8; + aux0.traj_mode = AUX_TRAJ_FIND_ZERO_BACK; + } + break; + case AUX_TRAJ_FIND_ZERO_BACK: + if (!(PINC & _BV (3))) + { + speed_aux0.cons = 0; + state_finish (&state_aux0); + aux0.pos = 0; + aux0.traj_mode = AUX_TRAJ_DONE; + } + break; + default: + assert (0); + } +} + +/** Start find zero mode. */ +void +aux_traj_find_zero_start (uint8_t seq) +{ + aux0.traj_mode = AUX_TRAJ_FIND_ZERO_START; + speed_aux0.use_pos = 0; + speed_aux0.cons = speed_aux0.max << 8; + state_start (&state_aux0, MODE_TRAJ, seq); +} + +/** Update trajectories. */ +void +aux_traj_update (void) +{ + switch (aux0.traj_mode) + { + case AUX_TRAJ_FIND_ZERO_START: + case AUX_TRAJ_FIND_ZERO_SLOW: + case AUX_TRAJ_FIND_ZERO_BACK: + aux_traj_find_zero (); + break; + case AUX_TRAJ_DONE: + break; + } +} -- cgit v1.2.3