summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv/src')
-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
4 files changed, 50 insertions, 3 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);