summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2009-05-19 20:04:17 +0200
committerNicolas Schodet2009-05-19 20:04:17 +0200
commitdbb94e5c0018371838914f62bfa14a62a1ad922a (patch)
treebd7f0f1974d4aa9718590c167762b4c1c66f6ce8
parent1e8fee2bf4e410a64401e30e6cbca526fba20209 (diff)
* digital/asserv, digital/io:
- added backward movements.
-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
-rw-r--r--digital/io/src/asserv.c15
-rw-r--r--digital/io/src/asserv.h6
8 files changed, 76 insertions, 81 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
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index a94fecad..3a22d0bc 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -475,9 +475,11 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a)
/* Put angle as parameter */
asserv_twi_buffer_param[6] = v16_to_v8 (a, 1);
asserv_twi_buffer_param[7] = v16_to_v8 (a, 0);
+ /* No backward. */
+ asserv_twi_buffer_param[8] = 0;
/* Send the got to an absolute position and them absolute angle command to
* the asserv board */
- asserv_twi_send_command ('X', 8);
+ asserv_twi_send_command ('X', 9);
}
/* Go to the wall (moving backward). */
@@ -637,8 +639,10 @@ asserv_goto (uint32_t x, uint32_t y)
asserv_twi_buffer_param[3] = v32_to_v8 (y, 2);
asserv_twi_buffer_param[4] = v32_to_v8 (y, 1);
asserv_twi_buffer_param[5] = v32_to_v8 (y, 0);
+ /* No backward. */
+ asserv_twi_buffer_param[6] = 0;
/* Send the got to an absolute position command to the asserv board */
- asserv_twi_send_command ('x', 6);
+ asserv_twi_send_command ('x', 7);
}
/* Go to an absolute position at (X, Y) with backward enabled. */
@@ -655,9 +659,10 @@ asserv_goto_back (uint32_t x, uint32_t y)
asserv_twi_buffer_param[3] = v32_to_v8 (y, 2);
asserv_twi_buffer_param[4] = v32_to_v8 (y, 1);
asserv_twi_buffer_param[5] = v32_to_v8 (y, 0);
- /* Send the goto to an absolute position with backward enabled command to
- * the asserv board */
- asserv_twi_send_command ('r', 6);
+ /* Authorise backward movements. */
+ asserv_twi_buffer_param[6] = ASSERV_REVERT_OK;
+ /* Send the goto to an absolute position command to the asserv board */
+ asserv_twi_send_command ('x', 7);
}
/* Notify get samples FSM when the arm reach desired position. */
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index e75b1ae2..7e2ba988 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -43,6 +43,12 @@
#define ASSERV_ARM_STEP_BY_DEGREE 14.814814
+/** Use backward movements. */
+#define ASSERV_BACKWARD 1
+/** Authorise reverse the requested movement direction, may be or'ed with the
+ * previous define. */
+#define ASSERV_REVERT_OK 2
+
/**
* Initialize the asserv control module.
* This functions does not initialize the asserv board, but the underling