summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2009-05-07 22:39:46 +0200
committerNicolas Schodet2009-05-07 22:39:46 +0200
commite5955adc9c33cee7d2fdda723fb02a2af5e612be (patch)
tree55f63d8ea3ae235d09b15d75fea685bedac85c20
parent362824bd00aad8e470baceef83f1c7b24dec6656 (diff)
* digital/asserv:
- added forward go to the wall. - use defines for contacts.
-rw-r--r--digital/asserv/src/asserv/aux.c10
-rw-r--r--digital/asserv/src/asserv/contacts.h37
-rw-r--r--digital/asserv/src/asserv/main.c7
-rw-r--r--digital/asserv/src/asserv/simu.host.c5
-rw-r--r--digital/asserv/src/asserv/traj.c32
-rw-r--r--digital/asserv/src/asserv/traj.h2
-rw-r--r--digital/asserv/src/asserv/twi_proto.c5
-rw-r--r--digital/asserv/tools/asserv/asserv.py6
-rw-r--r--digital/asserv/tools/inter_asserv.py6
-rw-r--r--digital/io/doc/proto_asserv.txt3
-rw-r--r--digital/io/src/asserv.c4
11 files changed, 97 insertions, 20 deletions
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. */