summaryrefslogtreecommitdiff
path: root/digital/asserv/src
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-14 00:53:22 +0200
committerNicolas Schodet2008-04-14 00:53:22 +0200
commit737e4f3e487c8849aab3900625ee64edb02ec263 (patch)
tree5fdf4ed76271e0d8c85c1e917a97b329da699694 /digital/asserv/src
parent144cd0c5de0042b880f36cf0db3f01a6060dabd7 (diff)
* digital/asserv/src/asserv:
- added motor-power-avr support.
Diffstat (limited to 'digital/asserv/src')
-rw-r--r--digital/asserv/src/asserv/Makefile7
-rw-r--r--digital/asserv/src/asserv/pwm.avr.c220
-rw-r--r--digital/asserv/src/asserv/pwm.h6
-rw-r--r--digital/asserv/src/asserv/pwm_config.h52
-rw-r--r--digital/asserv/src/asserv/pwm_mp.avr.c87
-rw-r--r--digital/asserv/src/asserv/pwm_mp.avr.h34
-rw-r--r--digital/asserv/src/asserv/pwm_ocr.avr.c221
-rw-r--r--digital/asserv/src/asserv/pwm_ocr.avr.h37
8 files changed, 453 insertions, 211 deletions
diff --git a/digital/asserv/src/asserv/Makefile b/digital/asserv/src/asserv/Makefile
index 7701cd21..f3987bd7 100644
--- a/digital/asserv/src/asserv/Makefile
+++ b/digital/asserv/src/asserv/Makefile
@@ -2,12 +2,13 @@ BASE = ../../../avr
PROGS = asserv
AVR_PROGS = test_counter
HOST_PROGS = test_motor_model
-asserv_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c pos.c speed.c \
- postrack.c traj.c twi_proto.c eeprom.avr.c state.c \
+asserv_SOURCES = main.c timer.avr.c counter_ext.avr.c pwm.avr.c pwm_mp.avr.c \
+ pwm_ocr.avr.c pos.c speed.c postrack.c traj.c \
+ twi_proto.c eeprom.avr.c state.c \
simu.host.c motor_model.host.c models.host.c
test_counter_SOURCES = test_counter.c timer.avr.c
test_motor_model_SOURCES = test_motor_model.c motor_model.host.c models.host.c
-MODULES = proto uart utils math/fixed twi
+MODULES = proto uart utils math/fixed twi spi
test_motor_model_MODULES =
CONFIGFILE = avrconfig.h
# atmega8, atmega8535, atmega128...
diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c
index f2caa220..8d12cc8a 100644
--- a/digital/asserv/src/asserv/pwm.avr.c
+++ b/digital/asserv/src/asserv/pwm.avr.c
@@ -1,4 +1,4 @@
-/* pwm.avr.c */
+/* pwm.avr.c - Handle all PWM generators. */
/* asserv - Position & speed motor control on AVR. {{{
*
* Copyright (C) 2005 Nicolas Schodet
@@ -24,96 +24,21 @@
* }}} */
#include "common.h"
#include "pwm.h"
-
-#include "modules/utils/utils.h"
-#include "io.h"
-
-/** Assign PWM outputs. */
-#define PWM1 pwm_left
-#define PWM2 pwm_right
-#define PWM3 pwm_aux0
-#undef PWM4
-
-#define PWM_REVERSE_BIT(x) PWM_REVERSE_BIT_ (x)
-#define PWM_REVERSE_BIT_(x) PWM_REVERSE_BIT_ ## x
-#define PWM_REVERSE_BIT_pwm_left _BV (0)
-#define PWM_REVERSE_BIT_pwm_right _BV (1)
-#define PWM_REVERSE_BIT_pwm_aux0 _BV (2)
-
-#define PWM1_OCR OCR1B
-#define PWM1_OCR_BIT 6
-#define PWM1_DIR 4
-#define PWM2_OCR OCR1C
-#define PWM2_OCR_BIT 7
-#define PWM2_DIR 5
-#define PWM3_OCR OCR3B
-#define PWM3_OCR_BIT 4
-#define PWM3_DIR 2
-#define PWM4_OCR OCR3C
-#define PWM4_OCR_BIT 5
-#define PWM4_DIR 3
-
-/* Simplify conditionnal compilation. */
-#define PWM1or2 (defined (PWM1) || defined (PWM2))
-#define PWM3or4 (defined (PWM3) || defined (PWM4))
-#ifdef PWM1
-# define PWM1c(x) x
-#else
-# define PWM1c(x) 0
-#endif
-#ifdef PWM2
-# define PWM2c(x) x
-#else
-# define PWM2c(x) 0
-#endif
-#ifdef PWM3
-# define PWM3c(x) x
-#else
-# define PWM3c(x) 0
-#endif
-#ifdef PWM4
-# define PWM4c(x) x
-#else
-# define PWM4c(x) 0
-#endif
+#include "pwm_mp.avr.h"
+#include "pwm_ocr.avr.h"
/** PWM values, this is an error if absolute value is greater than the
* maximum. */
int16_t pwm_left, pwm_right, pwm_aux0;
/** PWM reverse directions. */
uint8_t pwm_reverse;
-/** PWM reverse direction, port B. */
-static uint8_t pwm_dir_reverse_b;
-/** PWM reverse direction, port E. */
-static uint8_t pwm_dir_reverse_e;
-/** Initialise PWM generator. */
+/** Initialise PWM generators. */
void
pwm_init (void)
{
- /* Fast PWM, TOP = 0x3ff, OCnB & OCnC with positive logic.
- f_IO without prescaler.
- Fpwm = f_IO / (prescaler * (1 + TOP)) = 14400 Hz. */
-#if PWM1or2
- 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 |= PWM1c (_BV (PWM1_OCR_BIT) | _BV (PWM1_DIR))
- | PWM2c (_BV (PWM2_OCR_BIT) | _BV (PWM2_DIR));
-#endif
-#if PWM3or4
- TCCR3A =
- regv (COM3A1, COM3A0, COM3B1, COM3B0, COM3C1, COM3C0, WGM31, WGM30,
- 0, 0, 1, 0, 1, 0, 1, 1);
- TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30,
- 0, 0, 0, 0, 1, 0, 0, 1);
- /* Enable pwm and direction outputs in DDRE. */
- DDRE |= PWM3c (_BV (PWM3_OCR_BIT) | _BV (PWM3_DIR))
- | PWM4c (_BV (PWM4_OCR_BIT) | _BV (PWM4_DIR));
-#endif
+ pwm_mp_init ();
+ pwm_ocr_init ();
}
/** Update the hardware PWM values. */
@@ -121,138 +46,17 @@ void
pwm_update (void)
{
/* Some assumption checks. */
- assert (!PWM1c (PWM1 < -PWM_MAX || PWM1 > PWM_MAX));
- assert (!PWM2c (PWM2 < -PWM_MAX || PWM2 > PWM_MAX));
- assert (!PWM3c (PWM3 < -PWM_MAX || PWM3 > PWM_MAX));
- assert (!PWM4c (PWM4 < -PWM_MAX || PWM4 > PWM_MAX));
-#if PWM1or2
- uint8_t dir_b;
- /* Sample port B. */
- dir_b = PORTB & ~(PWM1c (_BV (PWM1_DIR)) | PWM2c (_BV (PWM2_DIR)));
-# ifdef PWM1
- uint16_t pwm1;
- /* Set PWM1. */
- if (PWM1 == 0)
- {
- pwm1 = 0;
- }
- else if (PWM1 < 0)
- {
- pwm1 = -PWM1;
- }
- else
- {
- dir_b |= _BV (PWM1_DIR);
- pwm1 = PWM1;
- }
-# endif /* PWM1 */
-# ifdef PWM2
- uint16_t pwm2;
- /* Set PWM2. */
- if (PWM2 == 0)
- {
- pwm2 = 0;
- }
- else if (PWM2 < 0)
- {
- pwm2 = -PWM2;
- }
- else
- {
- dir_b |= _BV (PWM2_DIR);
- pwm2 = PWM2;
- }
-# endif /* PWM2 */
-#endif /* PWM1or2 */
-#if PWM3or4
- uint8_t dir_e;
- /* Sample port E. */
- dir_e = PORTE & ~(PWM3c (_BV (PWM3_DIR)) | PWM4c (_BV (PWM4_DIR)));
-# ifdef PWM3
- uint16_t pwm3;
- /* Set PWM3. */
- if (PWM3 == 0)
- {
- pwm3 = 0;
- }
- else if (PWM3 < 0)
- {
- pwm3 = -PWM3;
- }
- else
- {
- dir_e |= _BV (PWM3_DIR);
- pwm3 = PWM3;
- }
-# endif /* PWM3 */
-# ifdef PWM4
- uint16_t pwm4;
- /* Set PWM4. */
- if (PWM4 == 0)
- {
- pwm4 = 0;
- }
- else if (PWM4 < 0)
- {
- pwm4 = -PWM4;
- }
- else
- {
- dir_e |= _BV (PWM4_DIR);
- pwm4 = PWM4;
- }
-# endif /* PWM4 */
-#endif /* PWM3or4 */
- /* Setup registers. */
- /* Here, there could be a problem because OCRx are double buffered, not
- * PORTx! */
- /* 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. */
-#if PWM1or2
- dir_b ^= pwm_dir_reverse_b;
- PORTB = dir_b;
-# ifdef PWM1
- PWM1_OCR = pwm1;
-# endif
-# ifdef PWM2
- PWM2_OCR = pwm2;
-# endif
-#endif /* PWM1or2 */
-#if PWM3or4
- dir_e ^= pwm_dir_reverse_e;
- PORTE = dir_e;
-# ifdef PWM3
- PWM3_OCR = pwm3;
-# endif
-# ifdef PWM4
- PWM4_OCR = pwm4;
-# endif
-#endif /* PWM3or4 */
+ assert (pwm_left > -PWM_MAX && pwm_left < PWM_MAX);
+ assert (pwm_right > -PWM_MAX && pwm_right < PWM_MAX);
+ assert (pwm_aux0 > -PWM_MAX && pwm_aux0 < PWM_MAX);
+ pwm_mp_update ();
+ pwm_ocr_update ();
}
void
pwm_set_reverse (uint8_t reverse)
{
pwm_reverse = reverse;
- pwm_dir_reverse_b = 0;
- pwm_dir_reverse_e = 0;
-#ifdef PWM1
- if (reverse & PWM_REVERSE_BIT (PWM1))
- pwm_dir_reverse_b |= _BV (PWM1_DIR);
-#endif
-#ifdef PWM2
- if (reverse & PWM_REVERSE_BIT (PWM2))
- pwm_dir_reverse_b |= _BV (PWM2_DIR);
-#endif
-#ifdef PWM3
- if (reverse & PWM_REVERSE_BIT (PWM3))
- pwm_dir_reverse_e |= _BV (PWM3_DIR);
-#endif
-#ifdef PWM4
- if (reverse & PWM_REVERSE_BIT (PWM4))
- pwm_dir_reverse_e |= _BV (PWM4_DIR);
-#endif
+ pwm_ocr_set_reverse (reverse);
}
diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h
index 36b2bb5a..3d738538 100644
--- a/digital/asserv/src/asserv/pwm.h
+++ b/digital/asserv/src/asserv/pwm.h
@@ -31,6 +31,12 @@
extern int16_t pwm_left, pwm_right, pwm_aux0;
extern uint8_t pwm_reverse;
+#define PWM_REVERSE_BIT(x) PWM_REVERSE_BIT_ (x)
+#define PWM_REVERSE_BIT_(x) PWM_REVERSE_BIT_ ## x
+#define PWM_REVERSE_BIT_pwm_left _BV (0)
+#define PWM_REVERSE_BIT_pwm_right _BV (1)
+#define PWM_REVERSE_BIT_pwm_aux0 _BV (2)
+
void
pwm_init (void);
diff --git a/digital/asserv/src/asserv/pwm_config.h b/digital/asserv/src/asserv/pwm_config.h
new file mode 100644
index 00000000..48b99217
--- /dev/null
+++ b/digital/asserv/src/asserv/pwm_config.h
@@ -0,0 +1,52 @@
+#ifndef pwm_config_h
+#define pwm_config_h
+/* pwm_config.h - Helper for PWM configuration. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2008 Nicolas Schodet
+ *
+ * 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.
+ *
+ * }}} */
+
+/* Simplify conditionnal compilation. */
+#define PWM1or2 (defined (PWM1) || defined (PWM2))
+#define PWM3or4 (defined (PWM3) || defined (PWM4))
+#ifdef PWM1
+# define PWM1c(x) x
+#else
+# define PWM1c(x) 0
+#endif
+#ifdef PWM2
+# define PWM2c(x) x
+#else
+# define PWM2c(x) 0
+#endif
+#ifdef PWM3
+# define PWM3c(x) x
+#else
+# define PWM3c(x) 0
+#endif
+#ifdef PWM4
+# define PWM4c(x) x
+#else
+# define PWM4c(x) 0
+#endif
+
+#endif /* pwm_config_h */
diff --git a/digital/asserv/src/asserv/pwm_mp.avr.c b/digital/asserv/src/asserv/pwm_mp.avr.c
new file mode 100644
index 00000000..4bc8f3f4
--- /dev/null
+++ b/digital/asserv/src/asserv/pwm_mp.avr.c
@@ -0,0 +1,87 @@
+/* pwm_mp.avr.c - PWM to be used with motor-power-avr. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2008 Nicolas Schodet
+ *
+ * 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"
+#include "pwm_mp.avr.h"
+#include "pwm.h"
+
+#include "modules/spi/spi.h"
+#include "io.h"
+
+/** Assign PWM outputs. */
+#define PWM1 pwm_left
+#define PWM2 pwm_right
+#undef PWM3
+#undef PWM4
+
+#include "pwm_config.h"
+
+/** When true, start transmission. */
+uint8_t pwm_mp_go;
+
+/** Initialise communication with motor-power-avr. */
+void
+pwm_mp_init (void)
+{
+#if PWM1or2 || PWM3or4
+ spi_init (SPI_IT_DISABLE | SPI_ENABLE | SPI_MSB_FIRST | SPI_MASTER |
+ SPI_CPOL_FALLING | SPI_CPHA_SETUP | SPI_FOSC_DIV16);
+#endif
+}
+
+/** Update PWM values. */
+void
+pwm_mp_update (void)
+{
+#if PWM1or2 || PWM3or4
+ uint8_t v;
+ uint8_t cks;
+ if (PWM1c (PWM1) || PWM2c (PWM2) || PWM3c (PWM3) || PWM4c (PWM4))
+ pwm_mp_go = 1;
+ if (!pwm_mp_go)
+ return;
+#endif
+#if PWM1or2
+ /* Chip enable. */
+ PORTB &= ~_BV (0);
+ /* Convert to 12 bits. */
+ uint16_t pwm1 = PWM1c (PWM1) << 1;
+ uint16_t pwm2 = PWM2c (PWM2) << 1;
+ /* Send, computing checksum on the way. */
+ cks = 0x42;
+ v = ((pwm1 >> 4) & 0xf0) | ((pwm2 >> 8) & 0x0f);
+ spi_send (v);
+ cks ^= v;
+ v = pwm1;
+ spi_send (v);
+ cks ^= v;
+ v = pwm2;
+ spi_send (v);
+ cks ^= v;
+ spi_send (cks);
+ /* Chip disable. */
+ PORTB |= _BV (0);
+#endif
+}
+
diff --git a/digital/asserv/src/asserv/pwm_mp.avr.h b/digital/asserv/src/asserv/pwm_mp.avr.h
new file mode 100644
index 00000000..cf30578f
--- /dev/null
+++ b/digital/asserv/src/asserv/pwm_mp.avr.h
@@ -0,0 +1,34 @@
+#ifndef pwm_mp_avr_h
+#define pwm_mp_avr_h
+/* pwm_mp.avr.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2008 Nicolas Schodet
+ *
+ * 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
+pwm_mp_init (void);
+
+void
+pwm_mp_update (void);
+
+#endif /* pwm_mp_avr_h */
diff --git a/digital/asserv/src/asserv/pwm_ocr.avr.c b/digital/asserv/src/asserv/pwm_ocr.avr.c
new file mode 100644
index 00000000..a5a21340
--- /dev/null
+++ b/digital/asserv/src/asserv/pwm_ocr.avr.c
@@ -0,0 +1,221 @@
+/* pwm_ocr.avr.c - PWM using internal generator. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2008 Nicolas Schodet
+ *
+ * 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"
+#include "pwm_ocr.avr.h"
+#include "pwm.h"
+
+#include "modules/utils/utils.h"
+#include "io.h"
+
+/** Assign PWM outputs. */
+#undef PWM1
+#undef PWM2
+#define PWM3 pwm_aux0
+#undef PWM4
+
+#define PWM1_OCR OCR1B
+#define PWM1_OCR_BIT 6
+#define PWM1_DIR 4
+#define PWM2_OCR OCR1C
+#define PWM2_OCR_BIT 7
+#define PWM2_DIR 5
+#define PWM3_OCR OCR3B
+#define PWM3_OCR_BIT 4
+#define PWM3_DIR 2
+#define PWM4_OCR OCR3C
+#define PWM4_OCR_BIT 5
+#define PWM4_DIR 3
+
+#include "pwm_config.h"
+
+/** PWM reverse direction, port B. */
+static uint8_t pwm_ocr_dir_reverse_b;
+/** PWM reverse direction, port E. */
+static uint8_t pwm_ocr_dir_reverse_e;
+
+/** Initialise PWM generator. */
+void
+pwm_ocr_init (void)
+{
+ /* Fast PWM, TOP = 0x3ff, OCnB & OCnC with positive logic.
+ f_IO without prescaler.
+ Fpwm = f_IO / (prescaler * (1 + TOP)) = 14400 Hz. */
+#if PWM1or2
+ 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 |= PWM1c (_BV (PWM1_OCR_BIT) | _BV (PWM1_DIR))
+ | PWM2c (_BV (PWM2_OCR_BIT) | _BV (PWM2_DIR));
+#endif
+#if PWM3or4
+ TCCR3A =
+ regv (COM3A1, COM3A0, COM3B1, COM3B0, COM3C1, COM3C0, WGM31, WGM30,
+ 0, 0, 1, 0, 1, 0, 1, 1);
+ TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30,
+ 0, 0, 0, 0, 1, 0, 0, 1);
+ /* Enable pwm and direction outputs in DDRE. */
+ DDRE |= PWM3c (_BV (PWM3_OCR_BIT) | _BV (PWM3_DIR))
+ | PWM4c (_BV (PWM4_OCR_BIT) | _BV (PWM4_DIR));
+#endif
+}
+
+/** Update the hardware PWM values. */
+void
+pwm_ocr_update (void)
+{
+#if PWM1or2
+ uint8_t dir_b;
+ /* Sample port B. */
+ dir_b = PORTB & ~(PWM1c (_BV (PWM1_DIR)) | PWM2c (_BV (PWM2_DIR)));
+# ifdef PWM1
+ uint16_t pwm1;
+ /* Set PWM1. */
+ if (PWM1 == 0)
+ {
+ pwm1 = 0;
+ }
+ else if (PWM1 < 0)
+ {
+ pwm1 = -PWM1;
+ }
+ else
+ {
+ dir_b |= _BV (PWM1_DIR);
+ pwm1 = PWM1;
+ }
+# endif /* PWM1 */
+# ifdef PWM2
+ uint16_t pwm2;
+ /* Set PWM2. */
+ if (PWM2 == 0)
+ {
+ pwm2 = 0;
+ }
+ else if (PWM2 < 0)
+ {
+ pwm2 = -PWM2;
+ }
+ else
+ {
+ dir_b |= _BV (PWM2_DIR);
+ pwm2 = PWM2;
+ }
+# endif /* PWM2 */
+#endif /* PWM1or2 */
+#if PWM3or4
+ uint8_t dir_e;
+ /* Sample port E. */
+ dir_e = PORTE & ~(PWM3c (_BV (PWM3_DIR)) | PWM4c (_BV (PWM4_DIR)));
+# ifdef PWM3
+ uint16_t pwm3;
+ /* Set PWM3. */
+ if (PWM3 == 0)
+ {
+ pwm3 = 0;
+ }
+ else if (PWM3 < 0)
+ {
+ pwm3 = -PWM3;
+ }
+ else
+ {
+ dir_e |= _BV (PWM3_DIR);
+ pwm3 = PWM3;
+ }
+# endif /* PWM3 */
+# ifdef PWM4
+ uint16_t pwm4;
+ /* Set PWM4. */
+ if (PWM4 == 0)
+ {
+ pwm4 = 0;
+ }
+ else if (PWM4 < 0)
+ {
+ pwm4 = -PWM4;
+ }
+ else
+ {
+ dir_e |= _BV (PWM4_DIR);
+ pwm4 = PWM4;
+ }
+# endif /* PWM4 */
+#endif /* PWM3or4 */
+ /* Setup registers. */
+ /* Here, there could be a problem because OCRx are double buffered, not
+ * PORTx! */
+ /* 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. */
+#if PWM1or2
+ dir_b ^= pwm_ocr_dir_reverse_b;
+ PORTB = dir_b;
+# ifdef PWM1
+ PWM1_OCR = pwm1;
+# endif
+# ifdef PWM2
+ PWM2_OCR = pwm2;
+# endif
+#endif /* PWM1or2 */
+#if PWM3or4
+ dir_e ^= pwm_ocr_dir_reverse_e;
+ PORTE = dir_e;
+# ifdef PWM3
+ PWM3_OCR = pwm3;
+# endif
+# ifdef PWM4
+ PWM4_OCR = pwm4;
+# endif
+#endif /* PWM3or4 */
+}
+
+void
+pwm_ocr_set_reverse (uint8_t reverse)
+{
+ pwm_reverse = reverse;
+ pwm_ocr_dir_reverse_b = 0;
+ pwm_ocr_dir_reverse_e = 0;
+#ifdef PWM1
+ if (reverse & PWM_REVERSE_BIT (PWM1))
+ pwm_ocr_dir_reverse_b |= _BV (PWM1_DIR);
+#endif
+#ifdef PWM2
+ if (reverse & PWM_REVERSE_BIT (PWM2))
+ pwm_ocr_dir_reverse_b |= _BV (PWM2_DIR);
+#endif
+#ifdef PWM3
+ if (reverse & PWM_REVERSE_BIT (PWM3))
+ pwm_ocr_dir_reverse_e |= _BV (PWM3_DIR);
+#endif
+#ifdef PWM4
+ if (reverse & PWM_REVERSE_BIT (PWM4))
+ pwm_ocr_dir_reverse_e |= _BV (PWM4_DIR);
+#endif
+}
+
diff --git a/digital/asserv/src/asserv/pwm_ocr.avr.h b/digital/asserv/src/asserv/pwm_ocr.avr.h
new file mode 100644
index 00000000..cd013824
--- /dev/null
+++ b/digital/asserv/src/asserv/pwm_ocr.avr.h
@@ -0,0 +1,37 @@
+#ifndef pwm_ocr_avr_h
+#define pwm_ocr_avr_h
+/* pwm_ocr.avr.h */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2008 Nicolas Schodet
+ *
+ * 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
+pwm_ocr_init (void);
+
+void
+pwm_ocr_update (void);
+
+void
+pwm_ocr_set_reverse (uint8_t reverse);
+
+#endif /* pwm_ocr_avr_h */