summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c3
-rw-r--r--digital/asserv/src/asserv/eeprom.h2
-rw-r--r--digital/asserv/src/asserv/main.c15
-rw-r--r--digital/asserv/src/asserv/traj.c64
-rw-r--r--digital/asserv/src/asserv/traj.h4
-rw-r--r--digital/asserv/src/asserv/twi_proto.c8
6 files changed, 95 insertions, 1 deletions
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index 96611d3b..9f4920a4 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -31,6 +31,7 @@
#include "pos.h"
#include "speed.h"
#include "postrack.h"
+#include "traj.h"
#define EEPROM_START 0
@@ -71,6 +72,7 @@ eeprom_read_params (void)
pos_e_sat = eeprom_read_word (p16++);
pos_int_sat = eeprom_read_word (p16++);
pos_blocked = eeprom_read_word (p16++);
+ traj_eps = eeprom_read_word (p16++);
}
/* Write parameters to eeprom. */
@@ -104,6 +106,7 @@ eeprom_write_params (void)
eeprom_write_word (p16++, pos_e_sat);
eeprom_write_word (p16++, pos_int_sat);
eeprom_write_word (p16++, pos_blocked);
+ eeprom_write_word (p16++, traj_eps);
}
/* Clear eeprom parameters. */
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index 95bb4e64..2f3d5c74 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 0x46
+#define EEPROM_KEY 0x47
void
eeprom_read_params (void);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index c4e601df..60aef03d 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -348,6 +348,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
traj_gtd_start (args[0]);
break;
+ case c ('x', 9):
+ /* Go to position.
+ * - d: x, f24.8.
+ * - d: y, f24.8.
+ * - 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]),
+ args[8]);
+ break;
case c ('a', 2):
/* Set both acknoledge.
* - b: main ack sequence number.
@@ -495,6 +506,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):
+ traj_eps = v8_to_v16 (args[1], args[2]);
+ break;
case c ('w', 2):
/* Set PWM direction.
* - b: bits: 0000[aux0][right][left]. */
@@ -526,6 +540,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send1w ('E', pos_e_sat);
proto_send1w ('I', pos_int_sat);
proto_send1w ('b', pos_blocked);
+ proto_send1w ('e', traj_eps);
proto_send1b ('w', pwm_reverse);
break;
default:
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index ce8a7585..19995818 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -27,8 +27,11 @@
#include "modules/math/fixed/fixed.h"
#include "modules/math/math.h"
+#include "modules/utils/utils.h"
#include "io.h"
+#include <math.h>
+
#include "state.h"
#include "pos.h"
@@ -48,6 +51,8 @@ enum
TRAJ_FTW,
/* Go to the dispenser. */
TRAJ_GTD,
+ /* Go to position. */
+ TRAJ_GOTO,
/* Everything done. */
TRAJ_DONE,
};
@@ -55,6 +60,12 @@ enum
/** Traj mode. */
uint8_t traj_mode;
+/** Epsilon, distance considered to be small enough. */
+int16_t traj_eps = 500;
+
+/** Go to position. */
+static uint32_t traj_goto_x, traj_goto_y;
+
/** Angle offset. Directly handled to speed layer. */
void
traj_angle_offset_start (int32_t angle, uint8_t seq)
@@ -146,6 +157,56 @@ traj_gtd_start (uint8_t seq)
state_start (&state_main, seq);
}
+/** Go to position mode. */
+static void
+traj_goto (void)
+{
+ int32_t dx = traj_goto_x - postrack_x;
+ int32_t dy = traj_goto_y - postrack_y;
+ if (UTILS_ABS (dx) < ((int32_t) traj_eps) << 8
+ && UTILS_ABS (dy) < ((int32_t) traj_eps) << 8)
+ {
+ /* Near enough, stop. */
+ state_finish (&state_main);
+ traj_mode = TRAJ_DONE;
+ }
+ else
+ {
+ /* Projection of destination in robot base. */
+ int32_t c = fixed_cos_f824 (postrack_a);
+ int32_t s = fixed_sin_f824 (postrack_a);
+ int32_t dt = fixed_mul_f824 (dx, c) + fixed_mul_f824 (dy, s);
+ int32_t da = fixed_mul_f824 (dy, c) - fixed_mul_f824 (dx, s);
+ /* Compute arc length. */
+ int32_t arad = atan2 (da, dt) * (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;
+ if (UTILS_ABS (arad) < 0.5 * M_PI * (1L << 24))
+ {
+ speed_theta.pos_cons = pos_theta.cur;
+ speed_theta.pos_cons += dt >> 8;
+ }
+ else
+ {
+ speed_theta.pos_cons = pos_theta.cons;
+ }
+ }
+}
+
+/** Start go to position mode (x, y: f24.8). */
+void
+traj_goto_start (uint32_t x, uint32_t y, uint8_t seq)
+{
+ state_main.mode = MODE_TRAJ;
+ traj_mode = TRAJ_GOTO;
+ traj_goto_x = x;
+ traj_goto_y = y;
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ state_start (&state_main, seq);
+}
+
/* Compute new speed according the defined trajectory. */
void
traj_update (void)
@@ -158,6 +219,9 @@ traj_update (void)
case TRAJ_GTD:
traj_gtd ();
break;
+ case TRAJ_GOTO:
+ traj_goto ();
+ break;
case TRAJ_DONE:
break;
}
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index d80460bf..c13eab9d 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -26,6 +26,7 @@
* }}} */
extern uint8_t traj_mode;
+extern int16_t traj_eps;
void
traj_update (void);
@@ -39,4 +40,7 @@ traj_ftw_start (uint8_t seq);
void
traj_gtd_start (uint8_t seq);
+void
+traj_goto_start (uint32_t x, uint32_t y, 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 e977bf76..75a0a110 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -139,6 +139,14 @@ twi_proto_callback (u8 *buf, u8 size)
/* Go to the dispenser. */
traj_gtd_start (0);
break;
+ case c ('x', 0):
+ /* 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);
+ break;
case c ('b', 3):
/* Move the arm.
* - w: new position.