summaryrefslogtreecommitdiff
path: root/n/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv')
-rw-r--r--n/asserv/src/main.c12
-rw-r--r--n/asserv/src/pwm.c66
-rw-r--r--n/asserv/src/taz.c3
-rw-r--r--n/asserv/src/test_pwm.c2
4 files changed, 49 insertions, 34 deletions
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index eec0699..1e77b7e 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -212,8 +212,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
reset ();
break;
/* Commands. */
- case c ('t', 0):
- motor_taz = 1;
+ case c ('t', 1):
+ motor_taz = args[0];
break;
case c ('l', 2):
/* Linear move.
@@ -368,6 +368,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Set maximum speed for automatic movements. */
speed_max = args[1];
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;
default:
proto_send0 ('?');
return;
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;
}
diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c
index 7c0d775..3980ca5 100644
--- a/n/asserv/src/taz.c
+++ b/n/asserv/src/taz.c
@@ -28,7 +28,8 @@ uint8_t taz_substate;
uint8_t taz_tempo;
/** Positions. */
//#define taz_scale (1.0/0.05026548245743669181)
-#define taz_scale (1.0/.05090591755319148936)
+//#define taz_scale (1.0/.05090591755319148936)
+#define taz_scale (1.0/.05132374757503138194)
static const uint16_t taz_rear_16 = taz_scale * 270 / 2;
static const uint32_t taz_rear_32 = taz_scale * 270 / 2 * 256;
static const uint16_t taz_front_16 = taz_scale * 270 / 2;
diff --git a/n/asserv/src/test_pwm.c b/n/asserv/src/test_pwm.c
index ef3b413..d92150c 100644
--- a/n/asserv/src/test_pwm.c
+++ b/n/asserv/src/test_pwm.c
@@ -30,6 +30,8 @@
/* +AutoDec */
/* -AutoDec */
+int8_t speed_left, speed_right;
+
#include "pwm.c"
void