summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/models.host.c13
-rw-r--r--digital/asserv/src/asserv/models.host.h2
-rw-r--r--digital/asserv/src/asserv/simu.host.c54
-rw-r--r--digital/asserv/src/asserv/simu.host.h6
-rw-r--r--digital/asserv/src/asserv/test_motor_model.c11
-rw-r--r--digital/asserv/src/asserv/traj.c2
6 files changed, 80 insertions, 8 deletions
diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c
index 95446784..75a68b1d 100644
--- a/digital/asserv/src/asserv/models.host.c
+++ b/digital/asserv/src/asserv/models.host.c
@@ -22,8 +22,11 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
+
#include "motor_model.host.h"
#include "models.host.h"
+#include "simu.host.h"
#include <math.h>
#include <string.h>
@@ -96,7 +99,7 @@ static const struct robot_t gloubi_robot =
13.0, // approx
/* Whether the encoder is mounted on the main motor (false) or not (true). */
0,
- 0.0, 0.0, { NULL, NULL }, { 0, 0 }
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL
};
/* Taz, APBTeam/Efrei 2005. */
@@ -116,7 +119,7 @@ static const struct robot_t taz_robot =
0.0,
/* Whether the encoder is mounted on the main motor (false) or not (true). */
0,
- 0.0, 0.0, { NULL, NULL }, { 0, 0 }
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL
};
/* TazG, Taz with RE25G motors. */
@@ -136,7 +139,7 @@ static const struct robot_t tazg_robot =
0.0,
/* Whether the encoder is mounted on the main motor (false) or not (true). */
0,
- 0.0, 0.0, { NULL, NULL }, { 0, 0 }
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL
};
/* Giboulée arm model, with a RE25CLL and a 1:10 ratio gearbox. */
@@ -183,6 +186,8 @@ static const struct robot_t giboulee_robot =
{ &giboulee_arm_model, NULL },
/** Number of steps for each auxiliary motor encoder. */
{ 500, 0 },
+ /** Sensor update function. */
+ simu_sensor_update_giboulee,
};
/* AquaJim arm model, with a RE40G and a 1:4 + 15:80 ratio gearbox. */
@@ -247,6 +252,8 @@ static const struct robot_t aquajim_robot =
{ &aquajim_arm_model, &aquajim_elevator_model },
/** Number of steps for each auxiliary motor encoder. */
{ 250, 250 },
+ /** Sensor update function. */
+ simu_sensor_update_aquajim,
};
/* Table of models. */
diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h
index ae2bc21d..403e9cd9 100644
--- a/digital/asserv/src/asserv/models.host.h
+++ b/digital/asserv/src/asserv/models.host.h
@@ -53,6 +53,8 @@ struct robot_t
const struct motor_def_t *aux_motor[AC_ASSERV_AUX_NB];
/** Number of steps for each auxiliary motor encoder. */
int aux_encoder_steps[AC_ASSERV_AUX_NB];
+ /** Sensor update function. */
+ void (*sensor_update) (void);
};
/** Get a pointer to a model by name, or return 0. */
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index f89fabd4..321c9f9d 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -139,9 +139,9 @@ simu_pos_update (double dl, double dr, double footing)
simu_pos_a = na;
}
-/** Update sensors. */
-static void
-simu_sensor_update (void)
+/** Update sensors for Giboulee. */
+void
+simu_sensor_update_giboulee (void)
{
/** Micro-switch sensors. */
static const double sensors[][2] =
@@ -177,6 +177,51 @@ simu_sensor_update (void)
}
}
+/** Update sensors for AquaJim. */
+void
+simu_sensor_update_aquajim (void)
+{
+ /** Micro-switch sensors. */
+ static const double sensors[][2] =
+ {
+ { -150.0, 150.0 },
+ { -150.0, -150.0 },
+ { 150.0, 150.0 },
+ { 150.0, -150.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. */
+ 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];
+ 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;
+ 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);
+}
+
/** Do a simulation step. */
static void
simu_step (void)
@@ -267,7 +312,8 @@ simu_step (void)
/ simu_right_model.m.i_G * simu_robot->wheel_r * 1000,
simu_robot->footing * 1000);
/* Update sensors. */
- simu_sensor_update ();
+ if (simu_robot->sensor_update)
+ simu_robot->sensor_update ();
}
/** Send information to the other nodes. */
diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h
index e5722369..b3b5b0df 100644
--- a/digital/asserv/src/asserv/simu.host.h
+++ b/digital/asserv/src/asserv/simu.host.h
@@ -38,4 +38,10 @@ timer_wait (void);
uint8_t
timer_read (void);
+void
+simu_sensor_update_giboulee (void);
+
+void
+simu_sensor_update_aquajim (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 c5d4ea4a..c6e60c81 100644
--- a/digital/asserv/src/asserv/test_motor_model.c
+++ b/digital/asserv/src/asserv/test_motor_model.c
@@ -68,3 +68,14 @@ main (int argc, char **argv)
simu (m, 1.0);
return 0;
}
+
+void
+simu_sensor_update_giboulee (void)
+{
+}
+
+void
+simu_sensor_update_aquajim (void)
+{
+}
+
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index 6e7c2bc7..92ff94f1 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -151,7 +151,7 @@ traj_ftw (void)
state_main.variant = 2;
#else
/* On host, we must do the job. */
- speed_theta.cons = speed / 2;
+ speed_theta.cons = speed / 4;
if (left)
speed_alpha.cons = speed / 2;
else