summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/main.c38
-rw-r--r--digital/asserv/src/asserv/traj.c20
-rw-r--r--digital/asserv/src/asserv/traj.h7
-rw-r--r--digital/asserv/src/asserv/twi_proto.c32
-rw-r--r--digital/asserv/tools/asserv/asserv.py22
-rw-r--r--digital/asserv/tools/inter_asserv.py17
6 files changed, 60 insertions, 76 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index b34ed364..b53e6d18 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -398,27 +398,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
traj_gtd_start (args[0]);
break;
- case c ('x', 9):
+ case c ('x', 10):
/* Go to position.
* - d: x, f24.8.
* - d: y, f24.8.
+ * - b: backward (see traj.h).
* - b: sequence number. */
- if (args[8] == state_main.sequence)
- break;
- traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
- v8_to_v32 (args[4], args[5], args[6], args[7]),
- 0, args[8]);
- break;
- case c ('r', 9):
- /* Go to position, backward allowed.
- * - d: x, f24.8.
- * - d: y, f24.8.
- * - b: sequence number. */
- if (args[8] == state_main.sequence)
+ if (args[9] == state_main.sequence)
break;
traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
v8_to_v32 (args[4], args[5], args[6], args[7]),
- 1, args[8]);
+ args[8], args[9]);
break;
case c ('x', 3):
/* Go to angle.
@@ -429,31 +419,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0),
args[2]);
break;
- case c ('x', 11):
+ case c ('x', 12):
/* Go to position, then angle.
* - d: x, f24.8.
* - d: y, f24.8.
* - w: a, f0.16.
+ * - b: backward (see traj.h).
* - b: sequence number. */
- if (args[10] == state_main.sequence)
- break;
- traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
- v8_to_v32 (args[4], args[5], args[6], args[7]),
- v8_to_v32 (0, args[8], args[9], 0),
- 0, args[10]);
- break;
- case c ('r', 11):
- /* Go to position, then angle, backward allowed.
- * - d: x, f24.8.
- * - d: y, f24.8.
- * - w: a, f0.16.
- * - b: sequence number. */
- if (args[10] == state_main.sequence)
+ if (args[11] == state_main.sequence)
break;
traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
v8_to_v32 (args[4], args[5], args[6], args[7]),
v8_to_v32 (0, args[8], args[9], 0),
- 1, args[10]);
+ args[10], args[11]);
break;
case c ('y', 4):
/* Auxiliary go to position.
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index 92ff94f1..bcd99256 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -93,9 +93,6 @@ static uint32_t traj_goto_x, traj_goto_y;
/** Go to angle. */
static uint32_t traj_goto_a;
-/** Allow backward movements. */
-static uint8_t traj_backward_ok;
-
/** Initialise computed factors. */
void
traj_init (void)
@@ -238,7 +235,14 @@ traj_goto (void)
int32_t da = fixed_mul_f824 (dy, c) - fixed_mul_f824 (dx, s);
/* Compute arc length. */
int32_t arad = atan2 (da, dt) * (1L << 24);
- if (traj_backward_ok)
+ if (traj_backward & TRAJ_BACKWARD)
+ {
+ if (arad > 0)
+ arad = - PI_F824 + arad;
+ else
+ arad = PI_F824 + arad;
+ }
+ if (traj_backward & TRAJ_REVERT_OK)
{
if (arad > TRAJ_GOTO_BACKWARD_THRESHOLD)
arad = - PI_F824 + arad;
@@ -263,12 +267,12 @@ traj_goto (void)
/** Start go to position mode (x, y: f24.8). */
void
-traj_goto_start (uint32_t x, uint32_t y, uint8_t backward_ok, uint8_t seq)
+traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, uint8_t seq)
{
traj_mode = TRAJ_GOTO;
traj_goto_x = x;
traj_goto_y = y;
- traj_backward_ok = backward_ok;
+ traj_backward = backward;
speed_theta.use_pos = speed_alpha.use_pos = 1;
speed_theta.pos_cons = pos_theta.cons;
speed_alpha.pos_cons = pos_alpha.cons;
@@ -332,14 +336,14 @@ traj_goto_xya (void)
/** Start go to position, then angle mode (x, y: f24.8, a: f8.24). */
void
-traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward_ok,
+traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward,
uint8_t seq)
{
traj_mode = TRAJ_GOTO_XYA;
traj_goto_x = x;
traj_goto_y = y;
traj_goto_a = a;
- traj_backward_ok = backward_ok;
+ traj_backward = backward;
speed_theta.use_pos = speed_alpha.use_pos = 1;
speed_theta.pos_cons = pos_theta.cons;
speed_alpha.pos_cons = pos_alpha.cons;
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index c360ea2e..8f9c28ac 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -25,6 +25,9 @@
*
* }}} */
+#define TRAJ_BACKWARD 1
+#define TRAJ_REVERT_OK 2
+
extern uint8_t traj_mode;
extern int16_t traj_eps;
extern int16_t traj_aeps;
@@ -46,13 +49,13 @@ void
traj_gtd_start (uint8_t seq);
void
-traj_goto_start (uint32_t x, uint32_t y, uint8_t backward_ok, uint8_t seq);
+traj_goto_start (uint32_t x, uint32_t y, uint8_t backward, uint8_t seq);
void
traj_goto_angle_start (uint32_t a, uint8_t seq);
void
-traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward_ok,
+traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward,
uint8_t seq);
void
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index df41efaa..dac59f93 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -172,46 +172,30 @@ twi_proto_callback (u8 *buf, u8 size)
/* Go to the dispenser. */
traj_gtd_start (0);
break;
- case c ('x', 6):
+ case c ('x', 7):
/* Go to position.
* - 3b: x position.
- * - 3b: y position. */
- traj_goto_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
- v8_to_v32 (buf[5], buf[6], buf[7], 0),
- 0, 0);
- break;
- case c ('r', 6):
- /* Go to position, backward allowed.
- * - 3b: x position.
- * - 3b: y position. */
+ * - 3b: y position.
+ * - b: backward (see traj.h). */
traj_goto_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
v8_to_v32 (buf[5], buf[6], buf[7], 0),
- 1, 0);
+ buf[8], 0);
break;
case c ('y', 2):
/* Go to angle.
* - w: angle. */
traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0), 0);
break;
- case c ('X', 8):
+ case c ('X', 9):
/* Go to position, then angle.
* - 3b: x position.
* - 3b: y position.
- * - w: angle. */
- traj_goto_xya_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
- v8_to_v32 (buf[5], buf[6], buf[7], 0),
- v8_to_v32 (0, buf[8], buf[9], 0),
- 0, 0);
- break;
- case c ('R', 8):
- /* Go to position, then angle, backward allowed.
- * - 3b: x position.
- * - 3b: y position.
- * - w: angle. */
+ * - w: angle.
+ * - b: backward (see traj.h). */
traj_goto_xya_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
v8_to_v32 (buf[5], buf[6], buf[7], 0),
v8_to_v32 (0, buf[8], buf[9], 0),
- 1, 0);
+ buf[10], 0);
break;
case c ('b', 3):
/* Move the aux0.
diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py
index a8b5c753..8e1f9171 100644
--- a/digital/asserv/tools/asserv/asserv.py
+++ b/digital/asserv/tools/asserv/asserv.py
@@ -225,11 +225,16 @@ class Proto:
if a is not None:
self.proto.send ('p', 'cl', 'A', self._angle_f824 (a))
- def goto (self, x, y, backward_ok = False):
+ def goto (self, x, y, backward = False, revert_ok = False):
"""Go to position."""
self.mseq += 1
- self.proto.send (backward_ok and 'r' or 'x', 'llB',
- self._dist_f248 (x), self._dist_f248 (y), self.mseq)
+ b = 0
+ if backward:
+ b |= 1
+ if revert_ok:
+ b |= 2
+ self.proto.send ('x', 'llBB',
+ self._dist_f248 (x), self._dist_f248 (y), b, self.mseq)
self.wait (self.finished, auto = True)
def goto_angle (self, a):
@@ -238,12 +243,17 @@ class Proto:
self.proto.send ('x', 'HB', self._angle_f16 (a), self.mseq)
self.wait (self.finished, auto = True)
- def goto_xya (self, x, y, a, backward_ok = False):
+ def goto_xya (self, x, y, a, backward = False, revert_ok = False):
"""Go to position, then angle."""
self.mseq += 1
- self.proto.send (backward_ok and 'r' or 'x', 'llHB',
+ b = 0
+ if backward:
+ b |= 1
+ if revert_ok:
+ b |= 2
+ self.proto.send ('x', 'llHBB',
self._dist_f248 (x), self._dist_f248 (y),
- self._angle_f16 (a), self.mseq)
+ self._angle_f16 (a), b, self.mseq)
self.wait (self.finished, auto = True)
def ftw (self, backward = True):
diff --git a/digital/asserv/tools/inter_asserv.py b/digital/asserv/tools/inter_asserv.py
index 144c79a3..47ac1837 100644
--- a/digital/asserv/tools/inter_asserv.py
+++ b/digital/asserv/tools/inter_asserv.py
@@ -67,10 +67,14 @@ class InterAsserv (Inter):
text = 'goto', value = 'goto',
variable = self.actionVar)
self.actionGotoButton.pack ()
- self.backwardOkVar = IntVar ()
- self.backwardOkButton = Checkbutton (self.rightFrame,
- text = 'backward ok', variable = self.backwardOkVar)
- self.backwardOkButton.pack ()
+ self.backwardVar = IntVar ()
+ self.backwardButton = Checkbutton (self.rightFrame,
+ text = 'backward', variable = self.backwardVar)
+ self.backwardButton.pack ()
+ self.revertokVar = IntVar ()
+ self.revertokButton = Checkbutton (self.rightFrame,
+ text = 'revert ok', variable = self.revertokVar)
+ self.revertokButton.pack ()
self.ftwButton = Button (self.rightFrame, text = 'FTW',
command = self.ftw)
self.ftwButton.pack ()
@@ -101,7 +105,8 @@ class InterAsserv (Inter):
if action == 'set_pos':
self.a.set_pos (x, y)
elif action == 'goto':
- self.a.goto (x, y, self.backwardOkVar.get ())
+ self.a.goto (x, y, self.backwardVar.get (),
+ self.revertokVar.get ())
else:
assert 0
@@ -119,7 +124,7 @@ class InterAsserv (Inter):
self.a.free ()
def ftw (self):
- self.a.ftw (self.backwardOkVar.get ())
+ self.a.ftw (self.backwardVar.get ())
if __name__ == '__main__':
import sys