From 23db3d37f57602453d21745810232b204b4ca264 Mon Sep 17 00:00:00 2001 From: schodet Date: Fri, 20 Jan 2006 20:36:48 +0000 Subject: PrĂȘt pour la compilation sur host. --- n/asserv/src/asserv/Makefile | 9 +- n/asserv/src/asserv/counter.avr.c | 173 +++++++++++++++++++++++++++++++++ n/asserv/src/asserv/counter.c | 173 --------------------------------- n/asserv/src/asserv/eeprom.avr.c | 78 +++++++++++++++ n/asserv/src/asserv/eeprom.c | 78 --------------- n/asserv/src/asserv/main.c | 12 ++- n/asserv/src/asserv/pwm.avr.c | 125 ++++++++++++++++++++++++ n/asserv/src/asserv/pwm.c | 125 ------------------------ n/asserv/src/asserv/simu.host.c | 94 ++++++++++++++++++ n/asserv/src/asserv/simu.host.h | 92 ++++++++++++++++++ n/asserv/src/asserv/taz_model.c | 52 ++++++++++ n/asserv/src/asserv/test_motor_model.c | 37 ++----- n/asserv/src/asserv/timer.avr.c | 70 +++++++++++++ n/asserv/src/asserv/timer.c | 57 ----------- 14 files changed, 701 insertions(+), 474 deletions(-) create mode 100644 n/asserv/src/asserv/counter.avr.c delete mode 100644 n/asserv/src/asserv/counter.c create mode 100644 n/asserv/src/asserv/eeprom.avr.c delete mode 100644 n/asserv/src/asserv/eeprom.c create mode 100644 n/asserv/src/asserv/pwm.avr.c delete mode 100644 n/asserv/src/asserv/pwm.c create mode 100644 n/asserv/src/asserv/simu.host.c create mode 100644 n/asserv/src/asserv/simu.host.h create mode 100644 n/asserv/src/asserv/taz_model.c create mode 100644 n/asserv/src/asserv/timer.avr.c delete mode 100644 n/asserv/src/asserv/timer.c (limited to 'n/asserv') diff --git a/n/asserv/src/asserv/Makefile b/n/asserv/src/asserv/Makefile index d618c0a..6fe0762 100644 --- a/n/asserv/src/asserv/Makefile +++ b/n/asserv/src/asserv/Makefile @@ -1,10 +1,8 @@ BASE = ../../../avr PROGS = asserv HOST_PROGS = test_motor_model -asserv_SOURCES = main.c -test_motor_model_SOURCES = test_motor_model.c motor_model.c -DOC = -EXTRACTDOC = +asserv_SOURCES = main.c simu.host.c +test_motor_model_SOURCES = test_motor_model.c motor_model.c taz_model.c MODULES = proto uart utils test_motor_model_MODULES = CONFIGFILE = avrconfig.h @@ -14,7 +12,4 @@ AVR_MCU = atmega128 # -Os : size OPTIMIZE = -O2 -DEFS = -LIBS = - include $(BASE)/make/Makefile.gen diff --git a/n/asserv/src/asserv/counter.avr.c b/n/asserv/src/asserv/counter.avr.c new file mode 100644 index 0000000..0dd59f4 --- /dev/null +++ b/n/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 1 +/** 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/n/asserv/src/asserv/counter.c b/n/asserv/src/asserv/counter.c deleted file mode 100644 index c4135b4..0000000 --- a/n/asserv/src/asserv/counter.c +++ /dev/null @@ -1,173 +0,0 @@ -/* counter.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 1 -/** 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/n/asserv/src/asserv/eeprom.avr.c b/n/asserv/src/asserv/eeprom.avr.c new file mode 100644 index 0000000..34770bb --- /dev/null +++ b/n/asserv/src/asserv/eeprom.avr.c @@ -0,0 +1,78 @@ +/* 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 + +#define EEPROM_KEY 0xa5 + +/* +AutoDec */ +/* -AutoDec */ + +/* Read parameters from eeprom. */ +static void +eeprom_read_params (void) +{ + uint8_t *p8 = 0; + uint16_t *p16; + if (eeprom_read_byte (p8++) != EEPROM_KEY) + return; + pwm_dir = eeprom_read_byte (p8++); + p16 = (uint16_t *) p8; + 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++); +} + +/* Write parameters to eeprom. */ +static void +eeprom_write_params (void) +{ + uint8_t *p8 = 0; + uint16_t *p16; + eeprom_write_byte (p8++, EEPROM_KEY); + eeprom_write_byte (p8++, pwm_dir); + p16 = (uint16_t *) p8; + 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); +} + +/* Clear eeprom parameters. */ +static void +eeprom_clear_params (void) +{ + uint8_t *p = 0; + eeprom_write_byte (p, 0xff); +} + diff --git a/n/asserv/src/asserv/eeprom.c b/n/asserv/src/asserv/eeprom.c deleted file mode 100644 index 34770bb..0000000 --- a/n/asserv/src/asserv/eeprom.c +++ /dev/null @@ -1,78 +0,0 @@ -/* 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 - -#define EEPROM_KEY 0xa5 - -/* +AutoDec */ -/* -AutoDec */ - -/* Read parameters from eeprom. */ -static void -eeprom_read_params (void) -{ - uint8_t *p8 = 0; - uint16_t *p16; - if (eeprom_read_byte (p8++) != EEPROM_KEY) - return; - pwm_dir = eeprom_read_byte (p8++); - p16 = (uint16_t *) p8; - 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++); -} - -/* Write parameters to eeprom. */ -static void -eeprom_write_params (void) -{ - uint8_t *p8 = 0; - uint16_t *p16; - eeprom_write_byte (p8++, EEPROM_KEY); - eeprom_write_byte (p8++, pwm_dir); - p16 = (uint16_t *) p8; - 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); -} - -/* Clear eeprom parameters. */ -static void -eeprom_clear_params (void) -{ - uint8_t *p = 0; - eeprom_write_byte (p, 0xff); -} - diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c index f56c662..87124cf 100644 --- a/n/asserv/src/asserv/main.c +++ b/n/asserv/src/asserv/main.c @@ -32,12 +32,16 @@ #include "misc.h" /* This is implementation include. */ -#include "timer.c" -#include "counter.c" -#include "pwm.c" +#ifndef HOST +# include "timer.avr.c" +# include "counter.avr.c" +# include "pwm.avr.c" +# include "eeprom.avr.c" +#else +# include "simu.host.h" +#endif #include "pos.c" #include "speed.c" -#include "eeprom.c" /** Motor control mode: * 0: pwm setup. diff --git a/n/asserv/src/asserv/pwm.avr.c b/n/asserv/src/asserv/pwm.avr.c new file mode 100644 index 0000000..773cfef --- /dev/null +++ b/n/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/n/asserv/src/asserv/pwm.c b/n/asserv/src/asserv/pwm.c deleted file mode 100644 index 50f4842..0000000 --- a/n/asserv/src/asserv/pwm.c +++ /dev/null @@ -1,125 +0,0 @@ -/* pwm.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/n/asserv/src/asserv/simu.host.c b/n/asserv/src/asserv/simu.host.c new file mode 100644 index 0000000..5b2b6d4 --- /dev/null +++ b/n/asserv/src/asserv/simu.host.c @@ -0,0 +1,94 @@ +/* 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/ + * Email: + * }}} */ +#include "common.h" +#include "simu.host.h" + +/** Simulate some AVR regs. */ +uint8_t DDRD, PORTD, PORTA, PORTC, PINA; + +/** 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; + +/* +AutoDec */ +/* -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) +{ + 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/n/asserv/src/asserv/simu.host.h b/n/asserv/src/asserv/simu.host.h new file mode 100644 index 0000000..c2a4934 --- /dev/null +++ b/n/asserv/src/asserv/simu.host.h @@ -0,0 +1,92 @@ +#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/ + * Email: + * }}} */ + +/** Simulate some AVR regs. */ +extern uint8_t DDRD, PORTD, PORTA, PORTC, PINA; + +/** 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; + +#define EEPROM_KEY 0xa5 +#define eeprom_read_params() +#define eeprom_write_params() +#define eeprom_clear_params() + +/* +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/n/asserv/src/asserv/taz_model.c b/n/asserv/src/asserv/taz_model.c new file mode 100644 index 0000000..a590029 --- /dev/null +++ b/n/asserv/src/asserv/taz_model.c @@ -0,0 +1,52 @@ +/* taz_model.c */ +/* 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/ + * Email: + * }}} */ +#include "motor_model.h" + +#include + +/* Taz model. */ +struct motor_t taz_model = +{ + /* Motor caracteristics. */ + 407 * M_2_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). */ + /* 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). */ +}; + diff --git a/n/asserv/src/asserv/test_motor_model.c b/n/asserv/src/asserv/test_motor_model.c index 14bab66..a478244 100644 --- a/n/asserv/src/asserv/test_motor_model.c +++ b/n/asserv/src/asserv/test_motor_model.c @@ -25,33 +25,9 @@ #include "common.h" #include "motor_model.h" -#include #include -/* Our robot model. */ -struct motor_t taz = -{ - /* Motor caracteristics. */ - 407 * M_2_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). */ - /* 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). */ -}; +extern struct motor_t taz_model; void simu (struct motor_t *m, double t) { @@ -67,12 +43,13 @@ void simu (struct motor_t *m, double t) int main (void) { + struct motor_t *m = &taz_model; /* Make a step response simulation. */ printf ("# %10s %12s %12s %12s %12s\n", "t", "u", "i", "omega", "theta"); - taz.u = 3.0; - printf ("%12f %12f %12f %12f %12f\n", taz.t, taz.u, taz.i, taz.o, taz.th); - simu (&taz, 1.0); - taz.u = 0.0; - simu (&taz, 1.0); + 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/n/asserv/src/asserv/timer.avr.c b/n/asserv/src/asserv/timer.avr.c new file mode 100644 index 0000000..dcad74d --- /dev/null +++ b/n/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/n/asserv/src/asserv/timer.c b/n/asserv/src/asserv/timer.c deleted file mode 100644 index e91d8a8..0000000 --- a/n/asserv/src/asserv/timer.c +++ /dev/null @@ -1,57 +0,0 @@ -/* timer.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 */ -/* -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; -} - -- cgit v1.2.3