summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-06 23:21:13 +0200
committerNicolas Schodet2008-04-06 23:21:13 +0200
commit9f3ae1c9bec44a32e8a79b9feceac507d683ee1d (patch)
treea5ada39776aa3719e7a28d4c7f1e8da4f23587de
parent15915f2e6fad90be5e491b1603617fcdf6f30a98 (diff)
* digital/asserv/src/asserv:
- added contact simulation support.
-rw-r--r--digital/asserv/src/asserv/simu.host.c34
1 files changed, 32 insertions, 2 deletions
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index 2264e74c..803142dd 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -122,6 +122,34 @@ simu_pos_update (double dl, double dr, double footing)
simu_pos_a = na;
}
+/** Update sensors. */
+static void
+simu_sensor_update (void)
+{
+ static const double sensors[][2] =
+ {
+ { -70.0, 200.0 },
+ { -70.0, -200.0 },
+ { 170.0, 0.0 },
+ };
+ static const double table_width = 3000.0, table_height = 2100.0;
+ PINC = 0;
+ uint8_t bit = 1;
+ unsigned int i;
+ double x, y;
+ for (i = 0; i < UTILS_COUNT (sensors); i++)
+ {
+ /* Compute absolute position. */
+ x = simu_pos_x + cos (simu_pos_a) * sensors[i][0]
+ - sin (simu_pos_a) * sensors[i][1];
+ y = simu_pos_y + sin (simu_pos_a) * sensors[i][0]
+ + cos (simu_pos_a) * sensors[i][1];
+ if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height)
+ PINC |= bit;
+ bit <<= 1;
+ }
+}
+
/** Do a simulation step. */
static void
simu_step (void)
@@ -190,16 +218,18 @@ simu_step (void)
counter_aux0 += counter_aux0_diff;
simu_counter_aux0 = counter_aux0_new;
}
- /* Update position */
+ /* 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. */
+ simu_sensor_update ();
}
/** Send information to the other nodes. */
-void
+static void
simu_send (void)
{
mex_msg_t *m;