summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-19 23:24:08 +0200
committerNicolas Schodet2011-05-19 23:29:49 +0200
commitf753bc39682f980feb49b7ff1bb9e9594f986e6b (patch)
treeedda6060cfd233985c4804d054f712163c182837
parent453c275fb70db0bdd7258c3bedbd8897fa0b35cc (diff)
digital/asserv: add push the wall traj mode, closes #167
-rw-r--r--digital/ai/src/twi_master/asserv.c22
-rw-r--r--digital/ai/src/twi_master/asserv.h6
-rw-r--r--digital/asserv/src/asserv/main.c21
-rw-r--r--digital/asserv/src/asserv/traj.c57
-rw-r--r--digital/asserv/src/asserv/traj.h4
-rw-r--r--digital/asserv/src/asserv/twi_proto.c18
-rw-r--r--digital/asserv/tools/asserv/asserv.py19
7 files changed, 145 insertions, 2 deletions
diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c
index d9ae9330..839b1d23 100644
--- a/digital/ai/src/twi_master/asserv.c
+++ b/digital/ai/src/twi_master/asserv.c
@@ -301,6 +301,28 @@ asserv_go_to_the_wall (uint8_t backward)
}
void
+asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y,
+ uint16_t init_a)
+{
+ if (init_x != (uint32_t) -1)
+ init_x = fixed_mul_f824 (init_x, asserv_scale_inv);
+ if (init_y != (uint32_t) -1)
+ init_y = fixed_mul_f824 (init_y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'G';
+ buffer[1] = backward;
+ buffer[2] = v32_to_v8 (init_x, 2);
+ buffer[3] = v32_to_v8 (init_x, 1);
+ buffer[4] = v32_to_v8 (init_x, 0);
+ buffer[5] = v32_to_v8 (init_y, 2);
+ buffer[6] = v32_to_v8 (init_y, 1);
+ buffer[7] = v32_to_v8 (init_y, 0);
+ buffer[8] = v16_to_v8 (init_a, 1);
+ buffer[9] = v16_to_v8 (init_a, 0);
+ twi_master_send_buffer (10);
+}
+
+void
asserv_move_motor0_absolute (uint16_t position, uint8_t speed)
{
uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h
index 2aa7a6f8..c9a52a11 100644
--- a/digital/ai/src/twi_master/asserv.h
+++ b/digital/ai/src/twi_master/asserv.h
@@ -198,6 +198,12 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward);
void
asserv_go_to_the_wall (uint8_t backward);
+/** Push the wall and initialise position. Use -1 for coordinates to keep
+ * unchanged. */
+void
+asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y,
+ uint16_t init_a);
+
/**
* Move the motor0.
* Motor0 class command.
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 2d53e2fa..677fea84 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -400,6 +400,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
traj_ftw_start_center (args[0], args[1], args[2]);
break;
+ case c ('f', 12):
+ /* Push the wall.
+ * - b: 0: forward, 1: backward.
+ * - d: init_x, f24.8.
+ * - d: init_y, f24.8.
+ * - w: init_a, f0.16.
+ * - b: sequence number. */
+ {
+ if (args[11] == state_main.sequence)
+ break;
+ int32_t angle;
+ if (args[9] == 0xff && args[10] == 0xff)
+ angle = -1;
+ else
+ angle = v8_to_v32 (0, args[9], args[10], 0);
+ traj_ptw_start (args[0],
+ v8_to_v32 (args[1], args[2], args[3], args[4]),
+ v8_to_v32 (args[5], args[6], args[7], args[8]),
+ angle, args[11]);
+ }
+ 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 11618eca..e147ad5d 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -37,6 +37,7 @@
#include "pos.h"
#include "speed.h"
#include "postrack.h"
+#include "pwm.h"
#include "contacts.h"
@@ -59,6 +60,8 @@ enum
TRAJ_FTW,
/* Go to the dispenser. */
TRAJ_GTD,
+ /* Push the wall. */
+ TRAJ_PTW,
/* Go to position. */
TRAJ_GOTO,
/* Go to angle. */
@@ -99,6 +102,9 @@ static uint8_t traj_use_center;
/** Center sensor delay. */
static uint8_t traj_center_delay;
+/** Initial values for x, y and angle, or -1. */
+static int32_t traj_init_x, traj_init_y, traj_init_a;
+
/** Initialise computed factors. */
void
traj_init (void)
@@ -210,6 +216,54 @@ traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq)
state_start (&state_main, MODE_TRAJ, seq);
}
+/** Push the wall mode. */
+static void
+traj_ptw (void)
+{
+ /* If blocking, the wall was found. */
+ if (pos_theta.blocked_counter >= pos_theta.blocked_counter_limit)
+ {
+ /* Initialise position. */
+ if (traj_init_x != -1)
+ postrack_x = traj_init_x;
+ if (traj_init_y != -1)
+ postrack_y = traj_init_y;
+ if (traj_init_a != -1)
+ postrack_a = traj_init_a;
+ /* Stop motor control. */
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
+ state_main.variant = 0;
+ state_main.mode = MODE_PWM;
+ pwm_set (&pwm_left, 0);
+ pwm_set (&pwm_right, 0);
+ state_finish (&state_main);
+ traj_mode = TRAJ_DONE;
+ }
+}
+
+/** Start push the wall mode. Position is initialised unless -1. */
+void
+traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y,
+ int32_t init_a, uint8_t seq)
+{
+ int16_t speed;
+ traj_mode = TRAJ_PTW;
+ traj_init_x = init_x;
+ traj_init_y = init_y;
+ traj_init_a = init_a;
+ state_start (&state_main, MODE_TRAJ, seq);
+ /* Use slow speed, without alpha control. */
+ speed = speed_theta.slow;
+ speed *= 256;
+ if (backward)
+ speed = -speed;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = speed;
+ speed_alpha.cons = 0;
+ state_main.variant = 2;
+}
+
/** Go to the dispenser mode. */
static void
traj_gtd (void)
@@ -392,6 +446,9 @@ traj_update (void)
case TRAJ_FTW:
traj_ftw ();
break;
+ case TRAJ_PTW:
+ traj_ptw ();
+ break;
case TRAJ_GTD:
traj_gtd ();
break;
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index 706fbdaf..ec345d0c 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -49,6 +49,10 @@ void
traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq);
void
+traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y,
+ int32_t init_a, 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 ff15c82f..fcf0a9cc 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -168,6 +168,24 @@ twi_proto_callback (u8 *buf, u8 size)
* - b: 0: forward, 1: backward. */
traj_ftw_start (buf[2], 0);
break;
+ case c ('G', 9):
+ /* Push the wall.
+ * - b: 0: forward, 1: backward.
+ * - 3b: init_x.
+ * - 3b: init_y.
+ * - w: init_a. */
+ {
+ int32_t angle;
+ if (buf[9] == 0xff && buf[10] == 0xff)
+ angle = -1;
+ else
+ angle = v8_to_v32 (0, buf[9], buf[10], 0);
+ traj_ptw_start (buf[2],
+ v8_to_v32 (buf[3], buf[4], buf[5], 0xff),
+ v8_to_v32 (buf[6], buf[7], buf[8], 0xff),
+ angle, 0);
+ }
+ break;
case c ('g', 2):
/* Go to the wall using center sensor with delay.
* - b: 0: forward, 1: backward.
diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py
index 997b05fb..4f386d7a 100644
--- a/digital/asserv/tools/asserv/asserv.py
+++ b/digital/asserv/tools/asserv/asserv.py
@@ -269,6 +269,15 @@ class Proto:
self.proto.send ('f', 'BB', backward and 1 or 0, self.mseq)
self.wait (self.finished, auto = True)
+ def ptw (self, backward = True, init_x = None, init_y = None,
+ init_a = None):
+ """Push the wall."""
+ self.mseq += 1
+ self.proto.send ('f', 'BllHB', backward and 1 or 0,
+ self._dist_f248 (init_x), self._dist_f248 (init_y),
+ self._angle_f16 (init_a), 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)),
@@ -381,10 +390,16 @@ class Proto:
return int (round (d / self.param['scale']))
def _dist_f248 (self, d):
- return int (round ((1 << 8) * d / self.param['scale']))
+ if d is None:
+ return -1
+ else:
+ return int (round ((1 << 8) * d / self.param['scale']))
def _angle_f16 (self, a):
- return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff
+ if a is None:
+ return 0xffff
+ else:
+ return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff
def _angle_f824 (self, a):
return int (round ((1 << 24) * a / (2 * math.pi)))