From fa8ff2dec76b1b366207a126dfc360331510c50d Mon Sep 17 00:00:00 2001 From: schodet Date: Fri, 19 Nov 2004 16:38:34 +0000 Subject: Ajout du calcul de position. Non testé. --- n/asserv/src/main.c | 30 ++++++++++++++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) (limited to 'n/asserv/src/main.c') diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index 8656644..2adbb76 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -33,6 +33,7 @@ #include "timer.c" #include "counter.c" #include "speed.c" +#include "postrack.c" /** Motor mode : * 0 - pwm setup; @@ -55,9 +56,12 @@ uint8_t motor_stat_timer, motor_stat_timer_cpt; /** Report of counters. */ uint8_t motor_stat_counter, motor_stat_counter_cpt; +/** Report position. */ +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; +uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3; /* +AutoDec */ @@ -80,6 +84,7 @@ main (void) timer_init (); counter_init (); speed_init (); + postrack_init (); rs232_init (); proto_init (proto_callback, rs232_putc); proto_send0 ('z'); @@ -98,6 +103,8 @@ main_loop (void) while (!timer_pending ()) counter_poll (); counter_update (); + motor_timer_3 = timer_read (); + postrack_update (); motor_timer_2 = timer_read (); /* Speed control. */ if (motor_mode >= 1) @@ -129,7 +136,8 @@ main_loop (void) } if (motor_stat_timer && !--motor_stat_timer_cpt) { - proto_send3 ('T', motor_timer_2, motor_timer_1, motor_timer_0); + proto_send4 ('T', motor_timer_3, motor_timer_2, motor_timer_1, + motor_timer_0); motor_stat_timer_cpt = motor_stat_timer; } if (motor_stat_counter && !--motor_stat_counter_cpt) @@ -138,6 +146,18 @@ main_loop (void) counter_left >> 8, counter_left, counter_right >> 8, counter_right); motor_stat_counter_cpt = motor_stat_counter; + if (motor_stat_postrack && !--motor_stat_postrack_cpt) + { + proto_send4 ('X', + postrack_x >> 24, postrack_x >> 16, + postrack_x >> 8, postrack_x); + proto_send4 ('Y', + postrack_y >> 24, postrack_y >> 16, + postrack_y >> 8, postrack_y); + proto_send4 ('A', + postrack_a >> 24, postrack_a >> 16, + postrack_a >> 8, postrack_a); + motor_stat_postrack_cpt = motor_stat_postrack; } /* Misc. */ if ((motor_loop_cpt & 7) == 0) @@ -185,6 +205,9 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) case c ('i', 2): speed_ki = argv[0] << 8 | argv[1]; break; + case c ('f', 2): + postrack_set_footing (argv[0] << 8 | argv[1]); + break; case c ('m', 1): motor_stat_speed_cpt = motor_stat_speed = argv[0]; motor_stat_pwm_cpt = motor_stat_pwm = argv[0]; @@ -195,6 +218,9 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) case c ('t', 1): motor_stat_timer_cpt = motor_stat_timer = argv[0]; break; + case c ('T', 1): + motor_stat_postrack_cpt = motor_stat_postrack = argv[0]; + break; default: proto_send0 ('e'); return; -- cgit v1.2.3