summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/main.c
diff options
context:
space:
mode:
authorschodet2006-04-01 17:58:15 +0000
committerschodet2006-04-01 17:58:15 +0000
commite7d3f72a1b83a67cd9ac5b366de6fb2be0437166 (patch)
tree034cb7d3cbc15561f6db371c78ab2ce001a1e8b7 /n/asserv/src/asserv/main.c
parenta995bb9ff446a514ce99064ad9fc6c2f8c773eba (diff)
Ajout du calcul de position absolu.
Diffstat (limited to 'n/asserv/src/asserv/main.c')
-rw-r--r--n/asserv/src/asserv/main.c47
1 files changed, 47 insertions, 0 deletions
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index 11ef09a..c74f786 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -42,6 +42,7 @@
#endif
#include "pos.c"
#include "speed.c"
+#include "postrack.c"
#ifndef HOST
# include "eeprom.avr.c"
#endif
@@ -56,6 +57,9 @@ int8_t main_mode;
/** Report of counters. */
uint8_t main_stat_counter, main_stat_counter_cpt;
+/** Report of position. */
+uint8_t main_stat_postrack, main_stat_postrack_cpt;
+
/** Statistics about speed control. */
uint8_t main_stat_speed, main_stat_speed_cpt;
@@ -121,6 +125,8 @@ main_loop (void)
/* Pwm setup. */
pwm_update ();
main_timer_2 = timer_read ();
+ /* Compute absolute position. */
+ postrack_update ();
/* Speed control. */
if (main_mode >= 2)
speed_update ();
@@ -131,6 +137,11 @@ main_loop (void)
proto_send2w ('C', counter_left, counter_right);
main_stat_counter_cpt = main_stat_counter;
}
+ if (main_stat_postrack && !--main_stat_postrack_cpt)
+ {
+ proto_send3d ('X', postrack_x, postrack_y, postrack_a);
+ main_stat_postrack_cpt = main_stat_postrack;
+ }
if (main_stat_speed && !--main_stat_speed_cpt)
{
proto_send2b ('S', speed_theta_cur >> 8, speed_alpha_cur >> 8);
@@ -172,6 +183,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
switch (c (cmd, size))
{
case c ('z', 0):
+ /* Reset. */
utils_reset ();
break;
/* Commands. */
@@ -240,6 +252,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Counter stats. */
main_stat_counter_cpt = main_stat_counter = args[0];
break;
+ case c ('X', 1):
+ /* Position stats. */
+ main_stat_postrack_cpt = main_stat_postrack = args[0];
+ break;
case c ('S', 1):
/* Motor speed control stats. */
main_stat_speed_cpt = main_stat_speed = args[0];
@@ -266,7 +282,36 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
switch (c (args[0], size))
{
+ case c ('X', 1):
+ /* Reset position. */
+ postrack_x = 0;
+ postrack_y = 0;
+ postrack_a = 0;
+ break;
+ case c ('X', 5):
+ /* Set current x position.
+ * - d: x position. */
+ postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('Y', 5):
+ /* Set current y position.
+ * - d: y position. */
+ postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('A', 5):
+ /* Set current angle.
+ * - d: angle. */
+ postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('f', 3):
+ /* Set footing.
+ * - w: footing. */
+ postrack_set_footing (v8_to_v16 (args[1], args[2]));
+ break;
case c ('a', 5):
+ /* Set acceleration.
+ * - w: theta.
+ * - w: alpha. */
speed_theta_acc = v8_to_v16 (args[1], args[2]);
speed_alpha_acc = v8_to_v16 (args[3], args[4]);
break;
@@ -316,6 +361,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('P', 1):
/* Print current settings. */
proto_send1b ('E', EEPROM_KEY);
+ proto_send1w ('f', postrack_footing);
+ proto_send2w ('a', speed_theta_acc, speed_alpha_acc);
proto_send2w ('p', pos_theta_kp, pos_alpha_kp);
proto_send2w ('i', pos_theta_ki, pos_alpha_ki);
proto_send2w ('d', pos_theta_kd, pos_alpha_kd);