summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/main.c9
-rw-r--r--digital/asserv/src/asserv/pos.c10
-rw-r--r--digital/asserv/src/asserv/pwm.avr.c9
-rw-r--r--digital/asserv/src/asserv/pwm.h23
-rw-r--r--digital/asserv/src/asserv/pwm_mp.avr.c11
-rw-r--r--digital/asserv/src/asserv/simu.host.c9
6 files changed, 49 insertions, 22 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 19fe2056..9516fd43 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -239,18 +239,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pos_reset (&pos_theta);
pos_reset (&pos_alpha);
state_main.mode = MODE_PWM;
- pwm_left = v8_to_v16 (args[0], args[1]);
- UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX);
- pwm_right = v8_to_v16 (args[2], args[3]);
- UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX);
+ PWM_SET (pwm_left, v8_to_v16 (args[0], args[1]));
+ PWM_SET (pwm_right, v8_to_v16 (args[2], args[3]));
break;
case c ('w', 2):
/* Set auxiliary pwm.
* - w: pwm. */
pos_reset (&pos_aux0);
state_aux0.mode = MODE_PWM;
- pwm_aux0 = v8_to_v16 (args[0], args[1]);
- UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX);
+ PWM_SET (pwm_aux0, v8_to_v16 (args[0], args[1]));
break;
case c ('c', 4):
/* Add to position consign.
diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
index d47694c2..9109e165 100644
--- a/digital/asserv/src/asserv/pos.c
+++ b/digital/asserv/src/asserv/pos.c
@@ -123,10 +123,8 @@ pos_update (void)
pid_theta = pos_compute_pid (diff_theta, &pos_theta);
pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha);
/* Update PWM. */
- pwm_left = pid_theta - pid_alpha;
- UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX);
- pwm_right = pid_theta + pid_alpha;
- UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX);
+ PWM_SET (pwm_left, pid_theta - pid_alpha);
+ PWM_SET (pwm_right, pid_theta + pid_alpha);
}
}
if (state_aux0.mode >= MODE_POS)
@@ -148,9 +146,7 @@ pos_update (void)
{
pid = pos_compute_pid (diff, &pos_aux0);
/* Update PWM. */
- pwm_aux0 = pid;
- /* WARNING: crude way to limit PWM for this 12V motor. */
- UTILS_BOUND (pwm_aux0, -(PWM_MAX / 2), (PWM_MAX / 2));
+ PWM_SET (pwm_aux0, pid);
}
}
}
diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c
index 8d12cc8a..932e5a7f 100644
--- a/digital/asserv/src/asserv/pwm.avr.c
+++ b/digital/asserv/src/asserv/pwm.avr.c
@@ -46,9 +46,12 @@ void
pwm_update (void)
{
/* Some assumption checks. */
- 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);
+ assert (pwm_left >= -PWM_MAX_FOR (pwm_left)
+ && pwm_left <= PWM_MAX_FOR (pwm_left));
+ assert (pwm_right >= -PWM_MAX_FOR (pwm_right)
+ && pwm_right <= PWM_MAX_FOR (pwm_right));
+ assert (pwm_aux0 >= -PWM_MAX_FOR (pwm_aux0)
+ && pwm_aux0 <= PWM_MAX_FOR (pwm_aux0));
pwm_mp_update ();
pwm_ocr_update ();
}
diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h
index d980a6a6..243ea213 100644
--- a/digital/asserv/src/asserv/pwm.h
+++ b/digital/asserv/src/asserv/pwm.h
@@ -33,12 +33,35 @@
extern int16_t pwm_left, pwm_right, pwm_aux0;
extern uint8_t pwm_reverse;
+/** Define maximum PWM value for each output. */
+#define PWM_MAX_FOR(x) PWM_MAX_FOR_ (x)
+#define PWM_MAX_FOR_(x) PWM_MAX_FOR_ ## x
+#define PWM_MAX_FOR_pwm_left PWM_MAX
+#define PWM_MAX_FOR_pwm_right PWM_MAX
+#define PWM_MAX_FOR_pwm_aux0 (PWM_MAX / 2)
+
+/** Define minimum PWM value for each output, if the value is less than the
+ * minimum, use 0. */
+#define PWM_MIN_FOR(x) PWM_MIN_FOR_ (x)
+#define PWM_MIN_FOR_(x) PWM_MIN_FOR_ ## x
+#define PWM_MIN_FOR_pwm_left 0x8
+#define PWM_MIN_FOR_pwm_right 0x8
+#define PWM_MIN_FOR_pwm_aux0 0x20
+
+/** Define which bit controls the PWM inversion. */
#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)
+/** Set pwm value and saturate. */
+#define PWM_SET(pwm, value) \
+ do { \
+ (pwm) = (value); \
+ UTILS_BOUND ((pwm), -PWM_MAX_FOR (pwm), PWM_MAX_FOR (pwm)); \
+ } while (0)
+
void
pwm_init (void);
diff --git a/digital/asserv/src/asserv/pwm_mp.avr.c b/digital/asserv/src/asserv/pwm_mp.avr.c
index 4ad7bd4a..0602c40a 100644
--- a/digital/asserv/src/asserv/pwm_mp.avr.c
+++ b/digital/asserv/src/asserv/pwm_mp.avr.c
@@ -65,7 +65,8 @@ pwm_mp_init (void)
/** Send command using SPI. */
static void
-pwm_mp_send (int16_t pwm1, int16_t pwm2, uint8_t invert1, uint8_t invert2)
+pwm_mp_send (int16_t pwm1, int16_t pwm2, uint16_t min1, uint16_t min2,
+ uint8_t invert1, uint8_t invert2)
{
uint8_t v;
uint8_t cks;
@@ -76,9 +77,9 @@ pwm_mp_send (int16_t pwm1, int16_t pwm2, uint8_t invert1, uint8_t invert2)
pwm1c = -pwm1c;
if (invert2)
pwm2c = -pwm2c;
- if (UTILS_ABS (pwm1c) < 0x10)
+ if (UTILS_ABS (pwm1c) < min1 * 2)
pwm1c = 0;
- if (UTILS_ABS (pwm2c) < 0x10)
+ if (UTILS_ABS (pwm2c) < min2 * 2)
pwm2c = 0;
/* Send, computing checksum on the way. */
cks = 0x42;
@@ -110,6 +111,8 @@ pwm_mp_update (void)
/* Chip enable. */
PORTB &= ~_BV (0);
pwm_mp_send (PWM1c (PWM1), PWM2c (PWM2),
+ PWM1c (PWM_MIN_FOR (PWM1)),
+ PWM2c (PWM_MIN_FOR (PWM2)),
PWM1c (pwm_reverse & PWM_REVERSE_BIT (PWM1)),
PWM2c (pwm_reverse & PWM_REVERSE_BIT (PWM2)));
/* Chip disable. */
@@ -119,6 +122,8 @@ pwm_mp_update (void)
/* Chip enable. */
PORTE &= ~_BV (4);
pwm_mp_send (PWM3c (PWM3), PWM4c (PWM4),
+ PWM3c (PWM_MIN_FOR (PWM3)),
+ PWM4c (PWM_MIN_FOR (PWM4)),
PWM3c (pwm_reverse & PWM_REVERSE_BIT (PWM3)),
PWM4c (pwm_reverse & PWM_REVERSE_BIT (PWM4)));
/* Chip disable. */
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index 64fc38db..795ee6a7 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -167,9 +167,12 @@ simu_step (void)
{
double old_left_th, old_right_th, old_aux0_th;
/* Convert pwm value into voltage. */
- 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);
+ assert (pwm_left >= -PWM_MAX_FOR (pwm_left)
+ && pwm_left <= PWM_MAX_FOR (pwm_left));
+ assert (pwm_right >= -PWM_MAX_FOR (pwm_right)
+ && pwm_right <= PWM_MAX_FOR (pwm_right));
+ assert (pwm_aux0 >= -PWM_MAX_FOR (pwm_aux0)
+ && pwm_aux0 <= PWM_MAX_FOR (pwm_aux0));
simu_left_model.u = simu_left_model.m.u_max
* ((double) pwm_left / (PWM_MAX + 1));
simu_right_model.u = simu_right_model.m.u_max