From c4395e067140b7bbc40a20c4d8d720d30b096033 Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 11 Mar 2006 12:33:39 +0000 Subject: Ajout de la simulation sur host avec simulation des moteurs. --- n/asserv/src/asserv/simu.host.c | 59 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 57 insertions(+), 2 deletions(-) (limited to 'n/asserv/src/asserv/simu.host.c') diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c index 5b2b6d4..e7cc4ee 100644 --- a/n/asserv/src/asserv/simu.host.c +++ b/n/asserv/src/asserv/simu.host.c @@ -22,7 +22,14 @@ * Email: * }}} */ #include "common.h" +#include "modules/host/host.h" #include "simu.host.h" +#include "motor_model.host.h" +#include "models.host.h" + +#include +#include +#include /** Simulate some AVR regs. */ uint8_t DDRD, PORTD, PORTA, PORTC, PINA; @@ -40,19 +47,67 @@ int16_t pwm_left, pwm_right; * on port B. */ uint8_t pwm_dir; -/* +AutoDec */ -/* -AutoDec */ +struct motor_t simu_left_model, simu_right_model; + +/** Initialise simulation. */ +static void +simu_init (void) +{ + int argc; + char **argv; + const struct motor_t *m; + host_get_program_arguments (&argc, &argv); + if (argc != 1) + { + fprintf (stderr, "need model name as first argument\n"); + exit (1); + } + m = models_get (argv[0]); + if (!m) + { + fprintf (stderr, "unknown model name: %s\n", argv[0]); + exit (1); + } + simu_left_model = *m; + simu_right_model = *m; +} + +/** Do a simulation step. */ +static void +simu_step (void) +{ + double old_left_th, old_right_th; + /* Convert pwm value into voltage. */ + assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX); + assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX); + simu_left_model.u = (double) pwm_left / PWM_MAX; + simu_right_model.u = (double) pwm_right / PWM_MAX; + /* Make one step. */ + old_left_th = simu_left_model.th; + old_right_th = simu_right_model.th; + motor_model_step (&simu_left_model); + motor_model_step (&simu_right_model); + /* Modifie counters. */ + counter_left_diff = (simu_left_model.th - old_left_th) / M_2_PI * 500 * + simu_left_model.i_G; + counter_left += counter_left_diff; + counter_right_diff = (simu_right_model.th - old_right_th) / M_2_PI * 500 * + simu_right_model.i_G; + counter_right += counter_right_diff; +} /** Initialise the timer. */ void timer_init (void) { + simu_init (); } /** Wait for timer overflow. */ void timer_wait (void) { + simu_step (); } /** Read timer value. Used for performance analysis. */ -- cgit v1.2.3