From 19e614348483e09ddb2acca5fcf905ec0c582625 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 21 Apr 2008 00:30:41 +0200 Subject: * digital/asserv/src/asserv: - added find zero auxiliary command. --- digital/asserv/src/asserv/simu.host.c | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'digital/asserv/src/asserv/simu.host.c') diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 8b499123..64fc38db 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -131,6 +131,7 @@ simu_pos_update (double dl, double dr, double footing) static void simu_sensor_update (void) { + /** Micro-switch sensors. */ static const double sensors[][2] = { { -70.0, 200.0 }, @@ -153,6 +154,11 @@ simu_sensor_update (void) PINC |= bit; bit <<= 1; } + /** Top zero sensor. */ + double aa = simu_aux0_model.th / simu_aux0_model.m.i_G * 3; + if (!(cos (aa) > 0 && fabs (sin (aa)) * 80.0 < 7.5)) + PINC |= bit; + bit <<= 1; } /** Do a simulation step. */ -- cgit v1.2.3