summaryrefslogtreecommitdiff
path: root/n/asserv/src/pwm.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/pwm.c')
-rw-r--r--n/asserv/src/pwm.c66
1 files changed, 35 insertions, 31 deletions
diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c
index 9452077..093e064 100644
--- a/n/asserv/src/pwm.c
+++ b/n/asserv/src/pwm.c
@@ -31,13 +31,11 @@
#define PWM_LEFT_DIR 3
/** Define the direction output for right motor. */
#define PWM_RIGHT_DIR 2
-/** Define the forward direction logic for left motor. */
-#define PWM_LEFT_DIR_FRW 0
-/** Define the forward direction logic for right motor. */
-#define PWM_RIGHT_DIR_FRW 1
/** PWM values. */
int16_t pwm_left, pwm_right;
+/** PWM reverse direction. */
+uint8_t pwm_dir = _BV (PWM_LEFT_DIR);
/* +AutoDec */
@@ -68,7 +66,7 @@ pwm_init (void)
TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10,
0, 0, 0, 0, 0, 0, 0, 1);
/* Enable pwm and direction outputs in DDRB. */
- DDRB |= _BV (7) | _BV (6) | _BV (3) | _BV (2);
+ DDRB |= _BV (7) | _BV (6) | _BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR);
}
/** Preprocess a PWM value before sending it to hardware. */
@@ -83,55 +81,61 @@ pwm_preproc (uint16_t v)
return v;
}
+extern int8_t speed_left, speed_right;
+
+/** Filter PWM values. */
+static inline void
+pwm_filter (void)
+{
+ /* This is a try to correct vibration problem. */
+ if ((speed_left > 0 && pwm_left < 0)
+ || (speed_left < 0 && pwm_left > 0))
+ pwm_left = 0;
+ if ((speed_right > 0 && pwm_right < 0)
+ || (speed_right < 0 && pwm_right > 0))
+ pwm_right = 0;
+}
+
/** Update the hardware PWM values. */
static inline void
pwm_update (void)
{
+ uint16_t left, right;
+ uint8_t dir;
+ pwm_filter ();
+ dir = PORTB & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR));
/* Set left PWM. */
if (pwm_left == 0)
{
- PWM_LEFT_OCR = 0;
+ left = 0;
}
else if (pwm_left < 0)
{
-#if PWM_LEFT_DIR_FRW == 1
- PORTB &= ~_BV (PWM_LEFT_DIR);
-#else
- PORTB |= _BV (PWM_LEFT_DIR);
-#endif
- PWM_LEFT_OCR = pwm_preproc (-pwm_left);
+ left = pwm_preproc (-pwm_left);
}
else
{
-#if PWM_LEFT_DIR_FRW == 1
- PORTB |= _BV (PWM_LEFT_DIR);
-#else
- PORTB &= ~_BV (PWM_LEFT_DIR);
-#endif
- PWM_LEFT_OCR = pwm_preproc (pwm_left);
+ dir |= _BV (PWM_LEFT_DIR);
+ left = pwm_preproc (pwm_left);
}
/* Set right PWM. */
if (pwm_right == 0)
{
- PWM_RIGHT_OCR = 0;
+ right = 0;
}
else if (pwm_right < 0)
{
-#if PWM_RIGHT_DIR_FRW == 1
- PORTB &= ~_BV (PWM_RIGHT_DIR);
-#else
- PORTB |= _BV (PWM_RIGHT_DIR);
-#endif
- PWM_RIGHT_OCR = pwm_preproc (-pwm_right);
+ right = pwm_preproc (-pwm_right);
}
else
{
-#if PWM_RIGHT_DIR_FRW == 1
- PORTB |= _BV (PWM_RIGHT_DIR);
-#else
- PORTB &= ~_BV (PWM_RIGHT_DIR);
-#endif
- PWM_RIGHT_OCR = pwm_preproc (pwm_right);
+ dir |= _BV (PWM_RIGHT_DIR);
+ right = pwm_preproc (pwm_right);
}
+ /* Setup registers. */
+ dir ^= pwm_dir;
+ PORTB = dir;
+ PWM_LEFT_OCR = left;
+ PWM_RIGHT_OCR = right;
}