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 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/aux.h | 8 ++++ digital/asserv/src/asserv/init | 4 +- digital/asserv/src/asserv/main.c | 9 ++++ digital/asserv/src/asserv/simu.host.c | 6 +++ digital/asserv/src/asserv/twi_proto.c | 6 +++ 6 files changed, 109 insertions(+), 2 deletions(-) (limited to 'digital/asserv') 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; + } +} diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h index 175933cc..3dc3c6ee 100644 --- a/digital/asserv/src/asserv/aux.h +++ b/digital/asserv/src/asserv/aux.h @@ -30,6 +30,8 @@ struct aux_t { /** Absolute position. */ int16_t pos; + /** Trajectory mode. */ + uint8_t traj_mode; }; extern struct aux_t aux0; @@ -40,4 +42,10 @@ aux_pos_update (void); void aux_traj_goto_start (uint16_t pos, uint8_t seq); +void +aux_traj_find_zero_start (uint8_t seq); + +void +aux_traj_update (void); + #endif /* aux_h */ diff --git a/digital/asserv/src/asserv/init b/digital/asserv/src/asserv/init index 7937e666..8bfa283e 100644 --- a/digital/asserv/src/asserv/init +++ b/digital/asserv/src/asserv/init @@ -5,10 +5,10 @@ !p'E03ff !p'D01ff !p'a00800080 -!p's60406040 +!p's60602020 !p'p0100 !p'i0000 !p'd1000 !p'a0100 -!p's1010 +!p's1002 !Aff diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 8be5373f..47b2af2f 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -137,6 +137,8 @@ main_loop (void) /* Compute trajectory. */ if (state_main.mode >= MODE_TRAJ) traj_update (); + if (state_aux0.mode >= MODE_TRAJ) + aux_traj_update (); /* Speed control. */ speed_update (); main_timer[3] = timer_read (); @@ -393,6 +395,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; aux_traj_goto_start (v8_to_v16 (args[0], args[1]), args[2]); break; + case c ('y', 1): + /* Auxiliary find zero. + * - b: sequence number. */ + if (args[0] == state_aux0.sequence) + break; + aux_traj_find_zero_start (args[0]); + break; case c ('a', 2): /* Set both acknoledge. * - b: main ack sequence number. diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 8b499123..64fc38db 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -131,6 +131,7 @@ simu_pos_update (double dl, double dr, double footing) static void simu_sensor_update (void) { + /** Micro-switch sensors. */ static const double sensors[][2] = { { -70.0, 200.0 }, @@ -153,6 +154,11 @@ simu_sensor_update (void) PINC |= bit; bit <<= 1; } + /** Top zero sensor. */ + double aa = simu_aux0_model.th / simu_aux0_model.m.i_G * 3; + if (!(cos (aa) > 0 && fabs (sin (aa)) * 80.0 < 7.5)) + PINC |= bit; + bit <<= 1; } /** Do a simulation step. */ diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index ffc55061..809f5112 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -170,6 +170,12 @@ twi_proto_callback (u8 *buf, u8 size) speed_aux0.max = buf[4]; aux_traj_goto_start (v8_to_v16 (buf[2], buf[3]), 0); break; + case c ('B', 1): + /* Find the zero position of the arm. + * - b: speed. */ + speed_aux0.max = buf[2]; + aux_traj_find_zero_start (0); + break; case c ('p', x): /* Set parameters. */ if (twi_proto_params (&buf[2], size - 2) != 0) -- cgit v1.2.3