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.c37
1 files changed, 23 insertions, 14 deletions
diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c
index 64573bb..bb204c1 100644
--- a/n/asserv/src/asserv/simu.host.c
+++ b/n/asserv/src/asserv/simu.host.c
@@ -103,12 +103,13 @@ struct simu_rgb_t
{
double x, y;
};
-static const struct simu_rgb_t simu_rgbs[4] =
+static const struct simu_rgb_t simu_rgbs[5] =
{
- { -55, 70 },
- { +55, 70 },
- { 0, 65 + 55 },
- { 0, 65 - 55 },
+ { 65, +55 },
+ { 65, -55 },
+ { 65 + 45, 0 },
+ { 65 - 45, 0 },
+ { 65, 0 },
};
/** Initialise simulation. */
@@ -164,9 +165,9 @@ static void
simu_rgb_update (void)
{
unsigned i, j;
- int rgb_x, rgb_y;
- int dx, dy, d2;
- int md2 = -1;
+ double rgb_x, rgb_y;
+ double dx, dy, d2;
+ double md2 = -1;
PINF &= 0xf0;
for (i = 0; i < UTILS_COUNT (simu_rgbs); i++)
{
@@ -181,16 +182,24 @@ simu_rgb_update (void)
dx = simu_holes[j].x - rgb_x;
dy = simu_holes[j].y - rgb_y;
d2 = dx * dx + dy * dy;
- if (d2 < SIMU_HOLES_RADIUS * SIMU_HOLES_RADIUS)
+ if (i != UTILS_COUNT (simu_rgbs) - 1)
{
- PINF |= 1 << i;
- break;
+ /* Rgb. */
+ if (d2 < SIMU_HOLES_RADIUS * SIMU_HOLES_RADIUS)
+ {
+ PINF |= 1 << i;
+ break;
+ }
+ }
+ else
+ {
+ /* Exit hole. */
+ if (md2 == -1 || d2 < md2)
+ md2 = d2;
}
- if (md2 == -1 || d2 < md2)
- md2 = d2;
}
}
- simu_nearest_hole = md2;
+ simu_nearest_hole = sqrt (md2);
}
/** Do a simulation step. */