summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/main.c12
-rw-r--r--digital/asserv/src/asserv/pos.c12
-rw-r--r--digital/asserv/src/asserv/pwm.avr.c15
-rw-r--r--digital/asserv/src/asserv/pwm.h37
-rw-r--r--digital/asserv/src/asserv/pwm_mp.avr.c17
-rw-r--r--digital/asserv/src/asserv/pwm_ocr.avr.c32
-rw-r--r--digital/asserv/src/asserv/simu.host.c21
-rw-r--r--digital/asserv/src/asserv/twi_proto.c4
8 files changed, 78 insertions, 72 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index db3064da..1ba1f956 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -193,7 +193,7 @@ main_loop (void)
#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
- proto_send3w ('W', pwm_left, pwm_right, pwm_aux0);
+ proto_send3w ('W', pwm_left.cur, pwm_right.cur, pwm_aux0.cur);
main_stat_pwm_cpt = main_stat_pwm;
}
if (main_stat_timer && !--main_stat_timer_cpt)
@@ -231,8 +231,8 @@ 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 = 0;
- pwm_right = 0;
+ pwm_set (&pwm_left, 0);
+ pwm_set (&pwm_right, 0);
break;
case c ('w', 4):
/* Set pwm.
@@ -241,15 +241,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_SET (pwm_left, v8_to_v16 (args[0], args[1]));
- PWM_SET (pwm_right, v8_to_v16 (args[2], args[3]));
+ 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_SET (pwm_aux0, v8_to_v16 (args[0], args[1]));
+ 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 af3d0315..e84a0905 100644
--- a/digital/asserv/src/asserv/pos.c
+++ b/digital/asserv/src/asserv/pos.c
@@ -133,16 +133,16 @@ pos_update (void)
pos_reset (&pos_theta);
pos_reset (&pos_alpha);
state_blocked (&state_main);
- pwm_left = 0;
- pwm_right = 0;
+ pwm_set (&pwm_left, 0);
+ pwm_set (&pwm_right, 0);
}
else
{
pid_theta = pos_compute_pid (diff_theta, &pos_theta);
pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha);
/* Update PWM. */
- PWM_SET (pwm_left, pid_theta - pid_alpha);
- PWM_SET (pwm_right, pid_theta + pid_alpha);
+ pwm_set (&pwm_left, pid_theta - pid_alpha);
+ pwm_set (&pwm_right, pid_theta + pid_alpha);
}
}
if (state_aux0.mode >= MODE_POS)
@@ -158,13 +158,13 @@ pos_update (void)
/* Blocked. */
pos_reset (&pos_aux0);
state_blocked (&state_aux0);
- pwm_aux0 = 0;
+ pwm_set (&pwm_aux0, 0);
}
else
{
pid = pos_compute_pid (diff, &pos_aux0);
/* Update PWM. */
- PWM_SET (pwm_aux0, pid);
+ pwm_set (&pwm_aux0, pid);
}
}
}
diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c
index 932e5a7f..6a5b0ff4 100644
--- a/digital/asserv/src/asserv/pwm.avr.c
+++ b/digital/asserv/src/asserv/pwm.avr.c
@@ -27,9 +27,10 @@
#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 control states. */
+struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left);
+struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right);
+struct pwm_t pwm_aux0 = PWM_INIT_FOR (pwm_aux0);
/** PWM reverse directions. */
uint8_t pwm_reverse;
@@ -45,17 +46,11 @@ pwm_init (void)
void
pwm_update (void)
{
- /* Some assumption checks. */
- 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 ();
}
+/** Set which PWM is reversed. */
void
pwm_set_reverse (uint8_t reverse)
{
diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h
index fd141df7..7877d4eb 100644
--- a/digital/asserv/src/asserv/pwm.h
+++ b/digital/asserv/src/asserv/pwm.h
@@ -30,7 +30,19 @@
* (rounding after shifting bug). */
#define PWM_MAX 0x3f0
-extern int16_t pwm_left, pwm_right, pwm_aux0;
+/** PWM control state. */
+struct pwm_t
+{
+ /** Current PWM value. */
+ int16_t cur;
+ /** Maximum value. */
+ int16_t max;
+ /** Minimum value (dead zone). */
+ int16_t min;
+};
+
+extern struct pwm_t pwm_left, pwm_right, pwm_aux0;
+
extern uint8_t pwm_reverse;
/** Define maximum PWM value for each output. */
@@ -55,12 +67,23 @@ extern uint8_t pwm_reverse;
#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)
+/** State init macro. */
+#define PWM_INIT_FOR(x) \
+ { 0, PWM_MAX_FOR (x), PWM_MIN_FOR (x) }
+
+/** Set PWM value. */
+static inline void
+pwm_set (struct pwm_t *pwm, int16_t value)
+{
+ if (value > pwm->max)
+ pwm->cur = pwm->max;
+ else if (value < -pwm->max)
+ pwm->cur = -pwm->max;
+ else if (value > -pwm->min && value < pwm->min)
+ pwm->cur = 0;
+ else
+ pwm->cur = value;
+}
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 0602c40a..47b50763 100644
--- a/digital/asserv/src/asserv/pwm_mp.avr.c
+++ b/digital/asserv/src/asserv/pwm_mp.avr.c
@@ -65,7 +65,7 @@ pwm_mp_init (void)
/** Send command using SPI. */
static void
-pwm_mp_send (int16_t pwm1, int16_t pwm2, uint16_t min1, uint16_t min2,
+pwm_mp_send (int16_t pwm1, int16_t pwm2,
uint8_t invert1, uint8_t invert2)
{
uint8_t v;
@@ -77,10 +77,6 @@ pwm_mp_send (int16_t pwm1, int16_t pwm2, uint16_t min1, uint16_t min2,
pwm1c = -pwm1c;
if (invert2)
pwm2c = -pwm2c;
- if (UTILS_ABS (pwm1c) < min1 * 2)
- pwm1c = 0;
- if (UTILS_ABS (pwm2c) < min2 * 2)
- pwm2c = 0;
/* Send, computing checksum on the way. */
cks = 0x42;
v = ((pwm1c >> 4) & 0xf0) | ((pwm2c >> 8) & 0x0f);
@@ -102,7 +98,8 @@ void
pwm_mp_update (void)
{
#if PWM1or2 || PWM3or4
- if (PWM1c (PWM1) || PWM2c (PWM2) || PWM3c (PWM3) || PWM4c (PWM4))
+ if (PWM1c (PWM1.cur) || PWM2c (PWM2.cur)
+ || PWM3c (PWM3.cur) || PWM4c (PWM4.cur))
pwm_mp_go = 1;
if (!pwm_mp_go)
return;
@@ -110,9 +107,7 @@ pwm_mp_update (void)
#if PWM1or2
/* Chip enable. */
PORTB &= ~_BV (0);
- pwm_mp_send (PWM1c (PWM1), PWM2c (PWM2),
- PWM1c (PWM_MIN_FOR (PWM1)),
- PWM2c (PWM_MIN_FOR (PWM2)),
+ pwm_mp_send (PWM1c (PWM1.cur), PWM2c (PWM2.cur),
PWM1c (pwm_reverse & PWM_REVERSE_BIT (PWM1)),
PWM2c (pwm_reverse & PWM_REVERSE_BIT (PWM2)));
/* Chip disable. */
@@ -121,9 +116,7 @@ pwm_mp_update (void)
#if PWM3or4
/* Chip enable. */
PORTE &= ~_BV (4);
- pwm_mp_send (PWM3c (PWM3), PWM4c (PWM4),
- PWM3c (PWM_MIN_FOR (PWM3)),
- PWM4c (PWM_MIN_FOR (PWM4)),
+ pwm_mp_send (PWM3c (PWM3.cur), PWM4c (PWM4.cur),
PWM3c (pwm_reverse & PWM_REVERSE_BIT (PWM3)),
PWM4c (pwm_reverse & PWM_REVERSE_BIT (PWM4)));
/* Chip disable. */
diff --git a/digital/asserv/src/asserv/pwm_ocr.avr.c b/digital/asserv/src/asserv/pwm_ocr.avr.c
index 1c0dfce0..75857f10 100644
--- a/digital/asserv/src/asserv/pwm_ocr.avr.c
+++ b/digital/asserv/src/asserv/pwm_ocr.avr.c
@@ -95,35 +95,35 @@ pwm_ocr_update (void)
# ifdef PWM1
uint16_t pwm1;
/* Set PWM1. */
- if (PWM1 == 0)
+ if (PWM1.cur == 0)
{
pwm1 = 0;
}
- else if (PWM1 < 0)
+ else if (PWM1.cur < 0)
{
- pwm1 = -PWM1;
+ pwm1 = -PWM1.cur;
}
else
{
dir_b |= _BV (PWM1_DIR);
- pwm1 = PWM1;
+ pwm1 = PWM1.cur;
}
# endif /* PWM1 */
# ifdef PWM2
uint16_t pwm2;
/* Set PWM2. */
- if (PWM2 == 0)
+ if (PWM2.cur == 0)
{
pwm2 = 0;
}
- else if (PWM2 < 0)
+ else if (PWM2.cur < 0)
{
- pwm2 = -PWM2;
+ pwm2 = -PWM2.cur;
}
else
{
dir_b |= _BV (PWM2_DIR);
- pwm2 = PWM2;
+ pwm2 = PWM2.cur;
}
# endif /* PWM2 */
#endif /* PWM1or2 */
@@ -134,35 +134,35 @@ pwm_ocr_update (void)
# ifdef PWM3
uint16_t pwm3;
/* Set PWM3. */
- if (PWM3 == 0)
+ if (PWM3.cur == 0)
{
pwm3 = 0;
}
- else if (PWM3 < 0)
+ else if (PWM3.cur < 0)
{
- pwm3 = -PWM3;
+ pwm3 = -PWM3.cur;
}
else
{
dir_e |= _BV (PWM3_DIR);
- pwm3 = PWM3;
+ pwm3 = PWM3.cur;
}
# endif /* PWM3 */
# ifdef PWM4
uint16_t pwm4;
/* Set PWM4. */
- if (PWM4 == 0)
+ if (PWM4.cur == 0)
{
pwm4 = 0;
}
- else if (PWM4 < 0)
+ else if (PWM4.cur < 0)
{
- pwm4 = -PWM4;
+ pwm4 = -PWM4.cur;
}
else
{
dir_e |= _BV (PWM4_DIR);
- pwm4 = PWM4;
+ pwm4 = PWM4.cur;
}
# endif /* PWM4 */
#endif /* PWM3or4 */
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index eec42f61..d496b61e 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -55,9 +55,10 @@ static int32_t counter_right_raw;
/** Correction factor (f8.24). */
uint32_t counter_right_correction = 1L << 24;
-/** PWM values, this is an error if absolute value is greater than the
- * maximum. */
-int16_t pwm_left, pwm_right, pwm_aux0;
+/** PWM control states. */
+struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left);
+struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right);
+struct pwm_t pwm_aux0 = PWM_INIT_FOR (pwm_aux0);
/** PWM reverse directions. */
uint8_t pwm_reverse;
@@ -168,18 +169,12 @@ simu_step (void)
{
double old_left_th, old_right_th, old_aux0_th;
/* Convert pwm value into voltage. */
- 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));
+ * ((double) pwm_left.cur / (PWM_MAX + 1));
simu_right_model.u = simu_right_model.m.u_max
- * ((double) pwm_right / (PWM_MAX + 1));
+ * ((double) pwm_right.cur / (PWM_MAX + 1));
simu_aux0_model.u = simu_aux0_model.m.u_max
- * ((double) pwm_aux0 / (PWM_MAX + 1));
+ * ((double) pwm_aux0.cur / (PWM_MAX + 1));
/* Make one step. */
old_left_th = simu_left_model.th;
old_right_th = simu_right_model.th;
@@ -259,7 +254,7 @@ simu_send (void)
mex_node_send (m);
/* Send PWM. */
m = mex_msg_new (0xa1);
- mex_msg_push (m, "hhh", pwm_left, pwm_right, pwm_aux0);
+ mex_msg_push (m, "hhh", pwm_left.cur, pwm_right.cur, pwm_aux0.cur);
mex_node_send (m);
/* Send Arm position. */
m = mex_msg_new (0xa8);
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 745a1a70..c235a626 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -109,8 +109,8 @@ twi_proto_callback (u8 *buf, u8 size)
pos_reset (&pos_theta);
pos_reset (&pos_alpha);
state_main.mode = MODE_PWM;
- pwm_left = 0;
- pwm_right = 0;
+ pwm_set (&pwm_left, 0);
+ pwm_set (&pwm_right, 0);
break;
case c ('s', 0):
/* Stop (set zero speed). */