summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2009-05-23 00:09:23 +0200
committerNicolas Schodet2009-05-23 00:09:23 +0200
commitd6647a90f4bfe59968cf0f94443335f2673a32f3 (patch)
tree7f51af2ed63cd149346b420928857974e561d903
parent8f3f3cea26f03d5c3979dd594ff02e7d6d49da54 (diff)
* digital/asserv, digital/io:
- added usage of center sensor.
-rw-r--r--digital/asserv/src/asserv/main.c9
-rw-r--r--digital/asserv/src/asserv/traj.c33
-rw-r--r--digital/asserv/src/asserv/traj.h3
-rw-r--r--digital/asserv/src/asserv/twi_proto.c8
-rw-r--r--digital/io/doc/proto_asserv.txt4
-rw-r--r--digital/io/src/asserv.c9
-rw-r--r--digital/io/src/top_cb.c2
7 files changed, 61 insertions, 7 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index b53e6d18..b980899e 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -391,6 +391,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
traj_ftw_start (args[0], args[1]);
break;
+ case c ('f', 3):
+ /* Go to the wall, using center with a delay.
+ * - b: 0: forward, 1: backward.
+ * - b: delay.
+ * - b: sequence number. */
+ if (args[1] == state_main.sequence)
+ break;
+ traj_ftw_start_center (args[0], args[1], args[2]);
+ break;
case c ('F', 1):
/* Go to the dispenser.
* - b: sequence number. */
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index a00a70c2..11618eca 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -93,6 +93,12 @@ static uint32_t traj_goto_x, traj_goto_y;
/** Go to angle. */
static uint32_t traj_goto_a;
+/** Use center sensor. */
+static uint8_t traj_use_center;
+
+/** Center sensor delay. */
+static uint8_t traj_center_delay;
+
/** Initialise computed factors. */
void
traj_init (void)
@@ -118,7 +124,7 @@ traj_angle_offset_start (int32_t angle, uint8_t seq)
static void
traj_ftw (void)
{
- uint8_t left, right;
+ uint8_t left, center, right;
int16_t speed;
speed = speed_theta.slow;
speed *= 256;
@@ -133,6 +139,17 @@ traj_ftw (void)
left = !IO_GET (CONTACT_BACK_LEFT_IO);
right = !IO_GET (CONTACT_BACK_RIGHT_IO);
}
+ center = 0;
+ if (traj_use_center)
+ {
+ if (!IO_GET (CONTACT_CENTER_IO))
+ {
+ if (traj_center_delay == 0)
+ center = 1;
+ else
+ traj_center_delay--;
+ }
+ }
speed_theta.use_pos = speed_alpha.use_pos = 0;
speed_theta.cons = speed;
speed_alpha.cons = 0;
@@ -145,7 +162,7 @@ traj_ftw (void)
state_main.variant = 2;
#endif
}
- else if (!left || !right)
+ else if (!center && (!left || !right))
{
#ifndef HOST
/* No angular control. */
@@ -178,6 +195,18 @@ traj_ftw_start (uint8_t backward, uint8_t seq)
{
traj_mode = TRAJ_FTW;
traj_backward = backward;
+ traj_use_center = 0;
+ state_start (&state_main, MODE_TRAJ, seq);
+}
+
+/** Start go to the wall mode, with center sensor. */
+void
+traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq)
+{
+ traj_mode = TRAJ_FTW;
+ traj_backward = backward;
+ traj_use_center = 1;
+ traj_center_delay = center_delay;
state_start (&state_main, MODE_TRAJ, seq);
}
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index 8f9c28ac..706fbdaf 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -46,6 +46,9 @@ void
traj_ftw_start (uint8_t backward, uint8_t seq);
void
+traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq);
+
+void
traj_gtd_start (uint8_t seq);
void
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index dac59f93..c4fa1f0d 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -163,11 +163,17 @@ twi_proto_callback (u8 *buf, u8 size)
* - w: angle offset. */
traj_angle_offset_start (((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8, 0);
break;
- case c ('f', 0):
+ case c ('f', 1):
/* Go to the wall.
* - b: 0: forward, 1: backward. */
traj_ftw_start (buf[2], 0);
break;
+ case c ('g', 2):
+ /* Go to the wall using center sensor with delay.
+ * - b: 0: forward, 1: backward.
+ * - b: delay. */
+ traj_ftw_start_center (buf[2], buf[3], 0);
+ break;
case c ('F', 0):
/* Go to the dispenser. */
traj_gtd_start (0);
diff --git a/digital/io/doc/proto_asserv.txt b/digital/io/doc/proto_asserv.txt
index fe21bdad..9fcf9808 100644
--- a/digital/io/doc/proto_asserv.txt
+++ b/digital/io/doc/proto_asserv.txt
@@ -73,6 +73,10 @@ This table describe the list of supported commands by the *asserv* card:
| 'f' | Move | - direction (b) | Go to the wall |
| | | | - direction: 0 = forward, 1 = backward |
+---------+-------+-----------------+------------------------------------------+
+| 'g' | Move | - direction (b) | Go to the wall, using center sensor |
+| | | - delay (b) | - direction: 0 = forward, 1 = backward |
+| | | | - delay: center sensor trigger delay |
++---------+-------+-----------------+------------------------------------------+
| 'F' | Move | None | Go to a distributor (forward movement) |
+---------+-------+-----------------+------------------------------------------+
| 'x' | Move | - x (3b) | Go to an absolute position |
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index 6f849054..0853a11b 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -503,7 +503,7 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward)
asserv_twi_send_command ('X', 9);
}
-/* Go to the wall (moving backward). */
+/* Go to the wall. */
void
asserv_go_to_the_wall (uint8_t backward)
{
@@ -517,8 +517,11 @@ asserv_go_to_the_wall (uint8_t backward)
void
asserv_go_to_distributor (void)
{
- /* Send the go the distributor command to the asserv board */
- asserv_twi_send_command ('F', 0);
+ /* Put direction and delay as parameters */
+ asserv_twi_buffer_param[0] = 0;
+ asserv_twi_buffer_param[1] = 25;
+ /* Send the go the wall command to the asserv board */
+ asserv_twi_send_command ('g', 2);
}
/* Move the arm. */
diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c
index ce6d6db4..0530ae97 100644
--- a/digital/io/src/top_cb.c
+++ b/digital/io/src/top_cb.c
@@ -743,7 +743,7 @@ top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_succeed (void)
/* Open cylinder. */
cylinder_close_order = 0;
/* Fuck the distributor. */
- asserv_go_to_the_wall (0);
+ asserv_go_to_distributor ();
return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_succeed);
}