summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/simu.host.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/asserv/simu.host.c')
-rw-r--r--n/asserv/src/asserv/simu.host.c59
1 files changed, 57 insertions, 2 deletions
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: <contact@ni.fr.eu.org>
* }}} */
#include "common.h"
+#include "modules/host/host.h"
#include "simu.host.h"
+#include "motor_model.host.h"
+#include "models.host.h"
+
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
/** 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. */