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.c30
1 files changed, 19 insertions, 11 deletions
diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c
index 1f47de5..64573bb 100644
--- a/n/asserv/src/asserv/simu.host.c
+++ b/n/asserv/src/asserv/simu.host.c
@@ -50,14 +50,17 @@ uint8_t pwm_dir;
struct motor_t simu_left_model, simu_right_model;
-/** Computed simulated position. */
+/** Computed simulated position (mm). */
double simu_pos_x, simu_pos_y, simu_pos_a;
+/** Nearest hole. */
+double simu_nearest_hole;
+
/** Distance between wheels. */
double simu_footing;
/** Holes positions. */
-#define SIMU_HOLES_RADIUS 5
+#define SIMU_HOLES_RADIUS 50
struct simu_hole_t
{
double x, y;
@@ -102,10 +105,10 @@ struct simu_rgb_t
};
static const struct simu_rgb_t simu_rgbs[4] =
{
- { 0, 70 },
- { -5.5, 70 },
- { +5.5, 70 },
- { 0, 0 },
+ { -55, 70 },
+ { +55, 70 },
+ { 0, 65 + 55 },
+ { 0, 65 - 55 },
};
/** Initialise simulation. */
@@ -162,7 +165,8 @@ simu_rgb_update (void)
{
unsigned i, j;
int rgb_x, rgb_y;
- int dx, dy;
+ int dx, dy, d2;
+ int md2 = -1;
PINF &= 0xf0;
for (i = 0; i < UTILS_COUNT (simu_rgbs); i++)
{
@@ -176,13 +180,17 @@ simu_rgb_update (void)
{
dx = simu_holes[j].x - rgb_x;
dy = simu_holes[j].y - rgb_y;
- if (dx * dx + dy * dy < SIMU_HOLES_RADIUS * SIMU_HOLES_RADIUS)
+ d2 = dx * dx + dy * dy;
+ if (d2 < SIMU_HOLES_RADIUS * SIMU_HOLES_RADIUS)
{
PINF |= 1 << i;
break;
}
+ if (md2 == -1 || d2 < md2)
+ md2 = d2;
}
}
+ simu_nearest_hole = md2;
}
/** Do a simulation step. */
@@ -200,7 +208,7 @@ simu_step (void)
old_right_th = simu_right_model.th;
motor_model_step (&simu_left_model);
motor_model_step (&simu_right_model);
- /* Modifie counters. */
+ /* Modify counters. */
counter_left_diff = (simu_left_model.th - old_left_th) / M_2_PI * 500 *
simu_left_model.i_G;
counter_left += counter_left_diff;
@@ -209,9 +217,9 @@ simu_step (void)
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_left_model.i_G * simu_left_model.w_r * 1000,
(simu_right_model.th - old_right_th)
- * simu_right_model.i_G * simu_right_model.w_r);
+ * simu_right_model.i_G * simu_right_model.w_r * 1000);
/* Update RGB sensors. */
simu_rgb_update ();
}