From 0cc8897a51d07baaf42db53f70c9c9aac5272b86 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 6 Nov 2005 18:01:08 +0000 Subject: Première version du programme asserv. --- n/asserv/src/asserv/Makefile | 17 +++ n/asserv/src/asserv/avrconfig.h | 86 +++++++++++++ n/asserv/src/asserv/counter.c | 173 ++++++++++++++++++++++++++ n/asserv/src/asserv/eeprom.c | 78 ++++++++++++ n/asserv/src/asserv/main.c | 260 ++++++++++++++++++++++++++++++++++++++++ n/asserv/src/asserv/pos.c | 104 ++++++++++++++++ n/asserv/src/asserv/pwm.c | 125 +++++++++++++++++++ n/asserv/src/asserv/timer.c | 57 +++++++++ 8 files changed, 900 insertions(+) create mode 100644 n/asserv/src/asserv/Makefile create mode 100644 n/asserv/src/asserv/avrconfig.h create mode 100644 n/asserv/src/asserv/counter.c create mode 100644 n/asserv/src/asserv/eeprom.c create mode 100644 n/asserv/src/asserv/main.c create mode 100644 n/asserv/src/asserv/pos.c create mode 100644 n/asserv/src/asserv/pwm.c create mode 100644 n/asserv/src/asserv/timer.c (limited to 'n') diff --git a/n/asserv/src/asserv/Makefile b/n/asserv/src/asserv/Makefile new file mode 100644 index 0000000..4e8f054 --- /dev/null +++ b/n/asserv/src/asserv/Makefile @@ -0,0 +1,17 @@ +BASE = ../../../avr +PROGS = asserv +asserv_SOURCES = main.c +DOC = +EXTRACTDOC = +MODULES = proto uart utils +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 + +DEFS = +LIBS = + +include $(BASE)/make/Makefile.gen diff --git a/n/asserv/src/asserv/avrconfig.h b/n/asserv/src/asserv/avrconfig.h new file mode 100644 index 0000000..461818a --- /dev/null +++ b/n/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 0 +/** 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 DROP +/** 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 8 +/** 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/n/asserv/src/asserv/counter.c b/n/asserv/src/asserv/counter.c new file mode 100644 index 0000000..6ac2cea --- /dev/null +++ b/n/asserv/src/asserv/counter.c @@ -0,0 +1,173 @@ +/* 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; + PORTD |= 0x40; + } + else + { + counter_left_frw += c - counter_left_old; + PORTD &= ~0x40; + } + 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; + PORTD |= 0x20; + } + else + { + counter_right_frw += c - counter_right_old; + PORTD &= ~0x20; + } + 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.c b/n/asserv/src/asserv/eeprom.c new file mode 100644 index 0000000..44f6ec2 --- /dev/null +++ b/n/asserv/src/asserv/eeprom.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 0xa9 + +/* +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 new file mode 100644 index 0000000..2f9c77b --- /dev/null +++ b/n/asserv/src/asserv/main.c @@ -0,0 +1,260 @@ +/* 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 "io.h" + +/* This is implementation include. */ +#include "timer.c" +#include "counter.c" +#include "pwm.c" +#include "pos.c" +#include "eeprom.c" + +/** Motor control mode: + * 0: pwm setup. + * 1: shaft position control. + * 2: speed control. + * 3: trajectory control. */ +int8_t main_mode; + +/** Report of counters. */ +uint8_t main_stat_counter, main_stat_counter_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; + +/** Record timer value at different stage of computing. Used for performance + * analisys. */ +uint8_t main_timer_0, main_timer_1, main_timer_2, main_timer_3, main_timer_4; + +/* +AutoDec */ + +/** Main loop. */ +static void +main_loop (void); + +/* -AutoDec */ + +/** Entry point. */ +int +main (void) +{ + DDRD = 0x60; + /* Pull-ups. */ + PORTA = _BV (7) | _BV (1) | _BV (0); + eeprom_read_params (); + pwm_init (); + timer_init (); + counter_init (); + uart0_init (); + proto_send0 ('z'); + sei (); + while (1) + main_loop (); + return 0; +} + +/** Main loop. */ +static void +main_loop (void) +{ + timer_wait (); + /* Counter update. */ + counter_update (); + /* Postion control. */ + if (main_mode >= 1) + pos_update (); + /* Pwm setup. */ + pwm_update (); + /* Stats. */ + 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_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; + } + 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_send5b ('T', main_timer_4, main_timer_3, main_timer_2, + main_timer_1, main_timer_0); + main_stat_timer_cpt = main_stat_timer; + } + if (main_print_pin && !--main_print_pin_cpt) + { + proto_send1b ('I', PINA); + main_print_pin_cpt = main_print_pin; + } + /* Misc. */ + while (uart0_poll ()) + proto_accept (uart0_getc ()); +} + +/** 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): + 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]); + pwm_right = v8_to_v16 (args[2], args[3]); + break; + /* Stats. + * - b: interval between stats. */ + case c ('C', 1): + /* Counter stats. */ + main_stat_counter_cpt = main_stat_counter = 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; + default: + /* Params. */ + if (cmd == 'p') + { + switch (c (args[0], size)) + { + 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 ('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_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; + } + } + else + { + proto_send0 ('?'); + return; + } + break; + } + proto_send (cmd, size, args); +#undef c +} + diff --git a/n/asserv/src/asserv/pos.c b/n/asserv/src/asserv/pos.c new file mode 100644 index 0000000..0a1af23 --- /dev/null +++ b/n/asserv/src/asserv/pos.c @@ -0,0 +1,104 @@ +/* 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. */ +uint16_t pos_theta_cur, pos_alpha_cur; +/** Consign theta/alpha. */ +uint16_t pos_theta_cons, pos_alpha_cons; + +/** Error saturation. */ +int16_t pos_e_sat = 1023; +/** Integral saturation. */ +int16_t pos_int_sat = 8191; +/** P coefficients. */ +uint16_t pos_theta_kp = 0, pos_alpha_kp = 0; +/** I coefficients. */ +uint16_t pos_theta_ki = 0, pos_alpha_ki = 0; +/** D coefficients. */ +uint16_t pos_theta_kd = 0, pos_alpha_kd = 0; +/** Current integral values. */ +int16_t pos_theta_int, pos_alpha_int; +/** Last error values. */ +int16_t pos_theta_e_old, pos_alpha_e_old; + +/* +AutoDec */ +/* -AutoDec */ + +/** Compute a PID. */ +static inline int16_t +pos_compute_pid (int16_t e, int16_t *i, int16_t *e_old, + uint16_t kp, uint16_t ki, uint16_t kd) +{ + int16_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; +} + +/** Update PWM according to consign. */ +static void +pos_update (void) +{ + int16_t pid_theta, pid_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. */ + pid_theta = pos_compute_pid (pos_theta_cons - pos_theta_cur, + &pos_theta_int, &pos_theta_e_old, + pos_theta_kp, pos_theta_ki, pos_theta_kd); + pid_alpha = pos_compute_pid (pos_alpha_cons - pos_alpha_cur, + &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; +} diff --git a/n/asserv/src/asserv/pwm.c b/n/asserv/src/asserv/pwm.c new file mode 100644 index 0000000..4b5abc5 --- /dev/null +++ b/n/asserv/src/asserv/pwm.c @@ -0,0 +1,125 @@ +/* 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 0x1ff + +/** 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 = 0x1ff, OC1B & OC1C with positive logic. + f_IO without prescaler. + Fpwm = f_IO / (prescaler * (1 + TOP)) = 28912 Hz. */ + TCCR1A = + regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, + 0, 0, 1, 0, 1, 0, 1, 0); + 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/timer.c b/n/asserv/src/asserv/timer.c new file mode 100644 index 0000000..c065317 --- /dev/null +++ b/n/asserv/src/asserv/timer.c @@ -0,0 +1,57 @@ +/* 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) +{ + /* 64 prescaler. */ + TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00, + 0, 0, 0, 0, 0, 1, 0, 0); + /* Fov = F_io / (prescaler * (TOP + 1)) + * TOP = 0xff + * Tov = 1 / Fov = 1.111 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