From 2dbdc0c21a686d55247d893988081eaac8044112 Mon Sep 17 00:00:00 2001 From: Pierre Prot Date: Thu, 7 Feb 2008 00:08:33 +0100 Subject: Pas mal de modifs en général - ADC - cleanup - cleanup - cleanup - ça compile pas encore --- analog/motor-power-avr/src/Makefile | 16 +++ analog/motor-power-avr/src/avrconfig.h | 76 +++++++++++ analog/motor-power-avr/src/main.c | 219 ++++++++++++++++++++++++++++++++ analog/motor-power-avr/src/mp_pwm_LR_.c | 63 +++++++++ analog/motor-power-avr/src/mp_pwm_LR_.h | 49 +++++++ analog/motor-power-avr/src/mp_pwm_L_.c | 175 +++++++++++++++++++++++++ analog/motor-power-avr/src/mp_pwm_L_.h | 35 +++++ analog/motor-power-avr/src/pwm.avr.c | 125 ++++++++++++++++++ analog/motor-power-avr/src/timer.avr.c | 70 ++++++++++ 9 files changed, 828 insertions(+) create mode 100644 analog/motor-power-avr/src/Makefile create mode 100644 analog/motor-power-avr/src/avrconfig.h create mode 100644 analog/motor-power-avr/src/main.c create mode 100644 analog/motor-power-avr/src/mp_pwm_LR_.c create mode 100644 analog/motor-power-avr/src/mp_pwm_LR_.h create mode 100644 analog/motor-power-avr/src/mp_pwm_L_.c create mode 100644 analog/motor-power-avr/src/mp_pwm_L_.h create mode 100644 analog/motor-power-avr/src/pwm.avr.c create mode 100644 analog/motor-power-avr/src/timer.avr.c diff --git a/analog/motor-power-avr/src/Makefile b/analog/motor-power-avr/src/Makefile new file mode 100644 index 00000000..f11fc6e9 --- /dev/null +++ b/analog/motor-power-avr/src/Makefile @@ -0,0 +1,16 @@ +BASE = ../../../digital/avr +PROGS = mp +HOST_PROGS = test_mp +mp_SOURCES = main.c mp_pwm_L_.c mp_pwm_LR_.c adc.c +test_mp_SOURCES = test_mp.c +MODULES = proto uart utils math/fixed +test_mp_MODULES = +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega16 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 +HOST_LIBS = -lm + +include $(BASE)/make/Makefile.gen diff --git a/analog/motor-power-avr/src/avrconfig.h b/analog/motor-power-avr/src/avrconfig.h new file mode 100644 index 00000000..59a35393 --- /dev/null +++ b/analog/motor-power-avr/src/avrconfig.h @@ -0,0 +1,76 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* mp - Motor-Power board control on AVR. {{{ + * + * Copyright (C) 2008 Pierre Prot + * + * Robot APB Team + * Web: http://apbteam.org + * Email: team AT apbteam DOT org + * + * 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 POLLING +/** Recv mode, same as send mode. */ +#define AC_UART0_RECV_MODE POLLING +/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */ +#define AC_UART0_CHAR_SIZE 8 +/** Parity : ODD, EVEN, NONE. */ +#define AC_UART0_PARITY EVEN +/** Stop bits : 1, 2. */ +#define AC_UART0_STOP_BITS 1 +/** Send buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_SEND_BUFFER_SIZE 32 +/** Recv buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_RECV_BUFFER_SIZE 32 +/** If the send buffer is full when putc: + * - DROP: drop the new byte. + * - WAIT: wait until there is room in the send buffer. */ +#define AC_UART0_SEND_BUFFER_FULL WAIT +/** In HOST compilation: + * - STDIO: use stdin/out. + * - PTS: use pseudo terminal. */ +#define AC_UART0_HOST_DRIVER PTS + +/* proto - Protocol module. */ +/** Maximum argument size. */ +#define AC_PROTO_ARGS_MAX_SIZE 12 +/** Callback function name. */ +#define AC_PROTO_CALLBACK proto_callback +/** Putchar function name. */ +#define AC_PROTO_PUTC uart0_putc +/** Support for quote parameter. */ +#define AC_PROTO_QUOTE 1 + +/* mp */ + +#endif /* avrconfig_h */ diff --git a/analog/motor-power-avr/src/main.c b/analog/motor-power-avr/src/main.c new file mode 100644 index 00000000..466bff91 --- /dev/null +++ b/analog/motor-power-avr/src/main.c @@ -0,0 +1,219 @@ +/* main.c */ +/* mp - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * Robot APB Team/Efrei 2006. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "modules/uart/uart.h" +#include "modules/proto/proto.h" +#include "modules/utils/utils.h" +#include "modules/utils/byte.h" +#include "modules/math/fixed/fixed.h" +#include "io.h" +#include "mp_pwm_LR_.h" +#include "mp_pwm_L_.h" +//#include "mp_pwm_R_.h" + +/* This is implementation include. */ +#ifdef HOST +# include "simu.host.h" +#endif + + +#ifdef HOST +/** Simulation data. */ +uint8_t main_simu, main_simu_cpt; +#endif /* HOST */ + +/** Record timer value at different stage of computing. Used for performance + * analisys. */ +uint8_t main_timer[6]; + +// Left channel variables +int8_t cmd_L_; +// Right channel variables +int8_t cmd_R_; + +// Environnemental variables +uint8_t temperature, battery; + +// current limitation values +uint8_t curLim_soft; +uint8_t curLim_temp; +uint8_t curLim_bat; + +// environemental test settings +uint16_t envTest_cpt, envTest_period, envTest_autosend; + +// current limit stats +uint8_t curLim_stat_cpt, curLim_stat_period; + +/* +AutoDec */ + +/** Main loop. */ +static void +main_loop (void); + +/* -AutoDec */ + +/** Entry point. */ +int +main (int argc, char **argv) +{ + avr_init (argc, argv); + DDRD = 0x60; + /* Pull-ups. */ + PORTA = 0xff; + PORTC = 0xff; + init_timer_LR_ (); + init_curLim (); + //uart0_init (); + //postrack_init (); + envTest_period = 200; + proto_send0 ('z'); + sei (); + while (1) + main_loop (); + return 0; +} + +/** Main loop. */ +static void +main_loop (void) +{ + /* Uart */ + if (uart0_poll ()) + proto_accept (uart0_getc ()); + + /* Counter for launching environemental tests */ + if (!(envTest_cpt --)) { + envTest_cpt = envTest_period; + launch_envTest (); + curLim_temp = get_curLim_temp (temperature); + curLim_bat= get_curLim_bat (battery); + update_curLim (); + } +} + +/** Handle incoming messages. */ +void +proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) +{ +#define c(cmd, size) (cmd << 8 | size) + switch (c (cmd, size)) + { + case c ('z', 0): + /* Reset. */ + stop_motor_L_ (); + //stop_motor_R_ (); + utils_reset (); + break; + /* Commands. */ + /* + case c ('R', 0): + // Set high Z state for channel R + stop_motor_R_ (); + break; + */ + case c ('L', 0): + /* Set high Z state for channel L */ + stop_motor_L_ (); + break; + + case c ('l', 1): + /* Set pwm value for _L_ side. + * - 0x80: -max on duty cycle (direction = 0) + * - 0x00: 0% on duty cycle (brake state) + * - 0xFF: max on duty cylcle (direction = 1) */ + cmd_L_ = (int8_t) args[0]; + if (cmd_L_ >= 0) + { + start_motor_L_(cmd_L_ , 0); + } + else + { + start_motor_L_ (-cmd_L_ , 1); + } + break; + + //case c ('r', 1): + /* Set pwm value for _R_ side. + * - 0x80: -max on duty cycle (direction = 0) + * - 0x00: 0% on duty cycle (brake state) + * TODO : recopier ce qui est fait pour le cote _L_ + } + */ + //break; + + case c ('e', 0): + /* Get environnemental test results */ + //TODO + // envoyer surune seule ligne : + // - valeur batterie + // - valeur température + // - curLim_bat + // - curLim_temp + // - curLim_soft + // - curLim + break; + + case c ('e', 1): + /* Set environnemental test automatic sending + * 0 : no automatic send + * other : automatic send */ + envTest_autosend = args[0]; + break; + + case c ('E', 1): + /* Set environnemental test period */ + envTest_cpt = envTest_period = args[0]; + break; + + case c ('c', 0): + /* Get current limit stat */ + //TODO + // envoyer le nombre d'IT overcurrent qu'il y a eu + // depuis le dernier envoi de cette stat + break; + + case c ('c', 1): + /* Set current limit software value */ + //TODO + setCurLim_soft (args[0]); + break; + + case c ('C', 1): + /* Set current limit stats period + * Can be set to 0 for no automatic stat send */ + curLim_stat_cpt = curLim_stat_period = args[0]; + break; + /*#ifdef HOST + case c ('Y', 1): + // Simulation data. + main_simu_cpt = main_simu = args[0]; + break; +#endif // HOST */ +} +proto_send (cmd, size, args); +#undef c +} + diff --git a/analog/motor-power-avr/src/mp_pwm_LR_.c b/analog/motor-power-avr/src/mp_pwm_LR_.c new file mode 100644 index 00000000..3e12cf58 --- /dev/null +++ b/analog/motor-power-avr/src/mp_pwm_LR_.c @@ -0,0 +1,63 @@ + + +// This file contains general pwm routines for mp (motor-power) board +// +// state_L_ : x - Inhib - changeDir - Dir - x - x - x - x +// Timer_L_ : timer dedicated to Left side + +void init_timer_LR_(void) { + Timer_L_ = 0x00; + state_L_ = 0x40; + Timer_R_ = 0x80; + state_R_ = 0x40; + // TODO : set interrupts + // TODO : set OCR to 0 + START_TIMER_L_; + START_TIMER_R_; +} + +void init_curLim (void) { + // TODO : set interrupts +} + +uint8_t get_curLim_temp (uint8_t temperature) { + return (temperature - 40) >> 2; // TODO : ajuster la fonction de transfert +} + +uint8_t get_curLim_bat (uint8_t battery) { + return (battery - 40) >> 2; // TODO : ajuster la fonction de transfert +} + +// this function shall be called after each adjustment of any current limit +void update_curLim(void) { + uint8 curLim_tmp; + uint8 ret; + + if (curLim_soft > curLim_temp) then + curLim_tmp = curLim_temp + else + curLim_tmp = curLim_soft; + + if (curLim_tmp > curLim_bat) then + curLim_tmp = curLim_bat; + + OCR_CurLim_L_ = curLim_tmp; + OCR_CurLim_R_ = curLim_tmp; +} + +// this function shall be called every 2ms or more often +void launch_envTest(void) { + // TODO : acquerir les donnees de batterie + // TODO : acquerir les donnees de temperature + curLim_temp = 1; + curLim_bat = 2; + + update_current_limit(); +} + +// set the software-programmed current limit +void setCurLim_soft(uint8_t curLim) { + curLim_soft = curLim; + update_current_limit(); +} + diff --git a/analog/motor-power-avr/src/mp_pwm_LR_.h b/analog/motor-power-avr/src/mp_pwm_LR_.h new file mode 100644 index 00000000..3aaf3672 --- /dev/null +++ b/analog/motor-power-avr/src/mp_pwm_LR_.h @@ -0,0 +1,49 @@ +#ifndef mp_pwm_LR__h +#define mp_pwm_LR__h +/* mp_pwm_LR_.h */ +/* {{{ + * + * Copyright (C) 2008 Pierre Prot + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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" + +#define OCR_L_ OCR0 +#define OCR_R_ OCR2 + +#define Timer_L_ TOCNT +#define Timer_R_ T2CNT + +#define PWM_MIN_LR_ 0x10 +#define PWM_MAX_LR_ 0xF0 +#define PWM_OFFSET_LR_ 0x00 + +void init_timer_LR_(void); +void init_curLim (void); +uint8_t get_curLim_temp (uint8_t temperature); +uint8_t get_curLim_bat (uint8_t battery); +void update_curLim(void); +void launch_envTest(void); +void setCurLim_soft(uint8_t curLim); + + +#endif /* mp_pwm_LR__h */ diff --git a/analog/motor-power-avr/src/mp_pwm_L_.c b/analog/motor-power-avr/src/mp_pwm_L_.c new file mode 100644 index 00000000..b7ef3732 --- /dev/null +++ b/analog/motor-power-avr/src/mp_pwm_L_.c @@ -0,0 +1,175 @@ +#include "mp_pwm_LR_.h" +#include "mp_pwm_L_.h" + + + +// Le PC, afin de faire le saut calculé +#define PC PC_REG + +// Les sorties de contrôdu pont en H +// A/B : branche du pont +// H/L : High ou Low +// _L_ : pont Left +#define _L_AL // TODO ! à conpléter ! +#define _L_AH // TODO ! à conpléter ! +#define _L_BL // TODO ! à conpléter ! +#define _L_BH // TODO ! à conpléter ! + +// static variables +static uint8_t state_L_; +static uint8_t state_L_cmd = 0x03; +static uint8_t pwm_L_; + + +// this file contains routines for managing the _L_ channel of mp board +// the command sed -e 's/_L_/_R_/g' can be used for generating the _R_ file +// +// rising edge = timer overflow = TOV interrupt (TODO : à programmer) +void rise_L_ (void) { + // programs the state which is ordered by the core code + state_L_ = state_L_cmd; + + // the falling of the other side may have delayed a few our IT + OCR_L_ = pwm_L_ + Timer_L_; // TODO: OCR_L_value shall be > than x% + + //PC = PC + state_L_; // j'aurais bien aimé faire un calculated jump + + switch (state_L_) + { + case 0x00: + // dir 0 + //rise_L_label0: + _L_BH = 0; + _L_BL = 1; + _L_AL = 0; + _L_AH = 1; + sei(); // set back interrupts + return; + break; + + case 0x01: + // dir 1 + //org rise_L_label0 + 0x10 + _L_AH = 0; + _L_AL = 1; + _L_BL = 0; + _L_BH = 1; + sei(); // set back interrupts + return; + break; + + case 0x02: + // switch to forced low steady state + //org rise_L_label0 + 0x20 + _L_AH = 0; + _L_AL = 1; + _L_BH = 0; + _L_BL = 1; + sei(); // set back interrupts + return; + break; + + case 0x03: + // switch to high impedance steady state + //org rise_L_label0 + 0x30 + _L_AL = 0; + _L_AH = 0; + _L_BL = 0; + _L_BH = 0; + sei(); // set back interrupts + return; + break; + + } + +} + +// falling edge = timer crossing OCR : OCn interrupt (TODO : à programmer) +void fall_L_ (void) { + // PC = PC + state_L_; TODO :saut calculé ? + + switch (state_L_) + { + case 0x00: + // in the case we are in 0x00 direction + _L_AH = 0; + _L_AL = 1; + sei(); // set back interrupts + return; + break; + + case 0x01: + // in the case we are in 0x10 direction + //org fall_L_label0 + 0x10 + _L_BH = 0; + _L_BL = 1; + sei(); // set back interrupts + return; + break; + + case 0x02: + // forced low + //org fall_L_label0 + 0x20 + sei(); // set back interrupts + return; + break; + + case 0x03: + // left high Z + //org fall_L_label0 + 0x30 + sei(); // set back interrupts + return; + break; + } +} + + +// overcurrent detected by comparators +void ovc_L_ (void) { + _L_AL = 0; + _L_AH = 0; + _L_BL = 0; + _L_BH = 0; + sei(); // set back interrupts + // following line orders to keep high Z state when faling edge will arrive + state_L_ = 0x30; + return; +} + +// starts the motor +void start_motor_L_ (uint8_t pwmspeed, uint8_t direction) { + // checking direction + if (direction) + { + state_L_cmd = 0x10; + } + else + { + state_L_cmd = 0x00; + } + + // setting pwm value + if (pwmspeed == 0) + {// brake + state_L_cmd = 0x20; + pwm_L_ = 0; + } + else if (pwmspeed > PWM_MAX_LR_) + {// over PWM_MAX_LR_ + pwm_L_ = PWM_MAX_LR_ + PWM_OFFSET_LR_; + } + else if (pwmspeed < PWM_MIN_LR_) + {// under PWM_MIN_LR_ + pwm_L_ = PWM_MIN_LR_ + PWM_OFFSET_LR_; + } + else + {// correct PWM value + pwm_L_ = pwmspeed + PWM_OFFSET_LR_; + } +} + +// puts motor in high Z +void stop_motor_L_ (void) { + state_L_ = 0x30; +} + diff --git a/analog/motor-power-avr/src/mp_pwm_L_.h b/analog/motor-power-avr/src/mp_pwm_L_.h new file mode 100644 index 00000000..dba1c09c --- /dev/null +++ b/analog/motor-power-avr/src/mp_pwm_L_.h @@ -0,0 +1,35 @@ +#ifndef mp_pwm_L__h +#define mp_pwm_L__h +/* mp_pwm_L_.h */ +/* {{{ + * + * Copyright (C) 2008 Pierre Prot + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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. + * + * }}} */ + +void rise_L_ (void); +void fall_L_ (void); +void ovc_L_ (void); +void start_motor_L_ (uint8_t pwmspeed, uint8_t direction); +void stop_motor_L_ (void); + + +#endif /* mp_pwm_L__h */ diff --git a/analog/motor-power-avr/src/pwm.avr.c b/analog/motor-power-avr/src/pwm.avr.c new file mode 100644 index 00000000..773cfef0 --- /dev/null +++ b/analog/motor-power-avr/src/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/analog/motor-power-avr/src/timer.avr.c b/analog/motor-power-avr/src/timer.avr.c new file mode 100644 index 00000000..dcad74d0 --- /dev/null +++ b/analog/motor-power-avr/src/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; +} + -- cgit v1.2.3