From e5955adc9c33cee7d2fdda723fb02a2af5e612be Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 7 May 2009 22:39:46 +0200 Subject: * digital/asserv: - added forward go to the wall. - use defines for contacts. --- digital/asserv/src/asserv/aux.c | 10 ++++++---- digital/asserv/src/asserv/contacts.h | 37 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/main.c | 7 ++++--- digital/asserv/src/asserv/simu.host.c | 5 ++++- digital/asserv/src/asserv/traj.c | 32 +++++++++++++++++++++++------- digital/asserv/src/asserv/traj.h | 2 +- digital/asserv/src/asserv/twi_proto.c | 5 +++-- digital/asserv/tools/asserv/asserv.py | 6 ++++++ digital/asserv/tools/inter_asserv.py | 6 ++++++ digital/io/doc/proto_asserv.txt | 3 ++- digital/io/src/asserv.c | 4 +++- 11 files changed, 97 insertions(+), 20 deletions(-) create mode 100644 digital/asserv/src/asserv/contacts.h (limited to 'digital') diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c index f8665fbb..f7c48b7a 100644 --- a/digital/asserv/src/asserv/aux.c +++ b/digital/asserv/src/asserv/aux.c @@ -34,6 +34,8 @@ #include "pos.h" #include "speed.h" +#include "contacts.h" + #ifdef HOST # include "simu.host.h" #endif @@ -64,12 +66,12 @@ aux_init (void) { aux[0].state = &state_aux[0]; aux[0].speed = &speed_aux[0]; - aux[0].zero_pin = &PINC; - aux[0].zero_bv = _BV (5); + aux[0].zero_pin = &IO_PIN (CONTACT_AUX0_ZERO_IO); + aux[0].zero_bv = IO_BV (CONTACT_AUX0_ZERO_IO); aux[1].state = &state_aux[1]; aux[1].speed = &speed_aux[1]; - aux[1].zero_pin = &PINC; - aux[1].zero_bv = _BV (6); + aux[1].zero_pin = &IO_PIN (CONTACT_AUX1_ZERO_IO); + aux[1].zero_bv = IO_BV (CONTACT_AUX1_ZERO_IO); } /** Update positions. */ diff --git a/digital/asserv/src/asserv/contacts.h b/digital/asserv/src/asserv/contacts.h new file mode 100644 index 00000000..0d36db29 --- /dev/null +++ b/digital/asserv/src/asserv/contacts.h @@ -0,0 +1,37 @@ +#ifndef contacts_h +#define contacts_h +/* contacts.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2009 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Define contacts. */ +#define CONTACT_BACK_LEFT_IO C, 0 +#define CONTACT_BACK_RIGHT_IO C, 1 +#define CONTACT_FRONT_LEFT_IO C, 2 +#define CONTACT_FRONT_RIGHT_IO C, 3 +#define CONTACT_CENTER_IO C, 4 +#define CONTACT_AUX0_ZERO_IO C, 5 +#define CONTACT_AUX1_ZERO_IO C, 6 + +#endif /* contacts_h */ diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 527b1940..983995f5 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -383,12 +383,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) speed->pos_cons += v8_to_v32 (args[1], args[2], args[3], args[4]); state_start (state, MODE_SPEED, args[5]); break; - case c ('f', 1): + case c ('f', 2): /* Go to the wall. + * - b: 0: forward, 1: backward. * - b: sequence number. */ - if (args[0] == state_main.sequence) + if (args[1] == state_main.sequence) break; - traj_ftw_start (args[0]); + traj_ftw_start (args[0], args[1]); break; case c ('F', 1): /* Go to the dispenser. diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 0570729a..f89fabd4 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -40,6 +40,8 @@ #include "pwm.h" #include "aux.h" +#include "contacts.h" + #include "motor_model.host.h" #include "models.host.h" @@ -149,7 +151,8 @@ simu_sensor_update (void) { 170.0, 0.0 }, }; static const uint8_t sensors_bit[] = - { _BV (0), _BV (1), _BV (3), }; + { IO_BV (CONTACT_BACK_LEFT_IO), IO_BV (CONTACT_BACK_RIGHT_IO), + IO_BV (CONTACT_CENTER_IO), }; static const double table_width = 3000.0, table_height = 2100.0; PINC = 0; unsigned int i; diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 810e1aa5..6e7c2bc7 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -38,6 +38,8 @@ #include "speed.h" #include "postrack.h" +#include "contacts.h" + #ifdef HOST # include "simu.host.h" #endif @@ -82,6 +84,9 @@ uint16_t traj_angle_limit = 0x2000; /** Angle at which to start going forward (rad, f8.24). */ int32_t traj_angle_limit_rad; +/** Go backward. */ +static uint8_t traj_backward; + /** Go to position. */ static uint32_t traj_goto_x, traj_goto_y; @@ -116,26 +121,38 @@ traj_angle_offset_start (int32_t angle, uint8_t seq) static void traj_ftw (void) { + uint8_t left, right; int16_t speed; speed = speed_theta.slow; speed *= 256; + if (!traj_backward) + { + left = !IO_GET (CONTACT_FRONT_LEFT_IO); + right = !IO_GET (CONTACT_FRONT_RIGHT_IO); + } + else + { + speed = -speed; + left = !IO_GET (CONTACT_BACK_LEFT_IO); + right = !IO_GET (CONTACT_BACK_RIGHT_IO); + } speed_theta.use_pos = speed_alpha.use_pos = 0; - speed_theta.cons = -speed; + speed_theta.cons = speed; speed_alpha.cons = 0; state_main.variant = 0; - if (PINC & _BV (0) && PINC & _BV (1)) + if (!left && !right) { /* Backward. */ } - else if (PINC & _BV (0) || PINC & _BV (1)) + else if (!left || !right) { #ifndef HOST /* No angular control. */ state_main.variant = 2; #else /* On host, we must do the job. */ - speed_theta.cons = -speed / 2; - if (PINC & _BV (0)) + speed_theta.cons = speed / 2; + if (left) speed_alpha.cons = speed / 2; else speed_alpha.cons = -speed / 2; @@ -156,9 +173,10 @@ traj_ftw (void) /** Start go to the wall mode. */ void -traj_ftw_start (uint8_t seq) +traj_ftw_start (uint8_t backward, uint8_t seq) { traj_mode = TRAJ_FTW; + traj_backward = backward; state_start (&state_main, MODE_TRAJ, seq); } @@ -170,7 +188,7 @@ traj_gtd (void) speed = speed_theta.slow; speed *= 256; speed_theta.use_pos = speed_alpha.use_pos = 0; - if (PINC & _BV (3)) + if (IO_GET (CONTACT_CENTER_IO)) { speed_theta.cons = speed; speed_alpha.cons = 0; diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index 2e5469c1..c360ea2e 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -40,7 +40,7 @@ void traj_angle_offset_start (int32_t angle, uint8_t seq); void -traj_ftw_start (uint8_t seq); +traj_ftw_start (uint8_t backward, uint8_t seq); void traj_gtd_start (uint8_t seq); diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index d93cfa18..10c49eeb 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -143,8 +143,9 @@ twi_proto_callback (u8 *buf, u8 size) traj_angle_offset_start (((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8, 0); break; case c ('f', 0): - /* Go to the wall. */ - traj_ftw_start (0); + /* Go to the wall. + * - b: 0: forward, 1: backward. */ + traj_ftw_start (buf[2], 0); break; case c ('F', 0): /* Go to the dispenser. */ diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py index 5c7830c6..a8b5c753 100644 --- a/digital/asserv/tools/asserv/asserv.py +++ b/digital/asserv/tools/asserv/asserv.py @@ -246,6 +246,12 @@ class Proto: self._angle_f16 (a), self.mseq) self.wait (self.finished, auto = True) + def ftw (self, backward = True): + """Go to the wall.""" + self.mseq += 1 + self.proto.send ('f', 'BB', backward and 1 or 0, self.mseq) + self.wait (self.finished, auto = True) + def set_simu_pos (self, x, y, a): """Set simulated position.""" self.proto.send ('h', 'chhh', 'X', int (round (x)), int (round (y)), diff --git a/digital/asserv/tools/inter_asserv.py b/digital/asserv/tools/inter_asserv.py index a3af95ea..144c79a3 100644 --- a/digital/asserv/tools/inter_asserv.py +++ b/digital/asserv/tools/inter_asserv.py @@ -71,6 +71,9 @@ class InterAsserv (Inter): self.backwardOkButton = Checkbutton (self.rightFrame, text = 'backward ok', variable = self.backwardOkVar) self.backwardOkButton.pack () + self.ftwButton = Button (self.rightFrame, text = 'FTW', + command = self.ftw) + self.ftwButton.pack () self.tableview.configure (cursor = 'crosshair') self.tableview.bind ('<1>', self.button1) @@ -115,6 +118,9 @@ class InterAsserv (Inter): def emergency_stop (self): self.a.free () + def ftw (self): + self.a.ftw (self.backwardOkVar.get ()) + if __name__ == '__main__': import sys app = InterAsserv (sys.argv[1:]) diff --git a/digital/io/doc/proto_asserv.txt b/digital/io/doc/proto_asserv.txt index 391950dd..e6e43c5b 100644 --- a/digital/io/doc/proto_asserv.txt +++ b/digital/io/doc/proto_asserv.txt @@ -70,7 +70,8 @@ This table describe the list of supported commands by the *asserv* card: +---------+-------+-----------------+------------------------------------------+ | 'a' | Move | - angle (2b) | Angular move (0 <=> 0° ; 65536 <=> 360) | +---------+-------+-----------------+------------------------------------------+ -| 'f' | Move | None | Go to the wall (backward movement) | +| 'f' | Move | - direction (b) | Go to the wall | +| | | | - direction: 0 = forward, 1 = backward | +---------+-------+-----------------+------------------------------------------+ | 'F' | Move | None | Go to a distributor (forward movement) | +---------+-------+-----------------+------------------------------------------+ diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index a3175176..e6cbc1b4 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -455,8 +455,10 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a) void asserv_go_to_the_wall (void) { + /* Put direction as parameters */ + asserv_twi_buffer_param[0] = 1; /* Send the go the wall command to the asserv board */ - asserv_twi_send_command ('f', 0); + asserv_twi_send_command ('f', 1); } /* Move forward to approach a ditributor. */ -- cgit v1.2.3