summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv/src/asserv/pwm.avr.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv/src/asserv/pwm.avr.c')
-rw-r--r--digital/asserv/src/asserv/pwm.avr.c221
1 files changed, 178 insertions, 43 deletions
diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c
index 673606bd..f2caa220 100644
--- a/digital/asserv/src/asserv/pwm.avr.c
+++ b/digital/asserv/src/asserv/pwm.avr.c
@@ -28,96 +28,231 @@
#include "modules/utils/utils.h"
#include "io.h"
-/** Define the PWM output used for left motor. */
-#define PWM_LEFT_OCR OCR1B
-/** Define the PWM output used for right motor. */
-#define PWM_RIGHT_OCR OCR1C
-/** Define the direction output for left motor. */
-#define PWM_LEFT_DIR 4
-/** Define the direction output for right motor. */
-#define PWM_RIGHT_DIR 5
+/** 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
/** 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);
+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. */
void
pwm_init (void)
{
- /* Fast PWM, TOP = 0x3ff, OC1B & OC1C with positive logic.
+ /* 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 |= _BV (7) | _BV (6) | _BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR);
+ 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_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);
+ 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 = PORTB & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR));
- /* Set left PWM. */
- if (pwm_left == 0)
+ 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)
{
- left = 0;
+ pwm3 = 0;
}
- else if (pwm_left < 0)
+ else if (PWM3 < 0)
{
- left = -pwm_left;
+ pwm3 = -PWM3;
}
else
{
- dir |= _BV (PWM_LEFT_DIR);
- left = pwm_left;
+ dir_e |= _BV (PWM3_DIR);
+ pwm3 = PWM3;
}
- /* Set right PWM. */
- if (pwm_right == 0)
+# endif /* PWM3 */
+# ifdef PWM4
+ uint16_t pwm4;
+ /* Set PWM4. */
+ if (PWM4 == 0)
{
- right = 0;
+ pwm4 = 0;
}
- else if (pwm_right < 0)
+ else if (PWM4 < 0)
{
- right = -pwm_right;
+ pwm4 = -PWM4;
}
else
{
- dir |= _BV (PWM_RIGHT_DIR);
- right = pwm_right;
+ 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
- * PORTB! */
+ * 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. */
- dir ^= pwm_dir;
- PORTB = dir;
- PWM_LEFT_OCR = left;
- PWM_RIGHT_OCR = right;
+#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 */
}
void
-pwm_reverse (uint8_t left, uint8_t right)
+pwm_set_reverse (uint8_t reverse)
{
- pwm_dir = 0;
- if (left) pwm_dir |= _BV (PWM_LEFT_DIR);
- if (right) pwm_dir |= _BV (PWM_RIGHT_DIR);
+ 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
}