summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-17 01:58:19 +0200
committerNicolas Schodet2011-05-17 01:58:19 +0200
commit9d26b60d08f937ba49962ebfeaa3664f5527bf1a (patch)
tree1dfdf73eb6ba5cb84eb5a5e556f0ac57ad8f7e5a /digital/asserv
parent201aa07cbd28c6be8c4003f126b1ae258f859c68 (diff)
digital/asserv: add robospierre model
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/models.host.c65
-rw-r--r--digital/asserv/src/asserv/models.host.h7
-rw-r--r--digital/asserv/src/asserv/simu.host.c76
-rw-r--r--digital/asserv/src/asserv/simu.host.h3
-rw-r--r--digital/asserv/src/asserv/test_motor_model.c6
5 files changed, 136 insertions, 21 deletions
diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c
index f4d8ca5a..ae123585 100644
--- a/digital/asserv/src/asserv/models.host.c
+++ b/digital/asserv/src/asserv/models.host.c
@@ -32,6 +32,8 @@
#include <math.h>
#include <string.h>
+#define NO_CORNER { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } }
+
/* RE25CLL with 1:10 gearbox model. */
static const struct motor_def_t re25cll_model =
{
@@ -89,6 +91,25 @@ static const struct motor_def_t amax32ghp_model =
-INFINITY, +INFINITY,
};
+/* Faulhaber 2657 and a 1:9.7 ratio gearbox. */
+static const struct motor_def_t faulhaber_2657_model =
+{
+ /* Motor characteristics. */
+ 274 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
+ 34.8 / 1000, /* Torque constant (N.m/A). */
+ 0, /* Bearing friction (N.m/(rad/s)). */
+ 2.84, /* Terminal resistance (Ohm). */
+ 0.380 / 1000, /* Terminal inductance (H). */
+ 24.0, /* Maximum voltage (V). */
+ /* Gearbox characteristics. */
+ 9.7, /* Gearbox ratio. */
+ 0.80, /* Gearbox efficiency. */
+ /* Load characteristics. */
+ 0.0, /* Load (kg.m^2). */
+ /* Hardware limits. */
+ 0.0, +INFINITY,
+};
+
/* Gloubi, Efrei 2006. */
static const struct robot_t gloubi_robot =
{
@@ -106,7 +127,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 }, NULL
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER
};
/* Taz, APBTeam/Efrei 2005. */
@@ -126,7 +147,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 }, NULL
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER
};
/* TazG, Taz with RE25G motors. */
@@ -146,7 +167,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 }, NULL
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER
};
/* Giboulée arm model, with a RE25CLL and a 1:10 ratio gearbox. */
@@ -197,6 +218,7 @@ static const struct robot_t giboulee_robot =
{ 500, 0 },
/** Sensor update function. */
simu_sensor_update_giboulee,
+ NULL, NO_CORNER,
};
/* AquaJim arm model, with a RE40G and a 1:4 + 15:80 ratio gearbox. */
@@ -267,6 +289,7 @@ static const struct robot_t aquajim_robot =
{ 250, 250 },
/** Sensor update function. */
simu_sensor_update_aquajim,
+ NULL, NO_CORNER,
};
/* Marcel elevator model, with a Faulhaber 2657 and a 1:9.7 ratio gearbox. */
@@ -338,6 +361,41 @@ static const struct robot_t marcel_robot =
{ 256, 250 },
/** Sensor update function. */
simu_sensor_update_marcel,
+ NULL, NO_CORNER,
+};
+
+/* Robospierre, APBTeam 2011. */
+static const struct robot_t robospierre_robot =
+{
+ /* Main motors. */
+ &faulhaber_2657_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. */
+ NULL,
+ /** Table test function, return false if given robot point is not in
+ * table. */
+ simu_table_test_robospierre,
+ /** Robot corners, from front left, then clockwise. */
+ { { 150, 50 }, { 150, -50 }, { -150, -50 }, { -150, 50 } },
};
/* Table of models. */
@@ -352,6 +410,7 @@ static const struct
{ "giboulee", &giboulee_robot },
{ "aquajim", &aquajim_robot },
{ "marcel", &marcel_robot },
+ { "robospierre", &robospierre_robot },
{ 0, 0 }
};
diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h
index 403e9cd9..d8f1dcb8 100644
--- a/digital/asserv/src/asserv/models.host.h
+++ b/digital/asserv/src/asserv/models.host.h
@@ -27,6 +27,8 @@
#define ECHANT_PERIOD (1.0 / (14745600.0 / 256 / 256))
+#define CORNERS_NB 4
+
/** Define a robot and its peripherals.
* Encoder characteristics are defined at gearbox output. */
struct robot_t
@@ -55,6 +57,11 @@ struct robot_t
int aux_encoder_steps[AC_ASSERV_AUX_NB];
/** Sensor update function. */
void (*sensor_update) (void);
+ /** Table test function, return false if given robot point is not in
+ * table. */
+ int (*table_test) (double p_x, double p_y);
+ /** Robot corners, from front left, then clockwise. */
+ double corners[CORNERS_NB][2];
};
/** 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 10cef711..d9a97fe2 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -154,6 +154,16 @@ simu_pos_update (double dl, double dr, double footing)
simu_pos_a = na;
}
+/** Compute a robot point absolute position. */
+static void
+simu_compute_absolute_position (double p_x, double p_y, double *x, double *y)
+{
+ double c = cos (simu_pos_a);
+ double s = sin (simu_pos_a);
+ *x = simu_pos_x + c * p_x - s * p_y;
+ *y = simu_pos_y + s * p_x + c * p_y;
+}
+
/** Update sensors for Giboulee. */
void
simu_sensor_update_giboulee (void)
@@ -175,10 +185,7 @@ simu_sensor_update_giboulee (void)
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];
+ 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];
}
@@ -217,10 +224,7 @@ simu_sensor_update_aquajim (void)
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];
+ 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;
@@ -268,10 +272,7 @@ simu_sensor_update_marcel (void)
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];
+ 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];
@@ -281,6 +282,18 @@ simu_sensor_update_marcel (void)
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)
@@ -306,6 +319,39 @@ simu_step (void)
if (simu_robot->aux_motor[i])
motor_model_step (&simu_aux_model[i]);
}
+ /* Update position. */
+ double old_pos_x = simu_pos_x, old_pos_y = simu_pos_y,
+ old_pos_a = simu_pos_a;
+ simu_pos_update ((simu_left_model.th - old_left_th)
+ / simu_left_model.m.i_G * simu_robot->wheel_r * 1000,
+ (simu_right_model.th - old_right_th)
+ / simu_right_model.m.i_G * simu_robot->wheel_r * 1000,
+ simu_robot->footing * 1000);
+ /* Check robot is still on the table. */
+ if (simu_robot->table_test)
+ {
+ static int old_out = 1;
+ int out = 0;
+ for (i = 0; i < CORNERS_NB; i++)
+ {
+ if (!simu_robot->table_test (simu_robot->corners[i][0],
+ simu_robot->corners[i][1]))
+ out = 1;
+ }
+ /* If out, cancel movement. */
+ if (out && !old_out)
+ {
+ simu_pos_x = old_pos_x;
+ simu_pos_y = old_pos_y;
+ simu_pos_a = old_pos_a;
+ simu_left_model.th = old_left_th;
+ simu_right_model.th = old_right_th;
+ }
+ else
+ {
+ old_out = out;
+ }
+ }
/* Modify counters. */
uint32_t counter_left_new;
uint32_t counter_right_new;
@@ -364,12 +410,6 @@ simu_step (void)
simu_counter_aux[i] = 0;
}
}
- /* Update position. */
- simu_pos_update ((simu_left_model.th - old_left_th)
- / simu_left_model.m.i_G * simu_robot->wheel_r * 1000,
- (simu_right_model.th - old_right_th)
- / simu_right_model.m.i_G * simu_robot->wheel_r * 1000,
- simu_robot->footing * 1000);
/* Update sensors. */
if (simu_robot->sensor_update)
simu_robot->sensor_update ();
diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h
index 143d3430..93785186 100644
--- a/digital/asserv/src/asserv/simu.host.h
+++ b/digital/asserv/src/asserv/simu.host.h
@@ -47,4 +47,7 @@ simu_sensor_update_aquajim (void);
void
simu_sensor_update_marcel (void);
+int
+simu_table_test_robospierre (double p_x, double p_y);
+
#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 706aa31b..044bab2e 100644
--- a/digital/asserv/src/asserv/test_motor_model.c
+++ b/digital/asserv/src/asserv/test_motor_model.c
@@ -84,3 +84,9 @@ simu_sensor_update_marcel (void)
{
}
+int
+simu_table_test_robospierre (double p_x, double p_y)
+{
+ return 0;
+}
+