summaryrefslogtreecommitdiff
path: root/digital/mimot/src/dirty/simu.host.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/mimot/src/dirty/simu.host.c')
-rw-r--r--digital/mimot/src/dirty/simu.host.c210
1 files changed, 117 insertions, 93 deletions
diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c
index af07d744..6aec1788 100644
--- a/digital/mimot/src/dirty/simu.host.c
+++ b/digital/mimot/src/dirty/simu.host.c
@@ -39,7 +39,6 @@
#include <string.h>
#include "pwm.h"
-#include "aux.h"
#include "contacts.h"
@@ -50,15 +49,18 @@
uint8_t PORTB, PORTC, PORTD, PINC;
/** Overall counter values. */
-uint16_t counter_aux[AC_ASSERV_AUX_NB];
+uint16_t counter_left, counter_right;
/** Counter differences since last update.
* Maximum of 9 significant bits, sign included. */
-int16_t counter_aux_diff[AC_ASSERV_AUX_NB];
+int16_t counter_left_diff, counter_right_diff;
+/** Overall uncorrected counter values. */
+static int32_t counter_right_raw;
+/** Correction factor (f8.24). */
+uint32_t counter_right_correction = 1L << 24;
/** PWM control states. */
-struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = {
- PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1)
-};
+struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left);
+struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right);
/** PWM reverse directions. */
uint8_t pwm_reverse;
@@ -66,52 +68,25 @@ uint8_t pwm_reverse;
const struct robot_t *simu_robot;
/** Motor models. */
-struct motor_t simu_aux_model[AC_ASSERV_AUX_NB];
+struct motor_t simu_left_model, simu_right_model;
+
+/** Computed simulated position (mm, rad). */
+double simu_pos_x, simu_pos_y, simu_pos_a;
/** Full counter values. */
-uint32_t simu_counter_aux[AC_ASSERV_AUX_NB];
+uint32_t simu_counter_left, simu_counter_right;
+double simu_counter_left_th, simu_counter_right_th;
/** Use mex. */
int simu_mex;
/** Mex message types. */
-uint8_t simu_mex_aux;
-uint8_t simu_mex_limits;
+uint8_t simu_mex_position;
+uint8_t simu_mex_pwm;
/** Counter to limit the interval between information is sent. */
int simu_send_cpt;
-static void
-simu_handle_limits__update_limit (double *th_limit, int32_t limit_int,
- double i_G, int sign)
-{
- if (limit_int == -1)
- /* No change. */
- ;
- else if (limit_int == -2)
- /* INFINITY. */
- *th_limit = sign > 0 ? INFINITY : -INFINITY;
- else
- *th_limit = (double) limit_int / 1024.0 * i_G;
-}
-
-static void
-simu_handle_limits (void *user, mex_msg_t *msg)
-{
- int i;
- int32_t limit_min_int, limit_max_int;
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- {
- mex_msg_pop (msg, "ll", &limit_min_int, &limit_max_int);
- simu_handle_limits__update_limit (&simu_aux_model[i].m.th_min,
- limit_min_int,
- simu_aux_model[i].m.i_G, -1);
- simu_handle_limits__update_limit (&simu_aux_model[i].m.th_max,
- limit_max_int,
- simu_aux_model[i].m.i_G, +1);
- }
-}
-
/** Initialise simulation. */
static void
simu_init (void)
@@ -128,9 +103,8 @@ simu_init (void)
simu_send_cpt = simu_mex;
mex_node_connect ();
mex_instance = host_get_instance ("mimot0", 0);
- simu_mex_aux = mex_node_reservef ("%s:aux", mex_instance);
- simu_mex_limits = mex_node_reservef ("%s:limits", mex_instance);
- mex_node_register (simu_mex_limits, simu_handle_limits, 0);
+ simu_mex_position = mex_node_reservef ("%s:position", mex_instance);
+ simu_mex_pwm = mex_node_reservef ("%s:pwm", mex_instance);
}
if (argc != 1)
{
@@ -143,50 +117,94 @@ simu_init (void)
fprintf (stderr, "unknown model name: %s\n", argv[0]);
exit (1);
}
- models_init (simu_robot, simu_aux_model);
+ models_init (simu_robot, &simu_left_model, &simu_right_model);
+ simu_pos_x = simu_pos_y = simu_pos_a = 0;
}
-/** Update sensors for Marcel. */
-void
-simu_sensor_update_marcel (void)
+/** Update simulation position. */
+static void
+simu_pos_update (double dl, double dr, double footing)
{
+ double d = 0.5 * (dl + dr);
+ double da = (dr - dl) / footing;
+ double na = simu_pos_a + da;
+ if (da < 0.0001 && da > -0.0001)
+ {
+ /* Avoid a division by zero when angle is too small. */
+ double a = simu_pos_a + da * 0.5;
+ simu_pos_x += d * cos (a);
+ simu_pos_y += d * sin (a);
+ }
+ else
+ {
+ /* Radius of turn is d / da. */
+ simu_pos_x += (sin (na) - sin (simu_pos_a)) * d / da;
+ simu_pos_y += (cos (simu_pos_a) - cos (na)) * d / da;
+ }
+ simu_pos_a = na;
}
/** Do a simulation step. */
static void
simu_step (void)
{
- int i;
- double old_aux_th[AC_ASSERV_AUX_NB];
+ double old_left_th, old_right_th;
/* Convert pwm value into voltage. */
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- simu_aux_model[i].u = simu_aux_model[i].m.u_max
- * ((double) pwm_aux[i].cur / (PWM_MAX + 1));
+ simu_left_model.u = simu_left_model.m.u_max
+ * ((double) pwm_left.cur / (PWM_MAX + 1));
+ simu_right_model.u = simu_right_model.m.u_max
+ * ((double) pwm_right.cur / (PWM_MAX + 1));
/* Make one step. */
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ 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);
+ /* Modify counters. */
+ uint32_t counter_left_new;
+ uint32_t counter_right_new;
+ if (!simu_robot->encoder_separated)
{
- old_aux_th[i] = simu_aux_model[i].th;
- if (simu_robot->aux_motor[i])
- motor_model_step (&simu_aux_model[i]);
+ counter_left_new = simu_left_model.th / (2*M_PI)
+ * simu_robot->main_encoder_steps;
+ counter_right_new = simu_right_model.th / (2*M_PI)
+ * simu_robot->main_encoder_steps;
}
- /* Update auxiliary counter. */
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ else
{
- if (simu_robot->aux_motor[i])
- {
- uint32_t counter_aux_new = simu_aux_model[i].th / (2*M_PI)
- * simu_robot->aux_encoder_steps[i];
- counter_aux_diff[i] = counter_aux_new - simu_counter_aux[i];
- counter_aux[i] += counter_aux_diff[i];
- simu_counter_aux[i] = counter_aux_new;
- }
- else
- {
- counter_aux_diff[i] = 0;
- counter_aux[i] = 0;
- simu_counter_aux[i] = 0;
- }
+ /* Thanks Thalès. */
+ double left_diff = (simu_left_model.th - old_left_th)
+ / simu_left_model.m.i_G * simu_robot->wheel_r;
+ double right_diff = (simu_right_model.th - old_right_th)
+ / simu_right_model.m.i_G * simu_robot->wheel_r;
+ double sum = left_diff + right_diff;
+ double diff = (left_diff - right_diff)
+ * (simu_robot->encoder_footing / simu_robot->footing);
+ double left_enc_diff = 0.5 * (sum + diff);
+ double right_enc_diff = 0.5 * (sum - diff);
+ simu_counter_left_th += left_enc_diff / simu_robot->encoder_wheel_r;
+ simu_counter_right_th += right_enc_diff / simu_robot->encoder_wheel_r;
+ counter_left_new = simu_counter_left_th / (2*M_PI)
+ * simu_robot->main_encoder_steps;
+ counter_right_new = simu_counter_right_th / (2*M_PI)
+ * simu_robot->main_encoder_steps;
}
+ /* Update an integer counter. */
+ counter_left_diff = counter_left_new - simu_counter_left;
+ counter_left += counter_left_diff;
+ simu_counter_left = counter_left_new;
+ counter_right_diff = counter_right_new - simu_counter_right;
+ counter_right_raw += counter_right_diff;
+ uint16_t right_new = fixed_mul_f824 (counter_right_raw,
+ counter_right_correction);
+ counter_right_diff = (int16_t) (right_new - counter_right);
+ counter_right = right_new;
+ simu_counter_right = counter_right_new;
+ /* Update position. */
+ simu_pos_update ((simu_left_model.th - old_left_th)
+ / simu_left_model.m.i_G * simu_robot->wheel_r * 1000,
+ (simu_right_model.th - old_right_th)
+ / simu_right_model.m.i_G * simu_robot->wheel_r * 1000,
+ simu_robot->footing * 1000);
/* Update sensors. */
if (simu_robot->sensor_update)
simu_robot->sensor_update ();
@@ -197,31 +215,37 @@ static void
simu_send (void)
{
static int first = 1;
- int i;
mex_msg_t *m;
- /* Send Aux position. */
- static int32_t simu_aux_model_sent[AC_ASSERV_AUX_NB];
- int32_t simu_aux_model_to_send[AC_ASSERV_AUX_NB];
- int simu_aux_model_changed = 0;
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ /* Send position. */
+ static int16_t simu_pos_x_sent, simu_pos_y_sent;
+ static int32_t simu_pos_a_sent;
+ int16_t simu_pos_x_to_send = simu_pos_x;
+ int16_t simu_pos_y_to_send = simu_pos_y;
+ int32_t simu_pos_a_to_send = 1024.0 * simu_pos_a;
+ if (first
+ || simu_pos_x_to_send != simu_pos_x_sent
+ || simu_pos_y_to_send != simu_pos_y_sent
+ || simu_pos_a_to_send != simu_pos_a_sent)
{
- simu_aux_model_to_send[i] = 1024.0 * simu_aux_model[i].th
- / simu_aux_model[i].m.i_G;
- if (!first && simu_aux_model_to_send[i] != simu_aux_model_sent[i])
- simu_aux_model_changed = 1;
+ m = mex_msg_new (simu_mex_position);
+ mex_msg_push (m, "hhl", simu_pos_x_to_send, simu_pos_y_to_send,
+ simu_pos_a_to_send);
+ mex_node_send (m);
+ simu_pos_x_sent = simu_pos_x_to_send;
+ simu_pos_y_sent = simu_pos_y_to_send;
+ simu_pos_a_sent = simu_pos_a_to_send;
}
- if (first || simu_aux_model_changed)
+ /* Send PWM. */
+ static int16_t pwm_left_sent, pwm_right_sent;
+ if (first
+ || pwm_left_sent == pwm_left.cur
+ || pwm_right_sent == pwm_right.cur)
{
- m = mex_msg_new (simu_mex_aux);
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- {
- if (simu_robot->aux_motor[i])
- mex_msg_push (m, "l", simu_aux_model_to_send[i]);
- else
- mex_msg_push (m, "l", 0);
- simu_aux_model_sent[i] = simu_aux_model_to_send[i];
- }
+ m = mex_msg_new (simu_mex_pwm);
+ mex_msg_push (m, "hh", pwm_left.cur, pwm_right.cur);
mex_node_send (m);
+ pwm_left_sent = pwm_left.cur;
+ pwm_right_sent = pwm_right.cur;
}
/* First send done. */
first = 0;