summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2008-03-10 23:38:30 +0100
committerNicolas Schodet2008-03-10 23:38:30 +0100
commit26b72c6065f73d1c50d4b9d5bcee82bed058148e (patch)
treec2558426fe2a030a1d8f9073648f9303f9c78709 /digital/asserv
parent828b2c296b3b70206ce6431f459fbfdb90a8a08d (diff)
* digital/asserv/src/asserv:
- added auxiliary motor state and PWM.
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c4
-rw-r--r--digital/asserv/src/asserv/main.c22
-rw-r--r--digital/asserv/src/asserv/pwm.avr.c221
-rw-r--r--digital/asserv/src/asserv/pwm.h6
-rw-r--r--digital/asserv/src/asserv/simu.host.c10
-rw-r--r--digital/asserv/src/asserv/state.h3
6 files changed, 206 insertions, 60 deletions
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index 05402154..6b54a02b 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -46,7 +46,7 @@ eeprom_read_params (void)
speed_alpha_max = eeprom_read_byte (p8++);
speed_theta_slow = eeprom_read_byte (p8++);
speed_alpha_slow = eeprom_read_byte (p8++);
- pwm_dir = eeprom_read_byte (p8++);
+ pwm_set_reverse (eeprom_read_byte (p8++));
p16 = (uint16_t *) p8;
postrack_set_footing (eeprom_read_word (p16++));
speed_theta_acc = eeprom_read_word (p16++);
@@ -73,7 +73,7 @@ eeprom_write_params (void)
eeprom_write_byte (p8++, speed_alpha_max);
eeprom_write_byte (p8++, speed_theta_slow);
eeprom_write_byte (p8++, speed_alpha_slow);
- eeprom_write_byte (p8++, pwm_dir);
+ eeprom_write_byte (p8++, pwm_reverse);
p16 = (uint16_t *) p8;
eeprom_write_word (p16++, postrack_footing);
eeprom_write_word (p16++, speed_theta_acc);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 4a4d2aee..e8f43c4a 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -142,7 +142,8 @@ main_loop (void)
&& state_main.sequence_ack != state_main.sequence_finish
&& !--main_sequence_ack_cpt)
{
- proto_send1b ('A', state_main.sequence_finish);
+ proto_send2b ('A', state_main.sequence_finish,
+ state_aux0.sequence_finish);
main_sequence_ack_cpt = main_sequence_ack;
}
if (main_stat_counter && !--main_stat_counter_cpt)
@@ -178,7 +179,7 @@ main_loop (void)
#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
- proto_send2w ('W', pwm_left, pwm_right);
+ proto_send3w ('W', pwm_left, pwm_right, pwm_aux0);
main_stat_pwm_cpt = main_stat_pwm;
}
if (main_stat_timer && !--main_stat_timer_cpt)
@@ -229,6 +230,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pwm_right = v8_to_v16 (args[2], args[3]);
UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX);
break;
+ case c ('w', 2):
+ /* Set auxiliary pwm.
+ * - w: pwm. */
+ pos_reset (); // TODO
+ state_aux0.mode = MODE_PWM;
+ pwm_aux0 = v8_to_v16 (args[0], args[1]);
+ UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX);
+ break;
case c ('c', 4):
/* Add to position consign.
* - w: theta consign offset.
@@ -405,11 +414,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('b', 3):
pos_blocked = v8_to_v16 (args[1], args[2]);
break;
- case c ('w', 3):
+ case c ('w', 2):
/* Set PWM direction.
- * - b: inverse left direction.
- * - b: inverse right direction. */
- pwm_reverse (args[1], args[2]);
+ * - b: bits: 0000[aux0][right][left]. */
+ pwm_set_reverse (args[1]);
break;
case c ('E', 2):
/* Write to eeprom.
@@ -430,7 +438,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send2w ('d', pos_theta_kd, pos_alpha_kd);
proto_send1w ('E', pos_e_sat);
proto_send1w ('I', pos_int_sat);
- proto_send1b ('w', pwm_dir);
+ proto_send1b ('w', pwm_reverse);
break;
default:
proto_send0 ('?');
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
}
diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h
index 2480f7eb..36b2bb5a 100644
--- a/digital/asserv/src/asserv/pwm.h
+++ b/digital/asserv/src/asserv/pwm.h
@@ -28,8 +28,8 @@
/** Define the absolute maximum PWM value. */
#define PWM_MAX 0x3ff
-extern int16_t pwm_left, pwm_right;
-extern uint8_t pwm_dir;
+extern int16_t pwm_left, pwm_right, pwm_aux0;
+extern uint8_t pwm_reverse;
void
pwm_init (void);
@@ -38,6 +38,6 @@ void
pwm_update (void);
void
-pwm_reverse (uint8_t left, uint8_t right);
+pwm_set_reverse (uint8_t reverse);
#endif /* pwm_h */
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index d572d24b..ca5f18dc 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -48,10 +48,9 @@ int16_t counter_left_diff, counter_right_diff;
/** 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;
+int16_t pwm_left, pwm_right, pwm_aux0;
+/** PWM reverse directions. */
+uint8_t pwm_reverse;
struct motor_t simu_left_model, simu_right_model;
@@ -199,7 +198,8 @@ eeprom_clear_params (void)
}
void
-pwm_reverse (uint8_t left, uint8_t right)
+pwm_set_reverse (uint8_t reverse)
{
+ pwm_reverse = reverse;
}
diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h
index 20000943..49ae1e60 100644
--- a/digital/asserv/src/asserv/state.h
+++ b/digital/asserv/src/asserv/state.h
@@ -74,6 +74,9 @@ struct state_t
/** Main motors state. */
struct state_t state_main;
+/** First auxiliary motor state. */
+struct state_t state_aux0;
+
/** Start a new command execution. */
static inline void
state_start (struct state_t *motor, uint8_t sequence)