summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/main.c
diff options
context:
space:
mode:
authorschodet2006-05-07 23:38:01 +0000
committerschodet2006-05-07 23:38:01 +0000
commitbb2dc882b10243fb0301379be0f1dbd43a393f49 (patch)
tree53965ac117cb9fa1a7f8a246e385cc43de5c4f56 /n/asserv/src/asserv/main.c
parentc5ec7a7a497328a9ac16f58464bdba2760689218 (diff)
Ajout de trou partout !
Diffstat (limited to 'n/asserv/src/asserv/main.c')
-rw-r--r--n/asserv/src/asserv/main.c66
1 files changed, 59 insertions, 7 deletions
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index 2a19877..aec9c8e 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -43,6 +43,7 @@
#include "pos.c"
#include "speed.c"
#include "postrack.c"
+#include "traj.c"
#ifndef HOST
# include "eeprom.avr.c"
#endif
@@ -78,6 +79,11 @@ uint8_t main_stat_timer, main_stat_timer_cpt;
/** Print input port. */
uint8_t main_print_pin, main_print_pin_cpt;
+#ifdef HOST
+/** Simulation data. */
+uint8_t main_simu, main_simu_cpt;
+#endif /* HOST */
+
/** Record timer value at different stage of computing. Used for performance
* analisys. */
uint8_t main_timer[6];
@@ -104,6 +110,7 @@ main (int argc, char **argv)
timer_init ();
counter_init ();
uart0_init ();
+ postrack_init ();
proto_send0 ('z');
sei ();
while (1)
@@ -129,6 +136,9 @@ main_loop (void)
main_timer[2] = timer_read ();
/* Compute absolute position. */
postrack_update ();
+ /* Compute trajectory. */
+ if (main_mode >= 3)
+ traj_update ();
/* Speed control. */
if (main_mode >= 2)
speed_update ();
@@ -155,6 +165,17 @@ main_loop (void)
pos_alpha_e_old, pos_alpha_int);
main_stat_pos_cpt = main_stat_pos;
}
+#ifdef HOST
+ if (main_simu && !--main_simu_cpt)
+ {
+ proto_send4w ('Y', (uint16_t) simu_pos_x,
+ (uint16_t) simu_pos_y,
+ (uint16_t) (simu_pos_a * 1024),
+ (uint16_t) simu_nearest_hole);
+ proto_send3b ('Z', traj_mode, 0, 0);
+ main_simu_cpt = main_simu;
+ }
+#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
proto_send2w ('W', pwm_left, pwm_right);
@@ -231,20 +252,28 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_theta_cons = args[0] << 8;
speed_alpha_cons = args[1] << 8;
break;
- case c ('s', 5):
+ case c ('s', 9):
/* Set speed controlled position consign.
- * - w: theta consign offset.
- * - w: alpha consign offset.
+ * - d: theta consign offset.
+ * - d: alpha consign offset.
* - b: sequence number. */
- if (args[4] == main_sequence)
+ if (args[8] == main_sequence)
break;
main_mode = 2;
speed_pos = 1;
speed_theta_pos_cons = pos_theta_cons;
- speed_theta_pos_cons += v8_to_v16 (args[0], args[1]);
+ speed_theta_pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
speed_alpha_pos_cons = pos_alpha_cons;
- speed_alpha_pos_cons += v8_to_v16 (args[2], args[3]);
- main_sequence = args[4];
+ speed_alpha_pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]);
+ main_sequence = args[8];
+ break;
+ case c ('h', 1):
+ /* Find a hole.
+ * - b: sequence number. */
+ if (args[0] == main_sequence)
+ break;
+ main_mode = 3;
+ traj_mode = 10;
break;
/* Stats.
* - b: interval between stats. */
@@ -276,6 +305,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Input port stats. */
main_print_pin_cpt = main_print_pin = args[0];
break;
+#ifdef HOST
+ case c ('Y', 1):
+ /* Simulation data. */
+ main_simu_cpt = main_simu = args[0];
+ break;
+#endif /* HOST */
default:
/* Params. */
if (cmd == 'p')
@@ -383,6 +418,23 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
return;
}
}
+#ifdef HOST
+ else if (cmd == 'y')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('X', 7):
+ /* Set simulated position.
+ * - w: x position.
+ * - w: y position.
+ * - w: angle (rad) * 1024. */
+ simu_pos_x = (double) v8_to_v16 (args[1], args[2]);
+ simu_pos_y = (double) v8_to_v16 (args[3], args[4]);
+ simu_pos_a = (double) v8_to_v16 (args[5], args[6]) / 1024;
+ break;
+ }
+ }
+#endif /* HOST */
else
{
proto_send0 ('?');