From bb2dc882b10243fb0301379be0f1dbd43a393f49 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 7 May 2006 23:38:01 +0000 Subject: Ajout de trou partout ! --- n/asserv/src/asserv/avrconfig.h | 2 +- n/asserv/src/asserv/eeprom.avr.c | 3 +- n/asserv/src/asserv/main.c | 66 +++++++++++++++++++++++++---- n/asserv/src/asserv/postrack.c | 4 +- n/asserv/src/asserv/simu.host.c | 30 +++++++++----- n/asserv/src/asserv/simu.host.h | 8 +++- n/asserv/src/asserv/traj.c | 89 ++++++++++++++++++++++++++++++++++++++++ 7 files changed, 179 insertions(+), 23 deletions(-) create mode 100644 n/asserv/src/asserv/traj.c 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 -#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; + } +} + -- cgit v1.2.3