summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/models.host.c168
-rw-r--r--digital/asserv/src/asserv/simu.host.c140
-rw-r--r--digital/asserv/src/asserv/simu.host.h19
3 files changed, 171 insertions, 156 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;
+}
+
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index 04ade3aa..b222b305 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -27,11 +27,8 @@
#include "modules/host/host.h"
#include "modules/host/mex.h"
-#include "modules/utils/utils.h"
#include "modules/math/fixed/fixed.h"
-#include "io.h"
-
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
@@ -40,14 +37,11 @@
#include <sys/time.h>
#include "cs.h"
-#include "aux.h"
-
-#include AC_ASSERV_CONTACTS_H
#include "models.host.h"
/** Simulate some AVR regs. */
-uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC;
+uint8_t DDRF, PORTB, PORTC, PORTD, PORTE, PORTF, PORTG, PINC;
/** Index of loaded eeprom block. */
int8_t eeprom_loaded = -1;
@@ -142,7 +136,7 @@ simu_pos_update (double dl, double dr, double footing)
}
/** Compute a robot point absolute position. */
-static void
+void
simu_compute_absolute_position (double p_x, double p_y, double *x, double *y)
{
double c = cos (simu_pos_a);
@@ -151,136 +145,6 @@ simu_compute_absolute_position (double p_x, double p_y, double *x, double *y)
*y = simu_pos_y + s * p_x + c * p_y;
}
-/** 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);
-}
-
-/* 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;
-}
-
/** 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 93785186..2d30ce97 100644
--- a/digital/asserv/src/asserv/simu.host.h
+++ b/digital/asserv/src/asserv/simu.host.h
@@ -24,11 +24,17 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "models.host.h"
-extern uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC;
+extern uint8_t DDRF, PORTB, PORTC, PORTD, PORTE, PORTF, PORTG, PINC;
extern double simu_pos_x, simu_pos_y, simu_pos_a;
+extern motor_model_t simu_left_model, simu_right_model,
+ simu_aux_model[AC_ASSERV_AUX_NB];
+
+extern const struct robot_t *simu_robot;
+
void
timer_init (void);
@@ -39,15 +45,6 @@ uint8_t
timer_read (void);
void
-simu_sensor_update_giboulee (void);
-
-void
-simu_sensor_update_aquajim (void);
-
-void
-simu_sensor_update_marcel (void);
-
-int
-simu_table_test_robospierre (double p_x, double p_y);
+simu_compute_absolute_position (double p_x, double p_y, double *x, double *y);
#endif /* simu_host_h */