From e4589d3099a11723b3cd017c9c8c97b070455ea9 Mon Sep 17 00:00:00 2001 From: schodet Date: Fri, 11 Feb 2005 14:36:33 +0000 Subject: Ajout de la première version d'asservissement en position. --- n/asserv/src/main.c | 41 ++++++++++++++++++++++++++++++++++------- 1 file changed, 34 insertions(+), 7 deletions(-) (limited to 'n/asserv/src/main.c') diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index 3fed431..bac4cfd 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -34,6 +34,7 @@ #include "counter.c" #include "speed.c" #include "postrack.c" +#include "goto.c" /** Motor mode : * 0 - pwm setup; @@ -61,7 +62,8 @@ uint8_t motor_stat_postrack, motor_stat_postrack_cpt; /** Record timer value at different stage of computing. Used for performance * analisys. */ -uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3; +uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3, + motor_timer_4; /* +AutoDec */ @@ -98,8 +100,15 @@ main_loop (void) while (!timer_pending ()) counter_poll (); counter_update (); - motor_timer_3 = timer_read (); + motor_timer_4 = timer_read (); + /* Position tracking. */ postrack_update (); + motor_timer_3 = timer_read (); + /* Position control. */ + if (motor_mode >= 2) + { + goto_update (); + } motor_timer_2 = timer_read (); /* Speed control. */ if (motor_mode >= 1) @@ -161,6 +170,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('z', 0): reset (); break; + case c ('g', 8): + motor_mode = 2; + goto_x = (uint32_t) args[0] << 24 | (uint32_t) args[1] << 16 + | args[2] << 8 | args[3]; + goto_y = (uint32_t) args[4] << 24 | (uint32_t) args[5] << 16 + | args[6] << 8 | args[7]; + break; + case c ('s', 0): + motor_mode = 1; + speed_left_aim = 0; + speed_right_aim = 0; + break; + case c ('s', 2): + motor_mode = 1; + speed_left_aim = args[0]; + speed_right_aim = args[1]; + break; + case c ('s', 3): + motor_mode = 1; + speed_left_aim = args[0]; + speed_right_aim = args[1]; + speed_max = args[2]; + break; case c ('w', 0): speed_restart (); motor_mode = 0; @@ -173,11 +205,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pwm_left = args[0] << 8 | args[1]; pwm_right = args[2] << 8 | args[3]; break; - case c ('s', 2): - motor_mode = 1; - speed_left_aim = args[0]; - speed_right_aim = args[1]; - break; case c ('a', 1): speed_acc_cpt = speed_acc = args[0]; break; -- cgit v1.2.3