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.c18
1 files changed, 18 insertions, 0 deletions
diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c
index 1035af1..01020d1 100644
--- a/n/asserv/src/asserv/simu.host.c
+++ b/n/asserv/src/asserv/simu.host.c
@@ -49,6 +49,9 @@ uint8_t pwm_dir;
struct motor_t simu_left_model, simu_right_model;
+/** Computed simulated position. */
+double simu_pos_x, simu_pos_y;
+
/** Initialise simulation. */
static void
simu_init (void)
@@ -70,6 +73,16 @@ simu_init (void)
}
simu_left_model = *m;
simu_right_model = *m;
+ simu_pos_x = simu_pos_y = 0;
+}
+
+/** Update simulation position. */
+static void
+simu_pos_update (double dl, double dr)
+{
+
+ double d = 0.5 * (dl + dr);
+ double da;
}
/** Do a simulation step. */
@@ -94,6 +107,11 @@ simu_step (void)
counter_right_diff = (simu_right_model.th - old_right_th) / M_2_PI * 500 *
simu_right_model.i_G;
counter_right += counter_right_diff;
+ /* Update position */
+ simu_pos_update ((simu_left_model.th - old_left_th)
+ * simu_left_model.i_G * simu_left_model.w_r,
+ (simu_right_model.th - old_right_th)
+ * simu_right_model.i_G * simu_right_model.w_r);
}
/** Initialise the timer. */