From 55b33a20a69479219745172575c4504acd52730d Mon Sep 17 00:00:00 2001 From: schodet Date: Wed, 29 Sep 2004 21:07:25 +0000 Subject: Initial revision --- n/asserv/src/Makefile | 18 +++ n/asserv/src/Makefile.avr | 100 ++++++++++++ n/asserv/src/avrconfig.h | 61 +++++++ n/asserv/src/counter.c | 97 +++++++++++ n/asserv/src/counter.h | 43 +++++ n/asserv/src/dsp.c | 55 +++++++ n/asserv/src/main.c | 36 +++++ n/asserv/src/motor.c | 402 ++++++++++++++++++++++++++++++++++++++++++++++ n/asserv/src/motor.h | 74 +++++++++ n/asserv/src/pwm.c | 92 +++++++++++ n/asserv/src/pwm.h | 42 +++++ n/asserv/src/test_pwm.c | 66 ++++++++ n/asserv/src/timer.c | 55 +++++++ n/asserv/src/timer.h | 39 +++++ 14 files changed, 1180 insertions(+) create mode 100644 n/asserv/src/Makefile create mode 100644 n/asserv/src/Makefile.avr create mode 100644 n/asserv/src/avrconfig.h create mode 100644 n/asserv/src/counter.c create mode 100644 n/asserv/src/counter.h create mode 100644 n/asserv/src/dsp.c create mode 100644 n/asserv/src/main.c create mode 100644 n/asserv/src/motor.c create mode 100644 n/asserv/src/motor.h create mode 100644 n/asserv/src/pwm.c create mode 100644 n/asserv/src/pwm.h create mode 100644 n/asserv/src/test_pwm.c create mode 100644 n/asserv/src/timer.c create mode 100644 n/asserv/src/timer.h (limited to 'n/asserv/src') diff --git a/n/asserv/src/Makefile b/n/asserv/src/Makefile new file mode 100644 index 0000000..474512c --- /dev/null +++ b/n/asserv/src/Makefile @@ -0,0 +1,18 @@ +#PROGS = test_pwm +#SOURCES = test_pwm.c pwm.c +PROGS = asserv +SOURCES = motor.c main.c +DOC = +EXTRACTDOC = +MODULES = n/avr/rs232 n/avr/proto n/avr/utils +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +MCU_TARGET = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 + +DEFS = +LIBS = + +include Makefile.avr diff --git a/n/asserv/src/Makefile.avr b/n/asserv/src/Makefile.avr new file mode 100644 index 0000000..01c9b7b --- /dev/null +++ b/n/asserv/src/Makefile.avr @@ -0,0 +1,100 @@ +# Flags. {{{1 + +CC = avr-gcc + +CFLAGS = -g -Wall $(OPTIMIZE) -mmcu=$(MCU_TARGET) \ + $(if $(CONFIGFILE), $(CONFIGFILE:%=-include %)) +CPPFLAGS = $(DEFS) -Imodules +LDFLAGS = + +OBJCOPY = avr-objcopy +OBJDUMP = avr-objdump + +# Main rules. {{{1 + +CSOURCES = $(filter %.c, $(SOURCES)) +SSOURCES = $(filter %.s, $(SOURCES)) +OBJECTS = $(CSOURCES:%.c=%.o) $(SSOURCES:%.s=%.o) +ELFS = $(PROGS:%=%.elf) + +all: elf lst hex + +.PHONY: all clean elf lst doc text hex srec bin eeprom ehex esrec ebin + +# Rules for modules. {{{1 + +MODULESFILES = $(MODULES:%=modules/%/Makefile.module) + +include $(MODULESFILES) + +SOURCES += $(MODULESSOURCES) + +$(MODULESFILES): + mkdir -p modules + cd modules && cvs co $(@:modules/%/Makefile.module=%) + test -f $@ + +# General rules. {{{1 + +elf: $(PROGS:%=%.elf) +lst: $(PROGS:%=%.lst) + +$(PROGS:%=%.elf): $(OBJECTS) + $(CC) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LIBS) + +%.lst: %.elf + $(OBJDUMP) -h -S $< > $@ + +clean: + rm -f *.o $(OBJECTS) $(PROGS:%=%.elf) *.lst *.map *.bak *~ + rm -f $(DOC) *.exd $(EXTRA_CLEAN_FILES) $(TEXTS) $(EEPROMS) + +# Rules for building the doc. {{{1 + +doc: $(DOC) + +%.html: %.txt %.exd + aft $< + +%.exd: $(EXTRACTDOC) + test -n "$^" && extractdoc $^ > $@ || true + +# Rules for building the .text rom images. {{{1 + +TEXTS = $(PROGS:%=%.hex) $(PROGS:%=%.bin) $(PROGS:%=%.srec) + +text: hex bin srec + +hex: $(PROGS:%=%.hex) +bin: $(PROGS:%=%.bin) +srec: $(PROGS:%=%.srec) + +%.hex: %.elf + $(OBJCOPY) -j .text -j .data -O ihex $< $@ + +%.srec: %.elf + $(OBJCOPY) -j .text -j .data -O srec $< $@ + +%.bin: %.elf + $(OBJCOPY) -j .text -j .data -O binary $< $@ + +# Rules for building the .eeprom rom images. {{{1 + +EEPROMS = $(PROGS:%=%_eeprom.hex) $(PROGS:%=%_eeprom.bin) \ + $(PROGS:%=%_eeprom.srec) + +eeprom: ehex ebin esrec + +ehex: $(PROGS:%=%_eeprom.hex) +ebin: $(PROGS:%=%_eeprom.bin) +esrec: $(PROGS:%=%_eeprom.srec) + +%_eeprom.hex: %.elf + $(OBJCOPY) -j .eeprom --change-section-lma .eeprom=0 -O ihex $< $@ + +%_eeprom.srec: %.elf + $(OBJCOPY) -j .eeprom --change-section-lma .eeprom=0 -O srec $< $@ + +%_eeprom.bin: %.elf + $(OBJCOPY) -j .eeprom --change-section-lma .eeprom=0 -O binary $< $@ + diff --git a/n/asserv/src/avrconfig.h b/n/asserv/src/avrconfig.h new file mode 100644 index 0000000..cf92dd8 --- /dev/null +++ b/n/asserv/src/avrconfig.h @@ -0,0 +1,61 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - config file for avr projects. */ +/* n.asserv - Asservissement moteur continu. {{{ + * + * Copyright (C) 2004 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: + * }}} */ + +/* global */ +/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, + * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#define AC_FREQ 14745600 + +/* rs232 - RS232 Module. */ +/** Baudrate : 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, + * 115200, 230400, 250000, 500000, 1000000. */ +#define AC_RS232_BAUDRATE 115200 +/** Send mode : + * - POLLING : no interrupts; + * - RING : interrupts, ring buffer. */ +#define AC_RS232_SEND_MODE POLLING +/** Recv mode, same as send mode. */ +#define AC_RS232_RECV_MODE POLLING +/** Character size : 5, 6, 7, 8, 9 (only 8 implemented). */ +#define AC_RS232_CHAR_SIZE 8 +/** Parity : ODD, EVEN, NONE. */ +#define AC_RS232_PARITY EVEN +/** Stop bits : 1, 2. */ +#define AC_RS232_STOP_BITS 1 +/** Send buffer size, should be power of 2 for RING mode. */ +#define AC_RS232_SEND_BUFFER_SIZE 32 +/** Recv buffer size, should be power of 2 for RING mode. */ +#define AC_RS232_RECV_BUFFER_SIZE 32 +/** Select serial port (0 or 1). */ +#define AC_RS232_PORT 1 + +/* proto - Protocol module. */ +/** Maximum argument number. */ +#define AC_PROTO_MAX_ARGS 3 +/** Protocol arguments type. */ +#define AC_PROTO_ARG_TYPE uint8_t + +#endif /* avrconfig_h */ diff --git a/n/asserv/src/counter.c b/n/asserv/src/counter.c new file mode 100644 index 0000000..a9b5760 --- /dev/null +++ b/n/asserv/src/counter.c @@ -0,0 +1,97 @@ +/* counter.c */ +/* {{{ + * + * Copyright (C) 2004 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 "counter.h" +#include + +/* +AutoDec */ +/* -AutoDec */ + +/** Forward and reverse counter value. */ +static uint8_t counter_l_frw, counter_l_rev, counter_r_frw, counter_r_rev; +/** Last TCNT value. */ +static uint8_t counter_l_old, counter_r_old; + +/** Initialize the counters. */ +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_reset (); +} + +/* Update counter, should be called as often as possible. */ +inline void +counter_update (void) +{ + uint8_t c; + /* Update left counter. */ + c = TCNT2; + if (PINE & _BV (4)) + { + PORTD &= ~0x40; + counter_l_frw += c - counter_l_old; + } + else + { + PORTD |= 0x40; + counter_l_rev += c - counter_l_old; + } + counter_l_old = c; + /* Update right counter. */ + c = TCNT3L; + if (PINE & _BV (5)) + { + PORTD &= ~0x20; + counter_r_frw += c - counter_r_old; + } + else + { + PORTD |= 0x20; + counter_r_rev += c - counter_r_old; + } + counter_r_old = c; +} + +/* Reset the counters. */ +inline void +counter_reset (void) +{ + counter_l_frw = 0; + counter_l_rev = 0; + counter_l_old = TCNT2; + counter_r_frw = 0; + counter_r_rev = 0; + counter_r_old = TCNT3L; +} + diff --git a/n/asserv/src/counter.h b/n/asserv/src/counter.h new file mode 100644 index 0000000..1f8ed88 --- /dev/null +++ b/n/asserv/src/counter.h @@ -0,0 +1,43 @@ +#ifndef counter_h +#define counter_h +/* counter.h */ +/* {{{ + * + * Copyright (C) 2004 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: + * }}} */ + +/* +AutoDec */ + +/** Initialize the counters. */ +inline void +counter_init (void); + +/* Update counter, should be called as often as possible. */ +inline void +counter_update (void); + +/* Reset the counters. */ +inline void +counter_reset (void); + +/* -AutoDec */ + +#endif /* counter_h */ diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c new file mode 100644 index 0000000..71317a5 --- /dev/null +++ b/n/asserv/src/dsp.c @@ -0,0 +1,55 @@ +/* dsp.c */ +/* {{{ + * + * Copyright (C) 2004 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 "dsp.h" + +/* +AutoDec */ +/* -AutoDec */ + +/* Add two signed words and saturate. */ +inline int16_t +dsp_i16i16_add_sat (int16_t a, int16_t b) +{ + asm ("add %A0, %A1\n\t" + "adc %B0, %B1\n\t" + /* Branch if not V. */ + "brvc 1f\n\t" + /* Load Max. */ + "ldi %A0, 0xff\n\t" + "ldi %B0, 0x7f\n\t" + /* Branch if not S. */ + "brlt 1f\n\t" + /* Load Min. */ + "ldi %A0, 0x00\n\t" + "ldi %B0, 0x80\n\t" + "1f" "\n\t" + : "=r" a : "0" a, "r" b); + return a; +} + +/* Multiply and saturate. */ +inline int16_t +dsp_i16u8_mul_sat (int16_t a, uint8_t b) +{ + asm ("mul +} diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c new file mode 100644 index 0000000..2a25fd7 --- /dev/null +++ b/n/asserv/src/main.c @@ -0,0 +1,36 @@ +/* main.c */ +/* APBTasserv - asservissement Robot 2005 {{{ + * + * Copyright (C) 2004 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.h" + +#include + +int +main (void) +{ + DDRD = 0x60; + motor_init (); + motor_main (); + sei (); + return 0; +} diff --git a/n/asserv/src/motor.c b/n/asserv/src/motor.c new file mode 100644 index 0000000..f54d989 --- /dev/null +++ b/n/asserv/src/motor.c @@ -0,0 +1,402 @@ +/* motor.c */ +/* APBTasserv - asservissement Robot 2005 {{{ + * + * Copyright (C) 2004 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.h" +#include "pwm.h" +#include "timer.h" +#include "counter.h" +#include +#include + +/* +AutoDec */ +/* -AutoDec */ + +/* Variables globales. */ +uint8_t motor_asservi; /* Asservissement activé ? */ +uint8_t motor_pos_asserv; /* Si vrai, la carte d'asservissement est en + mode d'asservissement en position. C'est à + dire, l'ordinateur connecté à la carte + d'asservissement gère lui même la vitesse + afin d'asservir la position. En pratique, + - pas d'acquitement lors d'un !v + - arrét automatique si aucune commande !v + reçue aprés n cycles. Voir motor_ttl. */ +uint8_t motor_ttl; /* Time to live : arrète le robot si arrive à + 0 en mode asservissement position. */ +uint8_t motor_kp, motor_ki, motor_kd; /* Coefficients du PID. */ +uint8_t motor_a; /* Acceleration. */ +uint8_t motor_a_cpt; /* Compteur d'acceleration. */ +int16_t motor_int_max; /* Terme intégral maximum. */ +uint8_t motor_pid_int; /* Compteur d'interruptions timer entre deux + calculs de PID. */ + +/* Statistiques, etc... */ +uint8_t motor_stat_delay; /* Delais entre les stats. */ +uint8_t motor_stat_delay_cpt; /* Compteur de stats. */ +uint8_t motor_cpt_delay; /* Delais entre rapport codeurs. */ +uint8_t motor_cpt_delay_cpt; /* Compteur entre les rapports. */ + +/* Entrées. */ +uint8_t motor_gpi_delay; /* Delais entre deux envois. */ +uint8_t motor_gpi_delay_cpt; /* Compteur. */ + +/* Moteurs. */ +int8_t motor_g_vdes, motor_g_vacc; /* Vitesse désirée, actuelle. */ +int8_t motor_d_vdes, motor_d_vacc; +int16_t motor_g_cpt, motor_d_cpt; /* Compteurs. */ +int16_t motor_g_e_old, motor_d_e_old; /* Dernière erreur, pour le + calcul de la dérivée. */ +int16_t motor_g_pwm_old, motor_d_pwm_old; /* Dernière pwm, pour les + stats. */ +int16_t motor_g_int, motor_d_int; /* Valeur integrale. */ +int16_t motor_g_der, motor_d_der; /* Valeur dérivée. */ + +/* On fait les cradingues... */ +#include "pwm.c" +#include "timer.c" +#include "counter.c" + +/* Initialise le module moteur. */ +void +motor_init (void) +{ + motor_asservi = 0; + motor_pos_asserv = 0; + motor_ttl = 10; + motor_kp = 10; motor_ki = 5; motor_kd = 0; + motor_a = 8; + motor_a_cpt = 8; + motor_int_max = 1000; + motor_pid_int = 0; + motor_stat_delay = 0; motor_stat_delay_cpt = 0; + motor_cpt_delay = 0; motor_cpt_delay_cpt = 0; + motor_gpi_delay = 0; motor_gpi_delay_cpt = 0; + motor_g_vdes = 0; motor_g_vacc = 0; + motor_d_vdes = 0; motor_d_vacc = 0; + motor_g_cpt = 0; motor_d_cpt = 0; + motor_g_e_old = 0; motor_d_e_old = 0; + motor_g_pwm_old = 0; motor_d_pwm_old = 0; + motor_g_int = 0; motor_d_int = 0; + motor_g_der = 0; motor_d_der = 0; + pwm_init (); + timer_init (); + counter_init (); + rs232_init (); + proto_init (motor_serial_callback, rs232_putc); + proto_send0 ('z'); +} + +/* Limite un entier entre -MAX et MAX. */ +inline int16_t +boundary (int16_t n, int16_t max) +{ + if (n > max) + return max; + if (n < -max) + return -max; + return n; +} + +/* Met à jour la vitesse du moteur gauche. */ +void +motor_update_left_speed (void) +{ + if (motor_g_vacc != motor_g_vdes) + { + if (motor_g_vacc < motor_g_vdes) + { + /* Accélère. */ + motor_g_vacc++; + } + else + { + /* Freine. */ + motor_g_vacc--; + } + } +} + +/* Met à jour la vitesse du moteur droit. */ +void +motor_update_right_speed (void) +{ + if (motor_d_vacc != motor_d_vdes) + { + if (motor_d_vacc < motor_d_vdes) + { + /* Accélère. */ + motor_d_vacc++; + } + else + { + /* Freine. */ + motor_d_vacc--; + } + } +} + +/* Calcule le PID associé au moteur gauche. */ +void +motor_compute_left_pid (void) +{ + int16_t e; + int16_t diff; + int16_t pwm; + int16_t temp; + /* Récupère les compteurs et calcule l'erreur. */ + diff = counter_l_frw; + counter_l_frw = 0; + diff -= counter_l_rev; + counter_l_rev = 0; + motor_g_cpt += diff; + e = motor_g_vacc - diff; /* 10b = 8b + 9b */ + /* Calcul de l'integrale. */ + motor_g_int = motor_g_int + e; /* 12b = 11b + 10b */ + motor_g_int = boundary (motor_g_int, motor_int_max); /* 11b */ + /* Calcul de la dérivée. */ + //motor_g_der = safe_sub_long (e, motor_g_e_old); + /* Calcul du PID. P... */ + pwm = e * motor_kp; /* 15b = 10b * 5b */ + /* I... */ + temp = motor_g_int * motor_ki; /* 15b = 11b * 4b */ + pwm = pwm + temp; /* 16b = 15b + 15b */ + /* D... */ + //temp = safe_mul_long (motor_g_der, motor_kd); + //pwm = safe_add_long (pwm, temp); + /* Conserve l'ancienne erreur pour le terme dérivé. */ + motor_g_e_old = e; + /* Conserve l'ancienne pwm. */ + motor_g_pwm_old = pwm; +} + +/* Calcule le PID associé au moteur droit. */ +void +motor_compute_right_pid (void) +{ + int16_t e; + int16_t diff; + int16_t pwm; + int16_t temp; + /* Récupère les compteurs et calcule l'erreur. */ + diff = counter_r_frw; + counter_r_frw = 0; + diff -= counter_r_rev; + counter_r_rev = 0; + motor_d_cpt += diff; + e = motor_d_vacc - diff; + /* Calcul de l'integrale. */ + motor_d_int = motor_d_int + e; + motor_d_int = boundary (motor_d_int, motor_int_max); + /* Calcul de la dérivée. */ + //motor_d_der = safe_sub_long (e, motor_d_e_old); + /* Calcul du PID. P... */ + pwm = e * motor_kp; + /* I... */ + temp = motor_d_int * motor_ki; + pwm = pwm + temp; + /* D... */ + //temp = safe_mul_long (motor_d_der, motor_kd); + //pwm = safe_add_long (pwm, temp); + /* Conserve l'ancienne erreur pour le terme dérivé. */ + motor_d_e_old = e; + /* Conserve l'ancienne pwm. */ + motor_d_pwm_old = pwm; +} + +/* Boucle principale. */ +void +motor_main (void) +{ + while (1) + { + /* Ne fait le traitement que s'il y a eu une interruption. */ + while (!timer_pending ()) + counter_update (); + /* Calcul du PID. */ + if (1) + { + motor_compute_left_pid (); + motor_compute_right_pid (); + /* Applique les nouvelles valeurs au même moment. */ + if (motor_asservi) + { + pwm_set_left (motor_g_pwm_old); + pwm_set_right (motor_d_pwm_old); + } + else + { + pwm_set_left (0); + pwm_set_right (0); + } + /* Information de PWM. */ + if (motor_stat_delay) + { + if (!--motor_stat_delay_cpt) + { + proto_send4 ('l', motor_g_vacc, motor_g_e_old, + motor_g_pwm_old >> 8, motor_g_pwm_old & 0xff); + proto_send4 ('r', motor_d_vacc, motor_d_e_old, + motor_d_pwm_old >> 8, motor_d_pwm_old & 0xff); + motor_stat_delay_cpt = motor_stat_delay; + } + } + /* Rapport des codeurs. */ + if (motor_cpt_delay) + { + if (!--motor_cpt_delay_cpt) + { + proto_send4 ('C', motor_g_cpt >> 8, motor_g_cpt & 0xff, + motor_d_cpt >> 8, motor_d_cpt & 0xff); + motor_cpt_delay_cpt = motor_cpt_delay; + } + } + /* Accélère. */ + if (motor_a) + { + if (!--motor_a_cpt) + { + motor_update_left_speed (); + motor_update_right_speed (); + motor_a_cpt = motor_a; + } + } + else + { + motor_g_vacc = motor_g_vdes; + motor_d_vacc = motor_d_vdes; + } + } + /* Le reste. */ + if ((motor_pid_int & 7) == 0) + { + /* Gestion du ttl. */ + if (motor_pos_asserv && (motor_g_vdes || motor_d_vdes) && + --motor_ttl == 0) + { + motor_g_vdes = 0; + motor_d_vdes = 0; + //serial_send_motor_ttl (); + } + if (rs232_poll ()) + proto_accept (rs232_getc ()); + /* GPI. */ + if (motor_gpi_delay) + { + if (!--motor_gpi_delay_cpt) + { + //serial_send_gpi (input_d ()); + motor_gpi_delay_cpt = motor_gpi_delay; + } + } + } + motor_pid_int++; + } +} + +/* Traite une entrée série. */ +void +motor_serial_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) +{ +#define c(cmd, argc) (cmd << 8 | argc) + switch (c (cmd, argc)) + { + case c ('z', 0): + reset (); + break; + case c ('v', 2): + motor_g_vdes = argv[0]; + motor_d_vdes = argv[1]; + if (motor_pos_asserv) + { + motor_ttl = 10; + return; + } + break; + case c ('V', 1): + motor_pos_asserv = argv[0]; + break; + case c ('s', 0): + motor_g_vdes = 0; + motor_d_vdes = 0; + break; + case c ('a', 1): + motor_a = argv[0]; + motor_a_cpt = motor_a; + break; + case c ('p', 1): + motor_kp = argv[0]; + break; + case c ('i', 1): + motor_ki = argv[0]; + break; + case c ('d', 1): + motor_kd = argv[0]; + break; + case c ('m', 1): + motor_stat_delay = argv[0]; + motor_stat_delay_cpt = motor_stat_delay; + break; + case c ('c', 1): + motor_cpt_delay = argv[0]; + motor_cpt_delay_cpt = motor_cpt_delay; + break; + case c ('g', 1): + motor_asservi = argv[0]; + motor_toggle_asservi (); + break; + case c ('h', 1): + motor_gpi_delay = argv[0]; + motor_gpi_delay_cpt = motor_gpi_delay; + break; + case c ('k', 1): + //temp = argv[0]; + //output_b (temp); + break; + case c ('D', 1): + //temp = argv[0]; + //if (temp == 0x42) + //enable_interrupts (INT_EXT); + break; + default: + proto_send0 ('e'); + return; + } + proto_send (cmd, argc, argv); +#undef c +} + +/* Démarre l'asservissement. */ +void +motor_toggle_asservi (void) +{ + if (motor_asservi) + { + counter_reset (); + motor_g_vacc = 0; + motor_d_vdes = 0; + motor_g_vdes = 0; + motor_d_vdes = 0; + motor_g_int = 0; + motor_d_int = 0; + } +} diff --git a/n/asserv/src/motor.h b/n/asserv/src/motor.h new file mode 100644 index 0000000..5412712 --- /dev/null +++ b/n/asserv/src/motor.h @@ -0,0 +1,74 @@ +#ifndef motor_h +#define motor_h +/* motor.h */ +/* {{{ + * + * Copyright (C) 2004 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 +#include +#include + +/* +AutoDec */ + +/* Initialise le module moteur. */ +void +motor_init (void); + +/* Limite un entier entre -MAX et MAX. */ +inline int16_t +boundary (int16_t n, int16_t max); + +/* Met à jour la vitesse du moteur gauche. */ +void +motor_update_left_speed (void); + +/* Met à jour la vitesse du moteur droit. */ +void +motor_update_right_speed (void); + +/* Calcule le PID associé au moteur gauche. */ +void +motor_compute_left_pid (void); + +/* Calcule le PID associé au moteur droit. */ +void +motor_compute_right_pid (void); + +/* Boucle principale. */ +void +motor_main (void); + +/* Traite une entrée série. */ +void +motor_serial_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]); + +short +motor_parse (void); + +/* Démarre l'asservissement. */ +void +motor_toggle_asservi (void); + +/* -AutoDec */ + +#endif /* motor_h */ diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c new file mode 100644 index 0000000..84791d4 --- /dev/null +++ b/n/asserv/src/pwm.c @@ -0,0 +1,92 @@ +/* pwm.c */ +/* {{{ + * + * Copyright (C) 2004 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 "pwm.h" +#include + +/* +AutoDec */ +/* -AutoDec */ + +inline void +pwm_init (void) +{ + /* No timer/counter interrupt. */ + //TIMSK = 0; + //ETIMSK = 0; + /* Phase correct PWM, TOP = 0xff, OC1B & OC1C with positive logic. + f_IO without prescaler. + Fpwm = f_IO / (2 * prescaler * TOP) = 28912 Hz. */ + TCCR1A = _BV (COM1B1) | _BV (COM1C1) | _BV (WGM10); + TCCR1B = _BV (CS10); + /* Enable pwm and direction outputs in DDRB. */ + DDRB |= _BV (7) | _BV (6) | _BV (3) | _BV (2); +} + +static inline int16_t +pwm_preproc (int16_t v) +{ + v = (v >> 2) + ((v >> 1) & 1); + /*if (v > 0) + v += 0x0f; + else if (v < 0) + v -= 0x0f;*/ + if (v > 255) + return 255; + else if (v < -255) + return -255; + else + return v; +} + +inline void +pwm_set_left (int16_t v) +{ + v = pwm_preproc (v); + if (v < 0) + { + PORTB &= ~_BV (2); + OCR1B = -v; + } + else + { + PORTB |= _BV (2); + OCR1B = v; + } +} + +inline void +pwm_set_right (int16_t v) +{ + v = pwm_preproc (v); + if (v < 0) + { + PORTB |= _BV (3); + OCR1C = -v; + } + else + { + PORTB &= ~_BV (3); + OCR1C = v; + } +} + diff --git a/n/asserv/src/pwm.h b/n/asserv/src/pwm.h new file mode 100644 index 0000000..7ae4902 --- /dev/null +++ b/n/asserv/src/pwm.h @@ -0,0 +1,42 @@ +#ifndef pwm_h +#define pwm_h +/* pwm.h */ +/* {{{ + * + * Copyright (C) 2004 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 + +/* +AutoDec */ + +inline void +pwm_init (void); + +inline void +pwm_set_left (int16_t v); + +inline void +pwm_set_right (int16_t v); + +/* -AutoDec */ + +#endif /* pwm_h */ diff --git a/n/asserv/src/test_pwm.c b/n/asserv/src/test_pwm.c new file mode 100644 index 0000000..ee1be2a --- /dev/null +++ b/n/asserv/src/test_pwm.c @@ -0,0 +1,66 @@ +/* test_pwm.c */ +/* {{{ + * + * Copyright (C) 2004 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 "pwm.h" + +#include +#include +#include + +/* +AutoDec */ +/* -AutoDec */ + +void +proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[]) +{ + switch (c | argc << 8) + { + case 'l' | 2 << 8: + pwm_set_left (argv[0] << 8 | argv[1]); + break; + case 'r' | 2 << 8: + pwm_set_right (argv[0] << 8 | argv[1]); + break; + default: + proto_send0 ('e'); + return; + } + proto_send (c, argc, argv); +} + +int +main (void) +{ + rs232_init (); + pwm_init (); + proto_init (proto_callback, rs232_putc); + rs232_putc ('!'); + rs232_putc ('z'); + rs232_putc ('\r'); + while (1) + { + uint8_t c = rs232_getc (); + proto_accept (c); + } + return 0; +} diff --git a/n/asserv/src/timer.c b/n/asserv/src/timer.c new file mode 100644 index 0000000..b5d0b4d --- /dev/null +++ b/n/asserv/src/timer.c @@ -0,0 +1,55 @@ +/* timer.c */ +/* {{{ + * + * Copyright (C) 2004 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 "timer.h" +#include +#include + +/* +AutoDec */ +/* -AutoDec */ + +/** Initialise the timer. */ +inline void +timer_init (void) +{ + /* 1024 prescaler. */ + TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00, + 0, 0, 0, 0, 0, 1, 1, 0); + /* Fov = F_io / (prescaler * (TOP + 1)) + * Tov = 1 / Fov = 4.44 ms */ +} + +/* Test if a overflow occured and reset the flag. */ +inline int +timer_pending (void) +{ + if (TIFR & _BV (TOV0)) + { + /* Write 1 to clear. */ + TIFR |= _BV (TOV0); + return 1; + } + else + return 0; +} + diff --git a/n/asserv/src/timer.h b/n/asserv/src/timer.h new file mode 100644 index 0000000..8f6ae1e --- /dev/null +++ b/n/asserv/src/timer.h @@ -0,0 +1,39 @@ +#ifndef timer_h +#define timer_h +/* timer.h */ +/* {{{ + * + * Copyright (C) 2004 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: + * }}} */ + +/* +AutoDec */ + +/** Initialise the timer. */ +inline void +timer_init (void); + +/* Test if a overflow occured and reset the flag. */ +inline int +timer_pending (void); + +/* -AutoDec */ + +#endif /* timer_h */ -- cgit v1.2.3