summaryrefslogtreecommitdiff
path: root/digital/asserv/src/asserv/models.host.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-04-06 22:13:09 +0200
committerNicolas Schodet2012-04-06 23:55:25 +0200
commit969f9f57656573a8aacc99c2f810392629a67455 (patch)
treed3f1fefd4fd3ecafc734e66a6819e8fb4753bf42 /digital/asserv/src/asserv/models.host.c
parent359f5fa7ca74e70044836c2d735c939fcea47aff (diff)
digital/asserv: move robot specific functions to models.host.c
Diffstat (limited to 'digital/asserv/src/asserv/models.host.c')
-rw-r--r--digital/asserv/src/asserv/models.host.c168
1 files changed, 161 insertions, 7 deletions
diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c
index 72524932..d5c33eef 100644
--- a/digital/asserv/src/asserv/models.host.c
+++ b/digital/asserv/src/asserv/models.host.c
@@ -25,14 +25,38 @@
#define _GNU_SOURCE 1 /* Need ISO C99 features as well. */
#include "common.h"
+#include "modules/utils/utils.h"
+
+#include "io.h"
+
#include "models.host.h"
#include "simu.host.h"
+#include "aux.h"
+
+#include AC_ASSERV_CONTACTS_H
+
#include <math.h>
#include <string.h>
#define NO_CORNER { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } }
+static void
+simu_sensor_update_giboulee (void);
+
+static void
+simu_sensor_update_aquajim (void);
+
+static void
+simu_sensor_update_marcel (void);
+
+static void
+marcel_robot_init (const struct robot_t *robot, struct motor_model_t *main_motor_left,
+ struct motor_model_t *main_motor_right, struct motor_model_t aux_motor[]);
+
+static int
+simu_table_test_robospierre (double p_x, double p_y);
+
/* Gloubi, Efrei 2006. */
static const struct robot_t gloubi_robot =
{
@@ -185,13 +209,6 @@ static const struct robot_t aquajim_robot =
NULL, NO_CORNER, NULL
};
-void
-marcel_robot_init (const struct robot_t *robot, struct motor_model_t *main_motor_left,
- struct motor_model_t *main_motor_right, struct motor_model_t aux_motor[])
-{
- aux_motor[0].m.th_min = 0.0;
-}
-
/* Marcel, APBTeam 2010. */
static const struct robot_t marcel_robot =
{
@@ -336,3 +353,140 @@ models_init (const struct robot_t *robot, motor_model_t *main_motor_left,
robot->init (robot, main_motor_left, main_motor_right, aux_motor);
}
+/** Update sensors for Giboulee. */
+void
+simu_sensor_update_giboulee (void)
+{
+ /** Micro-switch sensors. */
+ static const double sensors[][2] =
+ {
+ { -70.0, 200.0 },
+ { -70.0, -200.0 },
+ { 170.0, 0.0 },
+ };
+ static const uint8_t sensors_bit[] =
+ { IO_BV (CONTACT_BACK_LEFT_IO), IO_BV (CONTACT_BACK_RIGHT_IO),
+ IO_BV (CONTACT_CENTER_IO), };
+ static const double table_width = 3000.0, table_height = 2100.0;
+ PINC = 0;
+ unsigned int i;
+ double x, y;
+ for (i = 0; i < UTILS_COUNT (sensors); i++)
+ {
+ /* Compute absolute position. */
+ simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y);
+ if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height)
+ PINC |= sensors_bit[i];
+ }
+ /** Top zero sensor. */
+ double aa;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ aa = simu_aux_model[i].th / simu_aux_model[i].m.i_G * 3;
+ if (!(cos (aa) > 0 && fabs (sin (aa)) * 80.0 < 7.5))
+ *aux[i].zero_pin |= aux[i].zero_bv;
+ }
+}
+
+/** Update sensors for AquaJim. */
+void
+simu_sensor_update_aquajim (void)
+{
+ /** Micro-switch sensors. */
+ static const double sensors[][2] =
+ {
+ { -150.0, 70.0 },
+ { -150.0, -70.0 },
+ { 150.0, 130.0 },
+ { 150.0, -130.0 },
+ { 150.0, 0.0 },
+ };
+ static const uint8_t sensors_bit[] =
+ { IO_BV (CONTACT_BACK_LEFT_IO), IO_BV (CONTACT_BACK_RIGHT_IO),
+ IO_BV (CONTACT_FRONT_LEFT_IO), IO_BV (CONTACT_FRONT_RIGHT_IO),
+ IO_BV (CONTACT_CENTER_IO), };
+ static const double table_width = 3000.0, table_height = 2100.0;
+ static const double center_zone_radius = 150.0;
+ PINC = 0;
+ unsigned int i;
+ double x, y, cx, cy, ds;
+ for (i = 0; i < UTILS_COUNT (sensors); i++)
+ {
+ /* Compute absolute position. */
+ simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y);
+ cx = table_width / 2 - x;
+ cy = table_height / 2 - y;
+ ds = cx * cx + cy * cy;
+ if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height
+ && ds > center_zone_radius * center_zone_radius)
+ PINC |= sensors_bit[i];
+ }
+ /** Top zero sensors. */
+ double aa = simu_aux_model[0].th / simu_aux_model[0].m.i_G
+ /* Almost open. */
+ + 2 * M_PI / 6 / 10
+ /* Turn at the next hole. */
+ - 2 * M_PI / 3
+ /* Mechanical offset. */
+ + 2 * M_PI * 0x43e / simu_robot->aux_encoder_steps[0] /
+ simu_aux_model[0].m.i_G;
+ double apos = aa / (2 * M_PI / 3);
+ if (apos - floor (apos) > 0.5)
+ PINC |= IO_BV (CONTACT_AUX0_ZERO_IO);
+ if (simu_aux_model[1].th >= 0)
+ PINC |= IO_BV (CONTACT_AUX1_ZERO_IO);
+}
+
+/** Update sensors for Marcel. */
+void
+simu_sensor_update_marcel (void)
+{
+ /** Micro-switch sensors. */
+ static const double sensors[][2] =
+ {
+ { -160.0, 90.0 },
+ { -160.0, -90.0 },
+ { 120.0, 155.0 },
+ { 120.0, -155.0 },
+ };
+ static const uint8_t sensors_bit[] =
+ { IO_BV (CONTACT_BACK_LEFT_IO), IO_BV (CONTACT_BACK_RIGHT_IO),
+ IO_BV (CONTACT_FRONT_LEFT_IO), IO_BV (CONTACT_FRONT_RIGHT_IO), };
+ static const double table_width = 3000.0, table_height = 2100.0;
+ static const double stand_x_min = 1500.0 - 759.5,
+ stand_x_max = 1500.0 + 759.5, stand_y = 2100.0 - 522.0;
+ PINC = 0;
+ unsigned int i;
+ double x, y;
+ for (i = 0; i < UTILS_COUNT (sensors); i++)
+ {
+ /* Compute absolute position. */
+ simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y);
+ if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height
+ && (x < stand_x_min || x >= stand_x_max || y < stand_y))
+ PINC |= sensors_bit[i];
+ }
+ /** Top zero sensors. */
+ if (simu_aux_model[1].th < 0)
+ PINC |= IO_BV (CONTACT_AUX1_ZERO_IO);
+}
+
+static void
+marcel_robot_init (const struct robot_t *robot, struct motor_model_t *main_motor_left,
+ struct motor_model_t *main_motor_right, struct motor_model_t aux_motor[])
+{
+ aux_motor[0].m.th_min = 0.0;
+}
+
+/* Table test for Robospierre. */
+int
+simu_table_test_robospierre (double p_x, double p_y)
+{
+ static const double table_width = 3000.0, table_height = 2100.0;
+ double x, y;
+ simu_compute_absolute_position (p_x, p_y, &x, &y);
+ if (x < 0 || y < 0 || x >= table_width || y >= table_height)
+ return 0;
+ return 1;
+}
+