summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c2
-rw-r--r--digital/asserv/src/asserv/eeprom.h2
-rw-r--r--digital/asserv/src/asserv/main.c12
-rw-r--r--digital/asserv/src/asserv/traj.c50
-rw-r--r--digital/asserv/src/asserv/traj.h4
-rw-r--r--digital/asserv/src/asserv/twi_proto.c5
6 files changed, 73 insertions, 2 deletions
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index aee320fb..a8c1371e 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -98,6 +98,7 @@ eeprom_read_params (void)
pos_d_sat = eeprom_read_word (p16++);
pos_blocked = eeprom_read_word (p16++);
traj_eps = eeprom_read_word (p16++);
+ traj_aeps = eeprom_read_word (p16++);
}
/* Write parameters to eeprom. */
@@ -134,6 +135,7 @@ eeprom_write_params (void)
eeprom_write_word (p16++, pos_d_sat);
eeprom_write_word (p16++, pos_blocked);
eeprom_write_word (p16++, traj_eps);
+ eeprom_write_word (p16++, traj_aeps);
}
/* Clear eeprom parameters. */
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index b603942d..8c99fb5f 100644
--- a/digital/asserv/src/asserv/eeprom.h
+++ b/digital/asserv/src/asserv/eeprom.h
@@ -26,7 +26,7 @@
* }}} */
/** Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x49
+#define EEPROM_KEY 0x4a
void
eeprom_read_params (void);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 99c0e369..79969a6b 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -359,6 +359,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
v8_to_v32 (args[4], args[5], args[6], args[7]),
args[8]);
break;
+ case c ('x', 5):
+ /* Go to angle.
+ * - d: a, f8.24.
+ * - b: sequence number. */
+ if (args[4] == state_main.sequence)
+ break;
+ traj_goto_angle_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
+ args[4]);
+ break;
case c ('a', 2):
/* Set both acknoledge.
* - b: main ack sequence number.
@@ -515,8 +524,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('b', 3):
pos_blocked = v8_to_v16 (args[1], args[2]);
break;
- case c ('e', 3):
+ case c ('e', 5):
traj_eps = v8_to_v16 (args[1], args[2]);
+ traj_aeps = v8_to_v16 (args[3], args[4]);
break;
case c ('w', 2):
/* Set PWM direction.
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index bc95b39a..dc7c439b 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -53,6 +53,8 @@ enum
TRAJ_GTD,
/* Go to position. */
TRAJ_GOTO,
+ /* Go to angle. */
+ TRAJ_GOTO_ANGLE,
/* Everything done. */
TRAJ_DONE,
};
@@ -63,9 +65,15 @@ uint8_t traj_mode;
/** Epsilon, distance considered to be small enough. */
int16_t traj_eps = 500;
+/** Angle epsilon, angle considered to be small enough (f0.16). */
+int16_t traj_aeps = 0x0100;
+
/** Go to position. */
static uint32_t traj_goto_x, traj_goto_y;
+/** Go to angle. */
+static uint32_t traj_goto_a;
+
/** Angle offset. Directly handled to speed layer. */
void
traj_angle_offset_start (int32_t angle, uint8_t seq)
@@ -204,6 +212,45 @@ traj_goto_start (uint32_t x, uint32_t y, uint8_t seq)
traj_goto_x = x;
traj_goto_y = y;
speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_alpha.pos_cons = pos_alpha.cons;
+ state_start (&state_main, seq);
+}
+
+/** Go to angle mode. */
+static void
+traj_goto_angle (void)
+{
+ /* There is some tricky parts to handle rotation direction. */
+ int16_t da = (uint16_t) (traj_goto_a >> 8) - (uint16_t) (postrack_a >> 8);
+ if (UTILS_ABS (da) < traj_aeps)
+ {
+ /* Near enough, stop, let speed terminate the movement. */
+ state_main.mode = MODE_SPEED;
+ traj_mode = TRAJ_DONE;
+ }
+ else
+ {
+ /* Compute arc length. */
+ int32_t arad = fixed_mul_f824 (((int32_t) da) << 8,
+ 2 * M_PI * (1L << 24));
+ int32_t arc = fixed_mul_f824 (arad, postrack_footing);
+ /* Compute consign. */
+ speed_alpha.pos_cons = pos_alpha.cur;
+ speed_alpha.pos_cons += arc;
+ }
+}
+
+/** Start go to angle mode (a: f8.24). */
+void
+traj_goto_angle_start (uint32_t a, uint8_t seq)
+{
+ state_main.mode = MODE_TRAJ;
+ traj_mode = TRAJ_GOTO_ANGLE;
+ traj_goto_a = a;
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_alpha.pos_cons = pos_alpha.cons;
state_start (&state_main, seq);
}
@@ -222,6 +269,9 @@ traj_update (void)
case TRAJ_GOTO:
traj_goto ();
break;
+ case TRAJ_GOTO_ANGLE:
+ traj_goto_angle ();
+ break;
case TRAJ_DONE:
break;
}
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index c13eab9d..fe9830c9 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -27,6 +27,7 @@
extern uint8_t traj_mode;
extern int16_t traj_eps;
+extern int16_t traj_aeps;
void
traj_update (void);
@@ -43,4 +44,7 @@ traj_gtd_start (uint8_t seq);
void
traj_goto_start (uint32_t x, uint32_t y, uint8_t seq);
+void
+traj_goto_angle_start (uint32_t a, uint8_t seq);
+
#endif /* traj_h */
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 75a0a110..db6bac41 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -147,6 +147,11 @@ twi_proto_callback (u8 *buf, u8 size)
v8_to_v32 (buf[5], buf[6], buf[7], 0),
0);
break;
+ case c ('y', 0):
+ /* Go to angle.
+ * - w: angle. */
+ traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0), 0);
+ break;
case c ('b', 3):
/* Move the arm.
* - w: new position.