From bc38b408c53281ee985f6f9d67accc5733d99e37 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 28 Jan 2008 21:12:12 +0100 Subject: * digital/asserv/src/asserv: - imported old sources. - removed holes support (rule 2006). --- digital/asserv/src/asserv/Makefile | 16 + digital/asserv/src/asserv/avrconfig.h | 86 +++++ digital/asserv/src/asserv/counter.avr.c | 173 ++++++++++ digital/asserv/src/asserv/eeprom.avr.c | 96 ++++++ digital/asserv/src/asserv/main.c | 471 +++++++++++++++++++++++++++ digital/asserv/src/asserv/misc.h | 38 +++ digital/asserv/src/asserv/models.host.c | 154 +++++++++ digital/asserv/src/asserv/models.host.h | 38 +++ digital/asserv/src/asserv/motor_model.host.c | 88 +++++ digital/asserv/src/asserv/motor_model.host.h | 58 ++++ digital/asserv/src/asserv/pos.c | 144 ++++++++ digital/asserv/src/asserv/postrack.c | 116 +++++++ digital/asserv/src/asserv/pwm.avr.c | 125 +++++++ digital/asserv/src/asserv/simu.host.c | 185 +++++++++++ digital/asserv/src/asserv/simu.host.h | 94 ++++++ digital/asserv/src/asserv/speed.c | 123 +++++++ digital/asserv/src/asserv/test_motor_model.c | 70 ++++ digital/asserv/src/asserv/timer.avr.c | 70 ++++ digital/asserv/src/asserv/traj.c | 76 +++++ 19 files changed, 2221 insertions(+) create mode 100644 digital/asserv/src/asserv/Makefile create mode 100644 digital/asserv/src/asserv/avrconfig.h create mode 100644 digital/asserv/src/asserv/counter.avr.c create mode 100644 digital/asserv/src/asserv/eeprom.avr.c create mode 100644 digital/asserv/src/asserv/main.c create mode 100644 digital/asserv/src/asserv/misc.h create mode 100644 digital/asserv/src/asserv/models.host.c create mode 100644 digital/asserv/src/asserv/models.host.h create mode 100644 digital/asserv/src/asserv/motor_model.host.c create mode 100644 digital/asserv/src/asserv/motor_model.host.h create mode 100644 digital/asserv/src/asserv/pos.c create mode 100644 digital/asserv/src/asserv/postrack.c create mode 100644 digital/asserv/src/asserv/pwm.avr.c create mode 100644 digital/asserv/src/asserv/simu.host.c create mode 100644 digital/asserv/src/asserv/simu.host.h create mode 100644 digital/asserv/src/asserv/speed.c create mode 100644 digital/asserv/src/asserv/test_motor_model.c create mode 100644 digital/asserv/src/asserv/timer.avr.c create mode 100644 digital/asserv/src/asserv/traj.c (limited to 'digital/asserv/src') diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile new file mode 100644 index 00000000..023a8fd1 --- /dev/null +++ b/digital/asserv/src/asserv/Makefile @@ -0,0 +1,16 @@ +BASE = ../../../avr +PROGS = asserv +HOST_PROGS = test_motor_model +asserv_SOURCES = main.c simu.host.c motor_model.host.c models.host.c +test_motor_model_SOURCES = test_motor_model.c motor_model.host.c models.host.c +MODULES = proto uart utils math/fixed +test_motor_model_MODULES = +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 +HOST_LIBS = -lm + +include $(BASE)/make/Makefile.gen diff --git a/digital/asserv/src/asserv/avrconfig.h b/digital/asserv/src/asserv/avrconfig.h new file mode 100644 index 00000000..5d316a25 --- /dev/null +++ b/digital/asserv/src/asserv/avrconfig.h @@ -0,0 +1,86 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ + +/* global */ +/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, + * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#define AC_FREQ 14745600 + +/* uart - UART module. */ +/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */ +#define AC_UART0_PORT 1 +/** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, + * 115200, 230400, 250000, 500000, 1000000. */ +#define AC_UART0_BAUDRATE 115200 +/** Send mode: + * - POLLING: no interrupts. + * - RING: interrupts, ring buffer. */ +#define AC_UART0_SEND_MODE RING +/** Recv mode, same as send mode. */ +#define AC_UART0_RECV_MODE RING +/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */ +#define AC_UART0_CHAR_SIZE 8 +/** Parity : ODD, EVEN, NONE. */ +#define AC_UART0_PARITY EVEN +/** Stop bits : 1, 2. */ +#define AC_UART0_STOP_BITS 1 +/** Send buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_SEND_BUFFER_SIZE 32 +/** Recv buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_RECV_BUFFER_SIZE 32 +/** If the send buffer is full when putc: + * - DROP: drop the new byte. + * - WAIT: wait until there is room in the send buffer. */ +#define AC_UART0_SEND_BUFFER_FULL WAIT +/** In HOST compilation: + * - STDIO: use stdin/out. + * - PTS: use pseudo terminal. */ +#define AC_UART0_HOST_DRIVER PTS +/** Same thing for secondary port. */ +#define AC_UART1_PORT -1 +#define AC_UART1_BAUDRATE 115200 +#define AC_UART1_SEND_MODE RING +#define AC_UART1_RECV_MODE RING +#define AC_UART1_CHAR_SIZE 8 +#define AC_UART1_PARITY EVEN +#define AC_UART1_STOP_BITS 1 +#define AC_UART1_SEND_BUFFER_SIZE 32 +#define AC_UART1_RECV_BUFFER_SIZE 32 +#define AC_UART1_SEND_BUFFER_FULL WAIT +#define AC_UART1_HOST_DRIVER PTS + +/* proto - Protocol module. */ +/** Maximum argument size. */ +#define AC_PROTO_ARGS_MAX_SIZE 12 +/** Callback function name. */ +#define AC_PROTO_CALLBACK proto_callback +/** Putchar function name. */ +#define AC_PROTO_PUTC uart0_putc +/** Support for quote parameter. */ +#define AC_PROTO_QUOTE 1 + +#endif /* avrconfig_h */ diff --git a/digital/asserv/src/asserv/counter.avr.c b/digital/asserv/src/asserv/counter.avr.c new file mode 100644 index 00000000..c2b58781 --- /dev/null +++ b/digital/asserv/src/asserv/counter.avr.c @@ -0,0 +1,173 @@ +/* counter.avr.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ + +/** Define to 1 to reverse the left counter. */ +#define COUNTER_REVERSE_LEFT 0 +/** Define to 1 to reverse the right counter. */ +#define COUNTER_REVERSE_RIGHT 0 + +/** Forward and reverse counter values. */ +static uint8_t counter_left_frw, counter_left_rev, + counter_right_frw, counter_right_rev; +/** Last TCNT values. */ +static uint8_t counter_left_old, counter_right_old; +/** Overall counter values. */ +static uint16_t counter_left, counter_right; +/** Counter differences since last update. + * Maximum of 9 significant bits, sign included. */ +static int16_t counter_left_diff, counter_right_diff; + +/* +AutoDec */ + +/** Initialize the counters. */ +static inline void +counter_init (void); + +/** Update overall counter values and compute diffs. */ +static inline void +counter_update (void); + +/** Restart counting. */ +static inline void +counter_restart (void); + +/* -AutoDec */ + +/** Initialize the counters. */ +static inline void +counter_init (void) +{ + /* Left counter. */ + /* External, rising edge. */ + TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20, + 0, 0, 0, 0, 0, 1, 1, 1); + /* Right counter. */ + /* Normal counter. */ + TCCR3A = 0; + /* External, rising edge. */ + TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30, + 0, 0, 0, 0, 0, 1, 1, 1); + /* Begin with safe values. */ + counter_restart (); + /* Interrupts for direction. */ + EICRB = 0x05; + EIFR = _BV (4) | _BV (5); + EIMSK = _BV (4) | _BV (5); +} + +/** Left direction change. */ +SIGNAL (SIG_INTERRUPT4) +{ + uint8_t c; + c = TCNT2; + if (PINE & _BV (4)) + { + counter_left_rev += c - counter_left_old; + GREEN_LED (!COUNTER_REVERSE_LEFT); + } + else + { + counter_left_frw += c - counter_left_old; + GREEN_LED (COUNTER_REVERSE_LEFT); + } + counter_left_old = c; +} + +/** Right direction change. */ +SIGNAL (SIG_INTERRUPT5) +{ + uint8_t c; + c = TCNT3L; + if (PINE & _BV (5)) + { + counter_right_rev += c - counter_right_old; + RED_LED (!COUNTER_REVERSE_RIGHT); + } + else + { + counter_right_frw += c - counter_right_old; + RED_LED (COUNTER_REVERSE_RIGHT); + } + counter_right_old = c; +} + +/** Update overall counter values and compute diffs. */ +static inline void +counter_update (void) +{ + uint8_t c; + /* Disable ints. */ + EIMSK &= ~(_BV (4) | _BV (5)); + /* Read left counter. */ + c = TCNT2; + if (PINE & _BV (4)) + counter_left_frw += c - counter_left_old; + else + counter_left_rev += c - counter_left_old; + counter_left_old = c; + /* Read right counter. */ + c = TCNT3L; + if (PINE & _BV (5)) + counter_right_frw += c - counter_right_old; + else + counter_right_rev += c - counter_right_old; + counter_right_old = c; + /* Update counter values and diffs. */ +#if COUNTER_REVERSE_LEFT == 0 + counter_left_diff = counter_left_frw; + counter_left_diff -= counter_left_rev; +#else + counter_left_diff = counter_left_rev; + counter_left_diff -= counter_left_frw; +#endif + counter_left_frw = 0; + counter_left_rev = 0; + counter_left += counter_left_diff; +#if COUNTER_REVERSE_RIGHT == 0 + counter_right_diff = counter_right_frw; + counter_right_diff -= counter_right_rev; +#else + counter_right_diff = counter_right_rev; + counter_right_diff -= counter_right_frw; +#endif + counter_right_frw = 0; + counter_right_rev = 0; + counter_right += counter_right_diff; + /* Enable ints. */ + EIMSK |= _BV (4) | _BV (5); +} + +/** Restart counting. */ +static inline void +counter_restart (void) +{ + counter_left_frw = 0; + counter_left_rev = 0; + counter_left_old = TCNT2; + counter_right_frw = 0; + counter_right_rev = 0; + counter_right_old = TCNT3L; +} + diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c new file mode 100644 index 00000000..26e5c4b6 --- /dev/null +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -0,0 +1,96 @@ +/* eeprom.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ +#include + +/* Change the eeprom key each time you change eeprom format. */ +#define EEPROM_KEY 0x45 +#define EEPROM_START 256 + +/* +AutoDec */ +/* -AutoDec */ + +/* Read parameters from eeprom. */ +static void +eeprom_read_params (void) +{ + uint8_t *p8 = (uint8_t *) EEPROM_START; + uint16_t *p16; + if (eeprom_read_byte (p8++) != EEPROM_KEY) + return; + speed_theta_max = eeprom_read_byte (p8++); + speed_alpha_max = eeprom_read_byte (p8++); + speed_theta_slow = eeprom_read_byte (p8++); + speed_alpha_slow = eeprom_read_byte (p8++); + pwm_dir = eeprom_read_byte (p8++); + p16 = (uint16_t *) p8; + postrack_set_footing (eeprom_read_word (p16++)); + speed_theta_acc = eeprom_read_word (p16++); + speed_alpha_acc = eeprom_read_word (p16++); + pos_theta_kp = eeprom_read_word (p16++); + pos_alpha_kp = eeprom_read_word (p16++); + pos_theta_ki = eeprom_read_word (p16++); + pos_alpha_ki = eeprom_read_word (p16++); + pos_theta_kd = eeprom_read_word (p16++); + pos_alpha_kd = eeprom_read_word (p16++); + pos_e_sat = eeprom_read_word (p16++); + pos_int_sat = eeprom_read_word (p16++); + pos_blocked = eeprom_read_word (p16++); +} + +/* Write parameters to eeprom. */ +static void +eeprom_write_params (void) +{ + uint8_t *p8 = (uint8_t *) EEPROM_START; + uint16_t *p16; + eeprom_write_byte (p8++, EEPROM_KEY); + eeprom_write_byte (p8++, speed_theta_max); + eeprom_write_byte (p8++, speed_alpha_max); + eeprom_write_byte (p8++, speed_theta_slow); + eeprom_write_byte (p8++, speed_alpha_slow); + eeprom_write_byte (p8++, pwm_dir); + p16 = (uint16_t *) p8; + eeprom_write_word (p16++, postrack_footing); + eeprom_write_word (p16++, speed_theta_acc); + eeprom_write_word (p16++, speed_alpha_acc); + eeprom_write_word (p16++, pos_theta_kp); + eeprom_write_word (p16++, pos_alpha_kp); + eeprom_write_word (p16++, pos_theta_ki); + eeprom_write_word (p16++, pos_alpha_ki); + eeprom_write_word (p16++, pos_theta_kd); + eeprom_write_word (p16++, pos_alpha_kd); + eeprom_write_word (p16++, pos_e_sat); + eeprom_write_word (p16++, pos_int_sat); + eeprom_write_word (p16++, pos_blocked); +} + +/* Clear eeprom parameters. */ +static void +eeprom_clear_params (void) +{ + uint8_t *p = (uint8_t *) EEPROM_START; + eeprom_write_byte (p, 0xff); +} + diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c new file mode 100644 index 00000000..22a282cd --- /dev/null +++ b/digital/asserv/src/asserv/main.c @@ -0,0 +1,471 @@ +/* main.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ +#include "common.h" +#include "modules/uart/uart.h" +#include "modules/proto/proto.h" +#include "modules/utils/utils.h" +#include "modules/utils/byte.h" +#include "modules/math/fixed/fixed.h" +#include "io.h" + +#include "misc.h" + +/** Motor command sequence, do not use values above 127, do not use zero. */ +uint8_t main_sequence, main_sequence_ack, main_sequence_finish; + +/* This is implementation include. */ +#ifndef HOST +# include "timer.avr.c" +# include "counter.avr.c" +# include "pwm.avr.c" +#else +# include "simu.host.h" +#endif +#include "pos.c" +#include "speed.c" +#include "postrack.c" +#include "traj.c" +#ifndef HOST +# include "eeprom.avr.c" +#endif + +/** Motor control mode: + * 0: pwm setup. + * 1: shaft position control. + * 2: speed control. + * 3: trajectory control. */ +int8_t main_mode; + +/** Report trajectory end. */ +uint8_t main_sequence_ack_cpt = 2; + +/** Report of counters. */ +uint8_t main_stat_counter, main_stat_counter_cpt; + +/** Report of position. */ +uint8_t main_stat_postrack, main_stat_postrack_cpt; + +/** Statistics about speed control. */ +uint8_t main_stat_speed, main_stat_speed_cpt; + +/** Statistics about shaft position control. */ +uint8_t main_stat_pos, main_stat_pos_cpt; + +/** Statistics about pwm values. */ +uint8_t main_stat_pwm, main_stat_pwm_cpt; + +/** Report of timer. */ +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]; + +/* +AutoDec */ + +/** Main loop. */ +static void +main_loop (void); + +/* -AutoDec */ + +/** Entry point. */ +int +main (int argc, char **argv) +{ + avr_init (argc, argv); + DDRD = 0x60; + /* Pull-ups. */ + PORTA = 0xff; + PORTC = 0xff; + eeprom_read_params (); + pwm_init (); + timer_init (); + counter_init (); + uart0_init (); + postrack_init (); + proto_send0 ('z'); + sei (); + while (1) + main_loop (); + return 0; +} + +/** Main loop. */ +static void +main_loop (void) +{ + main_timer[5] = timer_read (); + timer_wait (); + /* Counter update. */ + counter_update (); + main_timer[0] = timer_read (); + /* Postion control. */ + if (main_mode >= 1) + pos_update (); + main_timer[1] = timer_read (); + /* Pwm setup. */ + pwm_update (); + 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 (); + main_timer[3] = timer_read (); + /* Stats. */ + if (main_sequence_ack != main_sequence_finish && !--main_sequence_ack_cpt) + { + proto_send1b ('A', main_sequence_finish); + main_sequence_ack_cpt = 4; + } + if (main_stat_counter && !--main_stat_counter_cpt) + { + proto_send2w ('C', counter_left, counter_right); + main_stat_counter_cpt = main_stat_counter; + } + if (main_stat_postrack && !--main_stat_postrack_cpt) + { + proto_send3d ('X', postrack_x, postrack_y, postrack_a); + main_stat_postrack_cpt = main_stat_postrack; + } + if (main_stat_speed && !--main_stat_speed_cpt) + { + proto_send2b ('S', speed_theta_cur >> 8, speed_alpha_cur >> 8); + main_stat_speed_cpt = main_stat_speed; + } + if (main_stat_pos && !--main_stat_pos_cpt) + { + proto_send4w ('P', pos_theta_e_old, pos_theta_int, + pos_alpha_e_old, pos_alpha_int); + main_stat_pos_cpt = main_stat_pos; + } +#ifdef HOST + if (main_simu && !--main_simu_cpt) + { + proto_send3w ('Y', (uint16_t) simu_pos_x, + (uint16_t) simu_pos_y, + (uint16_t) (simu_pos_a * 1024)); + 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); + main_stat_pwm_cpt = main_stat_pwm; + } + if (main_stat_timer && !--main_stat_timer_cpt) + { + proto_send6b ('T', main_timer[0], main_timer[2], main_timer[3], + main_timer[4], main_timer[4], main_timer[5]); + main_stat_timer_cpt = main_stat_timer; + } + if (main_print_pin && !--main_print_pin_cpt) + { + proto_send2b ('I', PINA, PINF & 0xf); + main_print_pin_cpt = main_print_pin; + } + /* Misc. */ + while (uart0_poll ()) + proto_accept (uart0_getc ()); + main_timer[4] = timer_read (); +} + +/** Handle incoming messages. */ +void +proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) +{ +#define c(cmd, size) (cmd << 8 | size) + switch (c (cmd, size)) + { + case c ('z', 0): + /* Reset. */ + utils_reset (); + break; + /* Commands. */ + case c ('w', 0): + /* Set zero pwm. */ + pos_reset (); + main_mode = 0; + pwm_left = 0; + pwm_right = 0; + break; + case c ('w', 4): + /* Set pwm. + * - w: left pwm. + * - w: right pwm. */ + pos_reset (); + main_mode = 0; + pwm_left = v8_to_v16 (args[0], args[1]); + UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX); + pwm_right = v8_to_v16 (args[2], args[3]); + UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX); + break; + case c ('c', 4): + /* Add to position consign. + * - w: theta consign offset. + * - w: alpha consign offset. */ + main_mode = 1; + pos_theta_cons += v8_to_v16 (args[0], args[1]); + pos_alpha_cons += v8_to_v16 (args[2], args[3]); + break; + case c ('s', 0): + /* Stop (set zero speed). */ + main_mode = 2; + speed_pos = 0; + speed_theta_cons = 0; + speed_alpha_cons = 0; + break; + case c ('s', 2): + /* Set speed. + * - b: theta speed. + * - b: alpha speed. */ + main_mode = 2; + speed_pos = 0; + speed_theta_cons = args[0] << 8; + speed_alpha_cons = args[1] << 8; + break; + case c ('s', 9): + /* Set speed controlled position consign. + * - d: theta consign offset. + * - d: alpha consign offset. + * - b: sequence number. */ + 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_v32 (args[0], args[1], args[2], args[3]); + speed_alpha_pos_cons = pos_alpha_cons; + speed_alpha_pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]); + main_sequence = args[8]; + break; + case c ('f', 1): + /* Go to the wall. + * - b: sequence number. */ + if (args[0] == main_sequence) + break; + main_mode = 3; + speed_pos = 0; + traj_mode = 10; + main_sequence = args[0]; + break; + case c ('a', 1): + /* Set acknoledge. + * - b: ack sequence number. */ + main_sequence_ack = args[0]; + if (pos_blocked_state) + pos_reset (); + break; + /* Stats. + * - b: interval between stats. */ + case c ('C', 1): + /* Counter stats. */ + main_stat_counter_cpt = main_stat_counter = args[0]; + break; + case c ('X', 1): + /* Position stats. */ + main_stat_postrack_cpt = main_stat_postrack = args[0]; + break; + case c ('S', 1): + /* Motor speed control stats. */ + main_stat_speed_cpt = main_stat_speed = args[0]; + break; + case c ('P', 1): + /* Motor position control stats. */ + main_stat_pos_cpt = main_stat_pos = args[0]; + break; + case c ('W', 1): + /* Pwm stats. */ + main_stat_pwm_cpt = main_stat_pwm = args[0]; + break; + case c ('T', 1): + /* Timing stats. */ + main_stat_timer_cpt = main_stat_timer = args[0]; + break; + case c ('I', 1): + /* 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') + { + switch (c (args[0], size)) + { + case c ('X', 1): + /* Reset position. */ + postrack_x = 0; + postrack_y = 0; + postrack_a = 0; + break; + case c ('X', 5): + /* Set current x position. + * - d: x position. */ + postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('Y', 5): + /* Set current y position. + * - d: y position. */ + postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('A', 5): + /* Set current angle. + * - d: angle. */ + postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('f', 3): + /* Set footing. + * - w: footing. */ + postrack_set_footing (v8_to_v16 (args[1], args[2])); + break; + case c ('a', 5): + /* Set acceleration. + * - w: theta. + * - w: alpha. */ + speed_theta_acc = v8_to_v16 (args[1], args[2]); + speed_alpha_acc = v8_to_v16 (args[3], args[4]); + break; + case c ('s', 5): + /* Set maximum and slow speed. + * - b: theta max. + * - b: alpha max. + * - b: theta slow. + * - b: alpha slow. */ + speed_theta_max = args[1]; + speed_alpha_max = args[2]; + speed_theta_slow = args[3]; + speed_alpha_slow = args[4]; + break; + case c ('p', 3): + pos_theta_kp = pos_alpha_kp = v8_to_v16 (args[1], args[2]); + break; + case c ('p', 5): + pos_theta_kp = v8_to_v16 (args[1], args[2]); + pos_alpha_kp = v8_to_v16 (args[3], args[4]); + break; + case c ('i', 3): + pos_theta_ki = pos_alpha_ki = v8_to_v16 (args[1], args[2]); + break; + case c ('i', 5): + pos_theta_ki = v8_to_v16 (args[1], args[2]); + pos_alpha_ki = v8_to_v16 (args[3], args[4]); + break; + case c ('d', 3): + pos_theta_kd = pos_alpha_kd = v8_to_v16 (args[1], args[2]); + break; + case c ('d', 5): + pos_theta_kd = v8_to_v16 (args[1], args[2]); + pos_alpha_kd = v8_to_v16 (args[3], args[4]); + break; + case c ('E', 3): + pos_e_sat = v8_to_v16 (args[1], args[2]); + break; + case c ('I', 3): + pos_int_sat = v8_to_v16 (args[1], args[2]); + break; + case c ('b', 3): + pos_blocked = v8_to_v16 (args[1], args[2]); + break; + case c ('w', 3): + /* Set PWM direction. + * - b: inverse left direction. + * - b: inverse right direction. */ + pwm_dir = 0; + if (args[1]) pwm_dir |= _BV (PWM_LEFT_DIR); + if (args[2]) pwm_dir |= _BV (PWM_RIGHT_DIR); + break; + case c ('E', 2): + /* Write to eeprom. + * - b: 00: clear config, 01: write config. */ + if (args[1]) + eeprom_write_params (); + else + eeprom_clear_params (); + break; + case c ('P', 1): + /* Print current settings. */ + proto_send1b ('E', EEPROM_KEY); + proto_send1w ('f', postrack_footing); + proto_send2w ('a', speed_theta_acc, speed_alpha_acc); + proto_send2b ('s', speed_theta_max, speed_alpha_max); + proto_send2w ('p', pos_theta_kp, pos_alpha_kp); + proto_send2w ('i', pos_theta_ki, pos_alpha_ki); + proto_send2w ('d', pos_theta_kd, pos_alpha_kd); + proto_send1w ('E', pos_e_sat); + proto_send1w ('I', pos_int_sat); + proto_send1b ('w', pwm_dir); + break; + default: + proto_send0 ('?'); + 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 ('?'); + return; + } + break; + } + proto_send (cmd, size, args); +#undef c +} + diff --git a/digital/asserv/src/asserv/misc.h b/digital/asserv/src/asserv/misc.h new file mode 100644 index 00000000..59f25135 --- /dev/null +++ b/digital/asserv/src/asserv/misc.h @@ -0,0 +1,38 @@ +#ifndef misc_h +#define misc_h +/* misc.h - Miscellaneous definitions. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ + +#define RED_LED(x) do { \ + if (!(x)) PORTD |= 0x20; \ + else PORTD &= ~0x20; \ +} while (0) + +#define GREEN_LED(x) do { \ + if (!(x)) PORTD |= 0x40; \ + else PORTD &= ~0x40; \ +} while (0) + +#endif /* misc_h */ diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c new file mode 100644 index 00000000..976edd3e --- /dev/null +++ b/digital/asserv/src/asserv/models.host.c @@ -0,0 +1,154 @@ +/* models.host.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ +#include "motor_model.host.h" +#include "models.host.h" + +#include +#include + +/* Gloubi model. */ +static const struct motor_t gloubi_model = +{ + /* Motor caracteristics. */ + 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 23.4 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.18, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + /* Gearbox caracteristics. */ + 10, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load caracteristics. */ + 4 * 0.02 * 0.02, /* Load (kg.m^2). */ + /* Wheel caracteristics. */ + 0.02, /* Wheel radius (m). */ + /* Simulation parameters. */ + 4.444444 / 1000, /* Simulation time step (s). */ + 1000, /* Simulation time step division. */ + /* Simulation current state. */ + 0, /* Current time (not realy used) (s). */ + 0, /* Current input voltage (V). */ + 0, /* Current current (A). */ + 0, /* Current angular speed (o for omega) (rad/s). */ + 0 /* Current theta (th for theta) (rad). */ +}; + +static const struct robot_t gloubi_robot = +{ + &gloubi_model, + 26.0, /* Distance between the wheels (m). */ +}; + +/* Taz model. */ +static const struct motor_t taz_model = +{ + /* Motor caracteristics. */ + 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 23.4 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.18, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + /* Gearbox caracteristics. */ + 10, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load caracteristics. */ + 10 * 0.04 * 0.04, /* Load (kg.m^2). */ + /* Wheel caracteristics. */ + 0.04, /* Wheel radius (m). */ + /* Simulation parameters. */ + 4.444444 / 1000, /* Simulation time step (s). */ + 1000, /* Simulation time step division. */ + /* Simulation current state. */ + 0, /* Current time (not realy used) (s). */ + 0, /* Current input voltage (V). */ + 0, /* Current current (A). */ + 0, /* Current angular speed (o for omega) (rad/s). */ + 0 /* Current theta (th for theta) (rad). */ +}; + +static const struct robot_t taz_robot = +{ + &taz_model, + 30.0, /* Distance between the wheels (m). */ +}; + +/* Taz model, with a RE25G and a 1:20.25 ratio gearbox. */ +static const struct motor_t tazg_model = +{ + /* Motor caracteristics. */ + 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 40.2 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.32, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + /* Gearbox caracteristics. */ + 20.25, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load caracteristics. */ + 10 * 0.04 * 0.04, /* Load (kg.m^2). */ + /* Wheel caracteristics. */ + 0.04, /* Wheel radius (m). */ + /* Simulation parameters. */ + 4.444444 / 1000, /* Simulation time step (s). */ + 1000, /* Simulation time step division. */ + /* Simulation current state. */ + 0, /* Current time (not realy used) (s). */ + 0, /* Current input voltage (V). */ + 0, /* Current current (A). */ + 0, /* Current angular speed (o for omega) (rad/s). */ + 0 /* Current theta (th for theta) (rad). */ +}; + +static const struct robot_t tazg_robot = +{ + &tazg_model, + 30.0, /* Distance between the wheels (m). */ +}; + +/* Table of models. */ +static const struct +{ + const char *name; + const struct robot_t *robot; +} models[] = { + { "gloubi", &gloubi_robot }, + { "taz", &taz_robot }, + { "tazg", &tazg_robot }, + { 0, 0 } +}; + +/** Get a pointer to a model by name, or return 0. */ +const struct robot_t * +models_get (const char *name) +{ + int i; + for (i = 0; models[i].name; i++) + { + if (strcmp (models[i].name, name) == 0) + return models[i].robot; + } + return 0; +} + diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h new file mode 100644 index 00000000..4ae55ad8 --- /dev/null +++ b/digital/asserv/src/asserv/models.host.h @@ -0,0 +1,38 @@ +#ifndef models_host_h +#define models_host_h +/* models.host.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ + +struct robot_t +{ + const struct motor_t *motor; + double footing; /* Distance between the wheels (m). */ +}; + +/** Get a pointer to a model by name, or return 0. */ +const struct robot_t * +models_get (const char *name); + +#endif /* models_host_h */ diff --git a/digital/asserv/src/asserv/motor_model.host.c b/digital/asserv/src/asserv/motor_model.host.c new file mode 100644 index 00000000..c8b4639c --- /dev/null +++ b/digital/asserv/src/asserv/motor_model.host.c @@ -0,0 +1,88 @@ +/* motor_model.c - DC motor model. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ +#include "common.h" +#include "motor_model.host.h" + +/** + * Switching to french for all thoses non english speaking people. + * + * Ce fichier permet de modéliser un moteur à courant continue. Il y a deux + * parties, la modélisation électrique et la modélisation mécanique. + * + * On peut trouver de l'aide sur : + * - le site de maxon : http://www.maxonmotor.com/ + * - ici : http://www.mathworks.com/access/helpdesk/help/toolbox/control/getstart/buildmo4.html + * - là : http://iai.eivd.ch/users/mee/Regulation_automatique_anal.htm + * + * Voici ce qui en résulte, des belles équations différentielles : + * u(t) = R i(t) + L di(t)/dt + Ke o(t) + * J do(t)/dt = Kt i(t) - Rf o(t) + * dth(t)/dt = o(t) + * + * Les variables sont décrites dans la structure motor_t. + * + * À cela, il faut ajouter un coef pour le réducteur, je vous laisse ça en + * exercice. + * + * On va résoudre ces belles équadiff numériquement par la méthode d'Euler (il + * est partout celui là). Si vous voulez plus de détail, mailez moi. On arrive + * à : + * + * i(t+h) = i(t) + h (1/L u(t) - R/L i(t) - 1/(Ke L) o(t)) + * o(t+h) = o(t) + h (i_G^2 ro_G)/J (Kt i(t) - Rf o(t)) + * th(t+h) = th(t) + h o(t) + * + * C'est consternant de simplicité non ? + */ + +/** Make a simulation step. */ +void motor_model_step (struct motor_t *m) +{ + double i_, o_, th_; /* New computed values. */ + double h; /* Infinitesimal step... Well, not so infinite. */ + int d; + d = m->d; + h = m->h / d; + /* Make several small steps to increase precision. */ + for (; d; d--) + { + /* Ah, the mistical power of computation. */ + i_ = m->i + + h * (1.0 / m->L * m->u + - m->R / m->L * m->i + - 1.0 / m->Ke / m->L * m->o); + o_ = m->o + + h * m->i_G * m->i_G * m->ro_G / m->J + * (m->Kt * m->i - m->Rf * m->o); + th_ = m->th + h * m->o; + /* Ok, now store this step. */ + m->i = i_; + m->o = o_; + m->th = th_; + } + /* Damn! It's finished yet! */ + m->t += m->h; +} + diff --git a/digital/asserv/src/asserv/motor_model.host.h b/digital/asserv/src/asserv/motor_model.host.h new file mode 100644 index 00000000..931890a7 --- /dev/null +++ b/digital/asserv/src/asserv/motor_model.host.h @@ -0,0 +1,58 @@ +#ifndef motor_model_host_h +#define motor_model_host_h +/* motor_model.host.h - DC motor model. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ + +/** Motor and load caracteristics and current data. */ +struct motor_t +{ + /* Motor caracteristics. */ + double Ke; /* Speed constant ((rad/s)/V). */ + double Kt; /* Torque constant (N.m/A). */ + double Rf; /* Bearing friction (N.m/(rad/s)). */ + double R; /* Terminal resistance (Ohm). */ + double L; /* Terminal inductance (H). */ + /* Gearbox caracteristics. */ + double i_G; /* Gearbox ratio. */ + double ro_G;/* Gearbox efficiency. */ + /* Load caracteristics. */ + double J; /* Load (kg.m^2). */ + /* Wheel caracteristics. */ + double w_r; /* Wheel radius (m). */ + /* Simulation parameters. */ + double h; /* Simulation time step (s). */ + int d; /* Simulation time step division. */ + /* Simulation current state. */ + double t; /* Current time (not realy used) (s). */ + double u; /* Current input voltage (V). */ + double i; /* Current current (A). */ + double o; /* Current angular speed (o for omega) (rad/s). */ + double th; /* Current theta (th for theta) (rad). */ +}; + +/** Make a simulation step. */ +void motor_model_step (struct motor_t *m); + +#endif /* motor_model_host_h */ diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c new file mode 100644 index 00000000..eecd71f3 --- /dev/null +++ b/digital/asserv/src/asserv/pos.c @@ -0,0 +1,144 @@ +/* pos.c - Position motor control. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ + +/** + * This file is responsible for position motor control. The consign is a + * position of the motor shafts, as theta/alpha. Theta is the sum of right + * and left position, alpha is the difference between the right and the left + * position. + * 16 bits are enough as long as there is no long blocking (see 2005 cup!). + */ + +/** Current theta/alpha. */ +uint32_t pos_theta_cur, pos_alpha_cur; +/** Consign theta/alpha. */ +uint32_t pos_theta_cons, pos_alpha_cons; + +/** Error saturation. */ +int32_t pos_e_sat = 1023; +/** Integral saturation. */ +int32_t pos_int_sat = 1023; +/** P coefficients. */ +uint16_t pos_theta_kp, pos_alpha_kp; +/** I coefficients. */ +uint16_t pos_theta_ki, pos_alpha_ki; +/** D coefficients. */ +uint16_t pos_theta_kd, pos_alpha_kd; +/** Blocked value. If error is greater than this value, stop the robot and + * report blocked state. */ +int32_t pos_blocked = 15000L; + +/** Current integral values. */ +int32_t pos_theta_int, pos_alpha_int; +/** Last error values. */ +int32_t pos_theta_e_old, pos_alpha_e_old; +/** One if blocked. */ +uint8_t pos_blocked_state; + +/* +AutoDec */ +/* -AutoDec */ + +/** Compute a PID. + * How to compute maximum numbers size: + * Result is 24 bits (16 bits kept after shift). + * If e_sat == 1023, e max is 11 bits (do not forget the sign bit), and diff + * max is 12 bits. + * If int_sat == 1023, i max is 11 bits. + * In the final addition, let's give 23 bits to the p part, and 22 bits to the + * i and d part (23b + 22b + 22b => 23b + 23b => 24b). + * Therefore, kp can be 23 - 11 = 12 bits (f4.8). + * ki can be 22 - 11 = 11 bits (f3.8). + * kd can be 22 - 12 = 10 bits (f2.8). + * How to increase this number: + * - lower the shift. + * - bound the value returned. + * - lower e & int saturation. */ +static inline int16_t +pos_compute_pid (int32_t e, int32_t *i, int32_t *e_old, + uint16_t kp, uint16_t ki, uint16_t kd) +{ + int32_t diff, pid; + /* Saturate error. */ + UTILS_BOUND (e, -pos_e_sat, pos_e_sat); + /* Integral update. */ + *i += e; + UTILS_BOUND (*i, -pos_int_sat, pos_int_sat); + /* Differential value. */ + diff = e - *e_old; + /* Compute PID. */ + pid = e * kp + *i * ki + diff * kd; + /* Save result. */ + *e_old = e; + return pid >> 8; +} + +/** Update PWM according to consign. */ +static void +pos_update (void) +{ + int16_t pid_theta, pid_alpha; + int32_t diff_theta, diff_alpha; + /* Update current shaft positions. */ + pos_theta_cur += counter_left_diff + counter_right_diff; + pos_alpha_cur += counter_right_diff - counter_left_diff; + /* Compute PID. */ + diff_theta = pos_theta_cons - pos_theta_cur; + diff_alpha = pos_alpha_cons - pos_alpha_cur; + if (pos_blocked_state + || diff_theta < -pos_blocked || pos_blocked < diff_theta + || diff_alpha < -pos_blocked || pos_blocked < diff_alpha) + { + /* Blocked. */ + pwm_left = 0; + pwm_right = 0; + pos_blocked_state = 1; + main_sequence_finish = main_sequence | 0x80; + } + else + { + pid_theta = pos_compute_pid (diff_theta, &pos_theta_int, + &pos_theta_e_old, pos_theta_kp, + pos_theta_ki, pos_theta_kd); + pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha_int, + &pos_alpha_e_old, pos_alpha_kp, + pos_alpha_ki, pos_alpha_kd); + /* Update PWM. */ + pwm_left = pid_theta - pid_alpha; + UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX); + pwm_right = pid_theta + pid_alpha; + UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX); + } +} + +/** Reset position control internal state. */ +static void +pos_reset (void) +{ + pos_theta_int = pos_alpha_int = 0; + pos_theta_cur = pos_alpha_cur = 0; + pos_theta_cons = pos_alpha_cons = 0; + pos_theta_e_old = pos_alpha_e_old = 0; + pos_blocked_state = 0; +} diff --git a/digital/asserv/src/asserv/postrack.c b/digital/asserv/src/asserv/postrack.c new file mode 100644 index 00000000..6da77592 --- /dev/null +++ b/digital/asserv/src/asserv/postrack.c @@ -0,0 +1,116 @@ +/* postrack.c */ +/* 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. + * + * }}} */ + +/** Current position, f24.8. */ +int32_t postrack_x, postrack_y; +/** Current angle, f8.24. */ +int32_t postrack_a; +/** Distance between the weels, u16. */ +uint16_t postrack_footing; +/** Precomputed footing factor, f8.24. + * postrack_footing_factor = (2 * 1/2pi * 256) / postrack_footing + * Explanations: + * - Angles are between 0 and 1, corresponding to 0 and 2pi, therefore we + * must divide by 2pi to convert unit (Arc=Angle * Radius only works for + * radians). + * - dd (see postrack_update) is in f10.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). + * - there is a two factor because of the sum done in the motor control. */ +uint32_t postrack_footing_factor; + +/* +AutoDec */ + +/** Initialise the position tracker. */ +static inline void +postrack_init (void); + +/** Update the current position. */ +static inline void +postrack_update (void); + +/** Change the footing value. */ +static inline void +postrack_set_footing (uint16_t footing); + +/* -AutoDec */ + +/** Initialise the position tracker. */ +static inline void +postrack_init (void) +{ + /* Prevent division by 0 by providing a default large value. */ + postrack_set_footing (0x1000); +} + +#define M_PI 3.14159265358979323846 /* pi */ + +/** Update the current position. */ +static inline void +postrack_update (void) +{ + int32_t d, dd, da, na, dsc; + d = counter_right_diff + counter_left_diff; /* 10b */ + d <<= 16; /* 10.16b */ + if (counter_right_diff == counter_left_diff) + { + /* Line. */ + postrack_x += fixed_mul_f824 (d, fixed_cos_f824 (postrack_a)) >> 8; + postrack_y += fixed_mul_f824 (d, fixed_sin_f824 (postrack_a)) >> 8; + } + else + { + /* Arc. */ + dd = counter_right_diff - counter_left_diff; /* 10b */ + dd <<= 16; /* 10.16b */ + da = fixed_mul_f824 (dd, postrack_footing_factor);/* 8.24b */ + /* New angle. */ + na = postrack_a + da; + /* Compute da in radians. */ + da = fixed_mul_f824 (da, 2 * M_PI * (1L << 24)); + /* X increment. */ + dsc = fixed_sin_f824 (na) - fixed_sin_f824 (postrack_a); /* 8.24b */ + dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */ + postrack_x += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */ + /* Y increment. */ + dsc = fixed_cos_f824 (postrack_a) - fixed_cos_f824 (na); /* 8.24b */ + dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */ + postrack_y += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */ + /* Angle update. */ + postrack_a = na; + postrack_a &= 0x00ffffff; + } +} + +#define M_1_PI 0.31830988618379067154 /* 1/pi */ + +/** Change the footing value. */ +static inline void +postrack_set_footing (uint16_t footing) +{ + postrack_footing = footing; + postrack_footing_factor = + (uint32_t) (M_1_PI * (1L << 8) * (1L << 24)) / footing; +} diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c new file mode 100644 index 00000000..773cfef0 --- /dev/null +++ b/digital/asserv/src/asserv/pwm.avr.c @@ -0,0 +1,125 @@ +/* pwm.avr.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ + +/** Define the PWM output used for left motor. */ +#define PWM_LEFT_OCR OCR1C +/** Define the PWM output used for right motor. */ +#define PWM_RIGHT_OCR OCR1B +/** Define the direction output for left motor. */ +#define PWM_LEFT_DIR 3 +/** Define the direction output for right motor. */ +#define PWM_RIGHT_DIR 2 + +/** Define the absolute maximum PWM value. */ +#define PWM_MAX 0x3ff + +/** PWM values, this is an error if absolute value is greater than the + * maximum. */ +int16_t pwm_left, pwm_right; +/** PWM reverse direction, only set pwm dir bits or you will get weird results + * on port B. */ +uint8_t pwm_dir = _BV (PWM_LEFT_DIR); + +/* +AutoDec */ + +/** Initialise PWM generator. */ +static inline void +pwm_init (void); + +/** Update the hardware PWM values. */ +static inline void +pwm_update (void); + +/* -AutoDec */ + +/** Initialise PWM generator. */ +static inline void +pwm_init (void) +{ + /* Fast PWM, TOP = 0x3ff, OC1B & OC1C with positive logic. + f_IO without prescaler. + Fpwm = f_IO / (prescaler * (1 + TOP)) = 14400 Hz. */ + TCCR1A = + regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, + 0, 0, 1, 0, 1, 0, 1, 1); + TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, + 0, 0, 0, 0, 1, 0, 0, 1); + /* Enable pwm and direction outputs in DDRB. */ + DDRB |= _BV (7) | _BV (6) | _BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR); +} + +/** Update the hardware PWM values. */ +static inline void +pwm_update (void) +{ + uint16_t left, right; + uint8_t dir; + /* Some assumption checks. */ + assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX); + assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX); + assert ((pwm_dir & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR))) == 0); + /* Sample port B. */ + dir = PORTB & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR)); + /* Set left PWM. */ + if (pwm_left == 0) + { + left = 0; + } + else if (pwm_left < 0) + { + left = -pwm_left; + } + else + { + dir |= _BV (PWM_LEFT_DIR); + left = pwm_left; + } + /* Set right PWM. */ + if (pwm_right == 0) + { + right = 0; + } + else if (pwm_right < 0) + { + right = -pwm_right; + } + else + { + dir |= _BV (PWM_RIGHT_DIR); + right = pwm_right; + } + /* Setup registers. */ + /* Here, there could be a problem because OCRx are double buffered, not + * PORTB! */ + /* Another problem arise if the OCR sampling is done between left and + * right OCR: the right PWM is one cycle late. */ + /* A solution could be to use interrupts to update PWM or to synchronise + * general timer with PWM. */ + dir ^= pwm_dir; + PORTB = dir; + PWM_LEFT_OCR = left; + PWM_RIGHT_OCR = right; +} + diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c new file mode 100644 index 00000000..beefafe2 --- /dev/null +++ b/digital/asserv/src/asserv/simu.host.c @@ -0,0 +1,185 @@ +/* simu.host.c */ +/* {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * 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. + * + * Contact : + * Web: http://perso.efrei.fr/~schodet/ + * }}} */ +#include "common.h" +#include "modules/host/host.h" +#include "modules/utils/utils.h" +#include "simu.host.h" +#include "motor_model.host.h" +#include "models.host.h" + +#include +#include +#include + +/** Simulate some AVR regs. */ +uint8_t DDRD, PORTD, PORTA, PORTC, PINA, PINF; + +/** Overall counter values. */ +uint16_t counter_left, counter_right; +/** Counter differences since last update. + * Maximum of 9 significant bits, sign included. */ +int16_t counter_left_diff, counter_right_diff; + +/** PWM values, this is an error if absolute value is greater than the + * maximum. */ +int16_t pwm_left, pwm_right; +/** PWM reverse direction, only set pwm dir bits or you will get weird results + * on port B. */ +uint8_t pwm_dir; + +struct motor_t simu_left_model, simu_right_model; + +/** Computed simulated position (mm). */ +double simu_pos_x, simu_pos_y, simu_pos_a; + +/** Distance between wheels. */ +double simu_footing; + +/** Initialise simulation. */ +static void +simu_init (void) +{ + int argc; + char **argv; + const struct robot_t *m; + host_get_program_arguments (&argc, &argv); + if (argc != 1) + { + fprintf (stderr, "need model name as first argument\n"); + exit (1); + } + m = models_get (argv[0]); + if (!m) + { + fprintf (stderr, "unknown model name: %s\n", argv[0]); + exit (1); + } + simu_left_model = *m->motor; + simu_right_model = *m->motor; + simu_footing = m->footing; + simu_pos_x = simu_pos_y = 0; +} + +/** Update simulation position. */ +static void +simu_pos_update (double dl, double dr) +{ + double d = 0.5 * (dl + dr); + double da = (dr - dl) / simu_footing; + double na = simu_pos_a + da; + if (da < 0.0001 && da > -0.0001) + { + /* Avoid a division by zero when angle is too small. */ + double a = simu_pos_a + da * 0.5; + simu_pos_x += d * cos (a); + simu_pos_y += d * sin (a); + } + else + { + /* Radius of turn is d / da. */ + simu_pos_x += (sin (na) - sin (simu_pos_a)) * d / da; + simu_pos_y += (cos (simu_pos_a) - cos (na)) * d / da; + } + simu_pos_a = na; +} + +/** Do a simulation step. */ +static void +simu_step (void) +{ + double old_left_th, old_right_th; + /* Convert pwm value into voltage. */ + assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX); + assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX); + simu_left_model.u = (double) (pwm_left + 1) / (PWM_MAX + 1); + simu_right_model.u = (double) (pwm_right + 1) / (PWM_MAX + 1); + /* Make one step. */ + old_left_th = simu_left_model.th; + old_right_th = simu_right_model.th; + motor_model_step (&simu_left_model); + motor_model_step (&simu_right_model); + /* Modify counters. */ + counter_left_diff = (simu_left_model.th - old_left_th) / (2*M_PI) + * 500 * simu_left_model.i_G; + counter_left += counter_left_diff; + counter_right_diff = (simu_right_model.th - old_right_th) / (2*M_PI) + * 500 * simu_right_model.i_G; + 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 * 1000, + (simu_right_model.th - old_right_th) + * simu_right_model.i_G * simu_right_model.w_r * 1000); +} + +/** Initialise the timer. */ +void +timer_init (void) +{ + simu_init (); +} + +/** Wait for timer overflow. */ +void +timer_wait (void) +{ + simu_step (); +} + +/** Read timer value. Used for performance analysis. */ +uint8_t +timer_read (void) +{ + return 0; +} + +/** Initialize the counters. */ +void +counter_init (void) +{ +} + +/** Update overall counter values and compute diffs. */ +void +counter_update (void) +{ +} + +/** Restart counting. */ +void +counter_restart (void) +{ +} + +/** Initialise PWM generator. */ +void +pwm_init (void) +{ +} + +/** Update the hardware PWM values. */ +void +pwm_update (void) +{ +} + diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h new file mode 100644 index 00000000..2730a71f --- /dev/null +++ b/digital/asserv/src/asserv/simu.host.h @@ -0,0 +1,94 @@ +#ifndef simu_host_h +#define simu_host_h +/* simu.host.h */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * 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. + * + * Contact : + * Web: http://perso.efrei.fr/~schodet/ + * }}} */ + +/** Simulate some AVR regs. */ +extern uint8_t DDRD, PORTD, PORTA, PORTC, PINA, PINF; + +/** Overall counter values. */ +extern uint16_t counter_left, counter_right; +/** Counter differences since last update. + * Maximum of 9 significant bits, sign included. */ +extern int16_t counter_left_diff, counter_right_diff; + +/** Define the direction output for left motor. */ +#define PWM_LEFT_DIR 3 +/** Define the direction output for right motor. */ +#define PWM_RIGHT_DIR 2 + +/** Define the absolute maximum PWM value. */ +#define PWM_MAX 0x3ff + +/** PWM values, this is an error if absolute value is greater than the + * maximum. */ +extern int16_t pwm_left, pwm_right; +/** PWM reverse direction, only set pwm dir bits or you will get weird results + * on port B. */ +extern uint8_t pwm_dir; + +/** Computed simulated position. */ +extern double simu_pos_x, simu_pos_y, simu_pos_a; + +#define EEPROM_KEY 0xa5 +#define eeprom_read_params() do { } while (0) +#define eeprom_write_params() do { } while (0) +#define eeprom_clear_params() do { } while (0) + +/* +AutoDec */ + +/** Initialise the timer. */ +void +timer_init (void); + +/** Wait for timer overflow. */ +void +timer_wait (void); + +/** Read timer value. Used for performance analysis. */ +uint8_t +timer_read (void); + +/** Initialize the counters. */ +void +counter_init (void); + +/** Update overall counter values and compute diffs. */ +void +counter_update (void); + +/** Restart counting. */ +void +counter_restart (void); + +/** Initialise PWM generator. */ +void +pwm_init (void); + +/** Update the hardware PWM values. */ +void +pwm_update (void); + +/* -AutoDec */ + +#endif /* simu_host_h */ diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c new file mode 100644 index 00000000..7e8ed558 --- /dev/null +++ b/digital/asserv/src/asserv/speed.c @@ -0,0 +1,123 @@ +/* speed.c - Speed control. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ + +/** + * This file is responsible for speed control. It changes the current shafts + * positions using ramps. It can be controlled by a wanted speed or wanted + * shaft position. + */ + +/** Current speed, f8.8. */ +int16_t speed_theta_cur, speed_alpha_cur; +/** Consign speed, f8.8. */ +int16_t speed_theta_cons, speed_alpha_cons; +/** Maximum speed for position consign, u8. */ +int8_t speed_theta_max, speed_alpha_max; +/** Slow speed for position consign, u8. */ +int8_t speed_theta_slow, speed_alpha_slow; +/** Consign position. */ +uint32_t speed_theta_pos_cons, speed_alpha_pos_cons; +/** Weither to use the consign position (1) or not (0). */ +uint8_t speed_pos; + +/** Acceleration, uf8.8. */ +int16_t speed_theta_acc, speed_alpha_acc; + +/* +AutoDec */ +/* -AutoDec */ + +/** Update shaft position consign according to a speed consign. */ +static void +speed_update_by_speed (void) +{ + /* Update current speed. */ + if (UTILS_ABS (speed_theta_cons - speed_theta_cur) < speed_theta_acc) + speed_theta_cur = speed_theta_cons; + else if (speed_theta_cons > speed_theta_cur) + speed_theta_cur += speed_theta_acc; + else + speed_theta_cur -= speed_theta_acc; + if (UTILS_ABS (speed_alpha_cons - speed_alpha_cur) < speed_alpha_acc) + speed_alpha_cur = speed_alpha_cons; + else if (speed_alpha_cons > speed_alpha_cur) + speed_alpha_cur += speed_alpha_acc; + else + speed_alpha_cur -= speed_alpha_acc; +} + +/** Compute maximum allowed speed according to: distance left, maximum speed, + * current speed and acceleration. */ +static int16_t +speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max) +{ + int16_t s; + /* Compute maximum speed in order to be able to brake in time. + * s = sqrt (2 * a * d) */ + s = fixed_sqrt_ui32 (2 * (acc >> 8) * UTILS_ABS (d)); + /* Apply consign. */ + s = UTILS_MIN (max, s); + /* Apply sign. */ + if (d < 0) + s = -s; + /* Convert to f8.8 and check acceleration. */ + s = s << 8; + UTILS_BOUND (s, cur - acc, cur + acc); + return s; +} + +/** Update shaft position consign according to a position consign. */ +static void +speed_update_by_position (void) +{ + int32_t theta_d = speed_theta_pos_cons - pos_theta_cons; + int32_t alpha_d = speed_alpha_pos_cons - pos_alpha_cons; + if (theta_d >= -speed_theta_max && theta_d <= speed_theta_max) + speed_theta_cur = theta_d << 8; + else + speed_theta_cur = speed_compute_max_speed (theta_d, speed_theta_cur, + speed_theta_acc, speed_theta_max); + if (alpha_d >= -speed_alpha_max && alpha_d <= speed_alpha_max) + speed_alpha_cur = alpha_d << 8; + else + speed_alpha_cur = speed_compute_max_speed (alpha_d, speed_alpha_cur, + speed_alpha_acc, speed_alpha_max); + if (speed_theta_cur == 0 && speed_alpha_cur == 0) + main_sequence_finish = main_sequence; +} + +/** Update shaft position consign according to consign. */ +static void +speed_update (void) +{ + /* Update speed. */ + if (speed_pos) + speed_update_by_position (); + else + speed_update_by_speed (); + /* Update shaft position. */ + pos_theta_cons += speed_theta_cur >> 8; + pos_alpha_cons += speed_alpha_cur >> 8; +} + diff --git a/digital/asserv/src/asserv/test_motor_model.c b/digital/asserv/src/asserv/test_motor_model.c new file mode 100644 index 00000000..f5fb575b --- /dev/null +++ b/digital/asserv/src/asserv/test_motor_model.c @@ -0,0 +1,70 @@ +/* test_motor_model.c - Test DC motor model. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ +#include "common.h" +#include "motor_model.host.h" +#include "models.host.h" + +#include + +void simu (struct motor_t *m, double t) +{ + int i, s; + s = t / m->h; + for (i = 0; i < s; i++) + { + motor_model_step (m); + printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th); + } +} + +int +main (int argc, char **argv) +{ + struct motor_t ms; + struct motor_t *m; + const struct robot_t *mr; + /* Check arguments. */ + if (argc != 2) + { + fprintf (stderr, "syntax: %s MODEL\n", argv[0]); + return 1; + } + mr = models_get (argv[1]); + if (!mr) + { + fprintf (stderr, "model unknown\n"); + return 1; + } + ms = *mr->motor; + m = &ms; + /* Make a step response simulation. */ + printf ("# %10s %12s %12s %12s %12s\n", "t", "u", "i", "omega", "theta"); + m->u = 3.0; + printf ("%12f %12f %12f %12f %12f\n", m->t, m->u, m->i, m->o, m->th); + simu (m, 1.0); + m->u = 0.0; + simu (m, 1.0); + return 0; +} diff --git a/digital/asserv/src/asserv/timer.avr.c b/digital/asserv/src/asserv/timer.avr.c new file mode 100644 index 00000000..dcad74d0 --- /dev/null +++ b/digital/asserv/src/asserv/timer.avr.c @@ -0,0 +1,70 @@ +/* timer.avr.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * 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. + * + * }}} */ + +/* +AutoDec */ + +/** Initialise the timer. */ +static inline void +timer_init (void); + +/** Wait for timer overflow. */ +static inline void +timer_wait (void); + +/** Read timer value. Used for performance analysis. */ +static inline uint8_t +timer_read (void); + +/* -AutoDec */ + +/** Initialise the timer. */ +static inline void +timer_init (void) +{ + TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00, + 0, 0, 0, 0, 0, 1, 1, 0); + /* Fov = F_io / (prescaler * (TOP + 1)) + * TOP = 0xff + * prescaler = 256 + * Tov = 1 / Fov = 4.444 ms */ +} + +/** Wait for timer overflow. */ +static inline void +timer_wait (void) +{ + while (!(TIFR & _BV (TOV0))) + ; + /* Write 1 to clear. */ + TIFR = _BV (TOV0); +} + +/** Read timer value. Used for performance analysis. */ +static inline uint8_t +timer_read (void) +{ + return TCNT0; +} + diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c new file mode 100644 index 00000000..b80a7513 --- /dev/null +++ b/digital/asserv/src/asserv/traj.c @@ -0,0 +1,76 @@ +/* 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: go to the wall. + */ +uint8_t traj_mode; + +/** Go to the wall mode. */ +void +traj_ftw (void) +{ + int16_t speed; + speed = speed_theta_slow; + speed *= 256; + if (PINA & _BV (0) && PINA & _BV (7)) + { + speed_theta_cons = -speed; + speed_alpha_cons = 0; + } + else if (PINA & _BV (0)) + { + speed_theta_cons = -speed / 2; + speed_alpha_cons = speed / 2; + } + else if (PINA & _BV (7)) + { + speed_theta_cons = -speed / 2; + speed_alpha_cons = -speed / 2; + } + else + { + speed_theta_cons = 0; + speed_alpha_cons = 0; + speed_theta_cur = 0; + speed_alpha_cur = 0; + main_sequence_finish = main_sequence; + traj_mode = 11; + } +} + +/* Compute new speed according the defined trajectory. */ +static void +traj_update (void) +{ + switch (traj_mode) + { + case 10: + traj_ftw (); + case 11: + break; + } +} + -- cgit v1.2.3