From dbb94e5c0018371838914f62bfa14a62a1ad922a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 19 May 2009 20:04:17 +0200 Subject: * digital/asserv, digital/io: - added backward movements. --- digital/asserv/src/asserv/main.c | 38 ++++++++--------------------------- digital/asserv/src/asserv/traj.c | 20 ++++++++++-------- digital/asserv/src/asserv/traj.h | 7 +++++-- digital/asserv/src/asserv/twi_proto.c | 32 ++++++++--------------------- digital/asserv/tools/asserv/asserv.py | 22 ++++++++++++++------ digital/asserv/tools/inter_asserv.py | 17 ++++++++++------ 6 files changed, 60 insertions(+), 76 deletions(-) (limited to 'digital/asserv') 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 -- cgit v1.2.3