summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2006-05-07 23:38:01 +0000
committerschodet2006-05-07 23:38:01 +0000
commitbb2dc882b10243fb0301379be0f1dbd43a393f49 (patch)
tree53965ac117cb9fa1a7f8a246e385cc43de5c4f56
parentc5ec7a7a497328a9ac16f58464bdba2760689218 (diff)
Ajout de trou partout !
-rw-r--r--n/asserv/src/asserv/avrconfig.h2
-rw-r--r--n/asserv/src/asserv/eeprom.avr.c3
-rw-r--r--n/asserv/src/asserv/main.c66
-rw-r--r--n/asserv/src/asserv/postrack.c4
-rw-r--r--n/asserv/src/asserv/simu.host.c30
-rw-r--r--n/asserv/src/asserv/simu.host.h8
-rw-r--r--n/asserv/src/asserv/traj.c89
7 files changed, 179 insertions, 23 deletions
diff --git a/n/asserv/src/asserv/avrconfig.h b/n/asserv/src/asserv/avrconfig.h
index 2c2c103..5d316a2 100644
--- a/n/asserv/src/asserv/avrconfig.h
+++ b/n/asserv/src/asserv/avrconfig.h
@@ -75,7 +75,7 @@
/* proto - Protocol module. */
/** Maximum argument size. */
-#define AC_PROTO_ARGS_MAX_SIZE 8
+#define AC_PROTO_ARGS_MAX_SIZE 12
/** Callback function name. */
#define AC_PROTO_CALLBACK proto_callback
/** Putchar function name. */
diff --git a/n/asserv/src/asserv/eeprom.avr.c b/n/asserv/src/asserv/eeprom.avr.c
index 0ef63a0..3abba92 100644
--- a/n/asserv/src/asserv/eeprom.avr.c
+++ b/n/asserv/src/asserv/eeprom.avr.c
@@ -24,7 +24,8 @@
* }}} */
#include <avr/eeprom.h>
-#define EEPROM_KEY 0x43
+/* Change the eeprom key each time you change eeprom format. */
+#define EEPROM_KEY 0x45
#define EEPROM_START 256
/* +AutoDec */
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index 2a19877..aec9c8e 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -43,6 +43,7 @@
#include "pos.c"
#include "speed.c"
#include "postrack.c"
+#include "traj.c"
#ifndef HOST
# include "eeprom.avr.c"
#endif
@@ -78,6 +79,11 @@ uint8_t main_stat_timer, main_stat_timer_cpt;
/** Print input port. */
uint8_t main_print_pin, main_print_pin_cpt;
+#ifdef HOST
+/** Simulation data. */
+uint8_t main_simu, main_simu_cpt;
+#endif /* HOST */
+
/** Record timer value at different stage of computing. Used for performance
* analisys. */
uint8_t main_timer[6];
@@ -104,6 +110,7 @@ main (int argc, char **argv)
timer_init ();
counter_init ();
uart0_init ();
+ postrack_init ();
proto_send0 ('z');
sei ();
while (1)
@@ -129,6 +136,9 @@ main_loop (void)
main_timer[2] = timer_read ();
/* Compute absolute position. */
postrack_update ();
+ /* Compute trajectory. */
+ if (main_mode >= 3)
+ traj_update ();
/* Speed control. */
if (main_mode >= 2)
speed_update ();
@@ -155,6 +165,17 @@ main_loop (void)
pos_alpha_e_old, pos_alpha_int);
main_stat_pos_cpt = main_stat_pos;
}
+#ifdef HOST
+ if (main_simu && !--main_simu_cpt)
+ {
+ proto_send4w ('Y', (uint16_t) simu_pos_x,
+ (uint16_t) simu_pos_y,
+ (uint16_t) (simu_pos_a * 1024),
+ (uint16_t) simu_nearest_hole);
+ proto_send3b ('Z', traj_mode, 0, 0);
+ main_simu_cpt = main_simu;
+ }
+#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
proto_send2w ('W', pwm_left, pwm_right);
@@ -231,20 +252,28 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_theta_cons = args[0] << 8;
speed_alpha_cons = args[1] << 8;
break;
- case c ('s', 5):
+ case c ('s', 9):
/* Set speed controlled position consign.
- * - w: theta consign offset.
- * - w: alpha consign offset.
+ * - d: theta consign offset.
+ * - d: alpha consign offset.
* - b: sequence number. */
- if (args[4] == main_sequence)
+ if (args[8] == main_sequence)
break;
main_mode = 2;
speed_pos = 1;
speed_theta_pos_cons = pos_theta_cons;
- speed_theta_pos_cons += v8_to_v16 (args[0], args[1]);
+ speed_theta_pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
speed_alpha_pos_cons = pos_alpha_cons;
- speed_alpha_pos_cons += v8_to_v16 (args[2], args[3]);
- main_sequence = args[4];
+ speed_alpha_pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]);
+ main_sequence = args[8];
+ break;
+ case c ('h', 1):
+ /* Find a hole.
+ * - b: sequence number. */
+ if (args[0] == main_sequence)
+ break;
+ main_mode = 3;
+ traj_mode = 10;
break;
/* Stats.
* - b: interval between stats. */
@@ -276,6 +305,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Input port stats. */
main_print_pin_cpt = main_print_pin = args[0];
break;
+#ifdef HOST
+ case c ('Y', 1):
+ /* Simulation data. */
+ main_simu_cpt = main_simu = args[0];
+ break;
+#endif /* HOST */
default:
/* Params. */
if (cmd == 'p')
@@ -383,6 +418,23 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
return;
}
}
+#ifdef HOST
+ else if (cmd == 'y')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('X', 7):
+ /* Set simulated position.
+ * - w: x position.
+ * - w: y position.
+ * - w: angle (rad) * 1024. */
+ simu_pos_x = (double) v8_to_v16 (args[1], args[2]);
+ simu_pos_y = (double) v8_to_v16 (args[3], args[4]);
+ simu_pos_a = (double) v8_to_v16 (args[5], args[6]) / 1024;
+ break;
+ }
+ }
+#endif /* HOST */
else
{
proto_send0 ('?');
diff --git a/n/asserv/src/asserv/postrack.c b/n/asserv/src/asserv/postrack.c
index ae13ec9..2f8aae5 100644
--- a/n/asserv/src/asserv/postrack.c
+++ b/n/asserv/src/asserv/postrack.c
@@ -38,7 +38,7 @@ uint16_t postrack_footing;
* - dd (see postrack_update) is in f11.16 format, we multiply by 256 to have
* a angle in f8.24 format.
* - this factor is in f8.24 format, therefore, 1 is writen (1L << 24). */
-int32_t postrack_footing_factor;
+uint32_t postrack_footing_factor;
/* +AutoDec */
@@ -111,5 +111,5 @@ postrack_set_footing (uint16_t footing)
{
postrack_footing = footing;
postrack_footing_factor =
- (int32_t) (0.5 * M_1_PI * (1L << 8) * (1L << 24)) / footing;
+ (uint32_t) (0.5 * M_1_PI * (1L << 8) * (1L << 24)) / footing;
}
diff --git a/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c
index 1f47de5..64573bb 100644
--- a/n/asserv/src/asserv/simu.host.c
+++ b/n/asserv/src/asserv/simu.host.c
@@ -50,14 +50,17 @@ uint8_t pwm_dir;
struct motor_t simu_left_model, simu_right_model;
-/** Computed simulated position. */
+/** Computed simulated position (mm). */
double simu_pos_x, simu_pos_y, simu_pos_a;
+/** Nearest hole. */
+double simu_nearest_hole;
+
/** Distance between wheels. */
double simu_footing;
/** Holes positions. */
-#define SIMU_HOLES_RADIUS 5
+#define SIMU_HOLES_RADIUS 50
struct simu_hole_t
{
double x, y;
@@ -102,10 +105,10 @@ struct simu_rgb_t
};
static const struct simu_rgb_t simu_rgbs[4] =
{
- { 0, 70 },
- { -5.5, 70 },
- { +5.5, 70 },
- { 0, 0 },
+ { -55, 70 },
+ { +55, 70 },
+ { 0, 65 + 55 },
+ { 0, 65 - 55 },
};
/** Initialise simulation. */
@@ -162,7 +165,8 @@ simu_rgb_update (void)
{
unsigned i, j;
int rgb_x, rgb_y;
- int dx, dy;
+ int dx, dy, d2;
+ int md2 = -1;
PINF &= 0xf0;
for (i = 0; i < UTILS_COUNT (simu_rgbs); i++)
{
@@ -176,13 +180,17 @@ simu_rgb_update (void)
{
dx = simu_holes[j].x - rgb_x;
dy = simu_holes[j].y - rgb_y;
- if (dx * dx + dy * dy < SIMU_HOLES_RADIUS * SIMU_HOLES_RADIUS)
+ d2 = dx * dx + dy * dy;
+ if (d2 < SIMU_HOLES_RADIUS * SIMU_HOLES_RADIUS)
{
PINF |= 1 << i;
break;
}
+ if (md2 == -1 || d2 < md2)
+ md2 = d2;
}
}
+ simu_nearest_hole = md2;
}
/** Do a simulation step. */
@@ -200,7 +208,7 @@ simu_step (void)
old_right_th = simu_right_model.th;
motor_model_step (&simu_left_model);
motor_model_step (&simu_right_model);
- /* Modifie counters. */
+ /* Modify counters. */
counter_left_diff = (simu_left_model.th - old_left_th) / M_2_PI * 500 *
simu_left_model.i_G;
counter_left += counter_left_diff;
@@ -209,9 +217,9 @@ simu_step (void)
counter_right += counter_right_diff;
/* Update position */
simu_pos_update ((simu_left_model.th - old_left_th)
- * simu_left_model.i_G * simu_left_model.w_r,
+ * simu_left_model.i_G * simu_left_model.w_r * 1000,
(simu_right_model.th - old_right_th)
- * simu_right_model.i_G * simu_right_model.w_r);
+ * simu_right_model.i_G * simu_right_model.w_r * 1000);
/* Update RGB sensors. */
simu_rgb_update ();
}
diff --git a/n/asserv/src/asserv/simu.host.h b/n/asserv/src/asserv/simu.host.h
index 11f817c..9aedcf9 100644
--- a/n/asserv/src/asserv/simu.host.h
+++ b/n/asserv/src/asserv/simu.host.h
@@ -25,7 +25,7 @@
* }}} */
/** Simulate some AVR regs. */
-extern uint8_t DDRD, PORTD, PORTA, PORTC, PINA;
+extern uint8_t DDRD, PORTD, PORTA, PORTC, PINA, PINF;
/** Overall counter values. */
extern uint16_t counter_left, counter_right;
@@ -48,6 +48,12 @@ extern int16_t pwm_left, pwm_right;
* on port B. */
extern uint8_t pwm_dir;
+/** Computed simulated position. */
+extern double simu_pos_x, simu_pos_y, simu_pos_a;
+
+/** Nearest hole. */
+extern double simu_nearest_hole;
+
#define EEPROM_KEY 0xa5
#define eeprom_read_params() do { } while (0)
#define eeprom_write_params() do { } while (0)
diff --git a/n/asserv/src/asserv/traj.c b/n/asserv/src/asserv/traj.c
new file mode 100644
index 0000000..8f2413b
--- /dev/null
+++ b/n/asserv/src/asserv/traj.c
@@ -0,0 +1,89 @@
+/* traj.c - Trajectories. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2006 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2005.
+ * Web: http://assos.efrei.fr/robot/
+ * Email: robot AT efrei DOT fr
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+/** Traj mode:
+ * 10, 11 and 12: find a hole.
+ */
+uint8_t traj_mode;
+
+/* Find a hole in the table.
+ * French: On avance, et si on voit un "pas vert" à gauche, on tourne à
+ * gauche. Pareil pour la droite. Au final, on centre le "pas vert" entre deux
+ * capteurs, c'est le trou. Il y a aussi les capteurs à l'avant et à l'arrière.
+ * Le robot commence par avancer à vive allure jusqu'à trouver un trou avec le
+ * capteur avant, puis il ralenti et passe dans le mode ou il avance s'il voit
+ * du "pas vert" à l'avant et il recule s'il voit du "pas-vert" à l'arrière.
+ * Enfin, il s'arrète.
+ */
+static void
+traj_holes (void)
+{
+ int16_t speed;
+ speed_pos = 0;
+ /* Change state if needed. */
+ if (traj_mode == 10 && (PINF & 0xf))
+ traj_mode++;
+ else if (traj_mode == 11 && (PINF & 0xf) == 0)
+ traj_mode++;
+ /* Determine speed. */
+ speed = traj_mode == 10 ? speed_theta_max : speed_theta_max / 2;
+ speed *= 256;
+ /* Left or right. */
+ if (PINF & 1)
+ {
+ speed_theta_cons = speed / 2;
+ speed_alpha_cons = speed / 2;
+ }
+ else if (PINF & 2)
+ {
+ speed_theta_cons = speed / 2;
+ speed_alpha_cons = -speed / 2;
+ }
+ else if (PINF & 8)
+ {
+ speed_theta_cons = -speed;
+ speed_alpha_cons = 0;
+ }
+ else
+ {
+ speed_theta_cons = speed;
+ speed_alpha_cons = 0;
+ }
+}
+
+/* Compute new speed according the defined trajectory. */
+static void
+traj_update (void)
+{
+ switch (traj_mode)
+ {
+ case 10:
+ case 11:
+ traj_holes ();
+ case 12:
+ break;
+ }
+}
+