summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-01 23:57:04 +0200
committerNicolas Schodet2010-04-01 23:57:04 +0200
commita4b92cbfb7aefbe35776967d0e249538e9a6d548 (patch)
tree7cef05435b48f0fc0b26730136615e9b6032e10b /digital/asserv
parentfd3bd15a200b9b82a5893b533d91706255ed581c (diff)
digital/asserv: add 2010 robot model, closes #127
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/models.host.c30
-rw-r--r--digital/asserv/src/asserv/simu.host.c34
-rw-r--r--digital/asserv/src/asserv/simu.host.h3
-rw-r--r--digital/asserv/src/asserv/test_motor_model.c5
4 files changed, 72 insertions, 0 deletions
diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c
index 75a68b1d..08ae3804 100644
--- a/digital/asserv/src/asserv/models.host.c
+++ b/digital/asserv/src/asserv/models.host.c
@@ -256,6 +256,35 @@ static const struct robot_t aquajim_robot =
simu_sensor_update_aquajim,
};
+/* Marcel, APBTeam 2010. */
+static const struct robot_t marcel_robot =
+{
+ /* Main motors. */
+ &amax32ghp_model,
+ /* Number of steps on the main motors encoders. */
+ 2500,
+ /* Wheel radius (m). */
+ 0.065 / 2,
+ /* Distance between the wheels (m). */
+ 0.16,
+ /* Weight of the robot (kg). */
+ 10.0,
+ /* Distance of the gravity center from the center of motors axis (m). */
+ 0.0,
+ /* Whether the encoder is mounted on the main motor (false) or not (true). */
+ 1,
+ /** Encoder wheel radius (m). */
+ 0.063 / 2,
+ /** Distance between the encoders wheels (m). */
+ 0.28,
+ /** Auxiliary motors, NULL if not present. */
+ { NULL, NULL },
+ /** Number of steps for each auxiliary motor encoder. */
+ { 0, 0 },
+ /** Sensor update function. */
+ simu_sensor_update_marcel,
+};
+
/* Table of models. */
static const struct
{
@@ -267,6 +296,7 @@ static const struct
{ "tazg", &tazg_robot },
{ "giboulee", &giboulee_robot },
{ "aquajim", &aquajim_robot },
+ { "marcel", &marcel_robot },
{ 0, 0 }
};
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index 07e441ea..15bb1ef9 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -234,6 +234,40 @@ simu_sensor_update_aquajim (void)
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. */
+ 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
+ && (x < stand_x_min || x >= stand_x_max || y < stand_y))
+ PINC |= sensors_bit[i];
+ }
+}
+
/** Do a simulation step. */
static void
simu_step (void)
diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h
index b3b5b0df..143d3430 100644
--- a/digital/asserv/src/asserv/simu.host.h
+++ b/digital/asserv/src/asserv/simu.host.h
@@ -44,4 +44,7 @@ simu_sensor_update_giboulee (void);
void
simu_sensor_update_aquajim (void);
+void
+simu_sensor_update_marcel (void);
+
#endif /* simu_host_h */
diff --git a/digital/asserv/src/asserv/test_motor_model.c b/digital/asserv/src/asserv/test_motor_model.c
index c6e60c81..706aa31b 100644
--- a/digital/asserv/src/asserv/test_motor_model.c
+++ b/digital/asserv/src/asserv/test_motor_model.c
@@ -79,3 +79,8 @@ simu_sensor_update_aquajim (void)
{
}
+void
+simu_sensor_update_marcel (void)
+{
+}
+