summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2009-04-04 16:22:53 +0200
committerNicolas Schodet2009-04-04 16:22:53 +0200
commit1431438a2b0f3fafbafb61a4a15296164dadf5e1 (patch)
tree5d62fd3881d20756988bcf1d7af69dc53b984319 /digital/asserv
parentbcfaa95e57d29918e4beb19f4f726d0336889ebd (diff)
* digital/asserv:
- added second auxiliary motor support.
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/aux.c24
-rw-r--r--digital/asserv/src/asserv/aux.h2
-rw-r--r--digital/asserv/src/asserv/avrconfig.h2
-rw-r--r--digital/asserv/src/asserv/counter.h6
-rw-r--r--digital/asserv/src/asserv/counter_ext.avr.c35
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c36
-rw-r--r--digital/asserv/src/asserv/eeprom.h2
-rw-r--r--digital/asserv/src/asserv/init14
-rw-r--r--digital/asserv/src/asserv/main.c254
-rw-r--r--digital/asserv/src/asserv/models.host.c31
-rw-r--r--digital/asserv/src/asserv/models.host.h10
-rw-r--r--digital/asserv/src/asserv/move_bot_blue1
-rw-r--r--digital/asserv/src/asserv/move_bot_red1
-rw-r--r--digital/asserv/src/asserv/pos.c7
-rw-r--r--digital/asserv/src/asserv/pos.h2
-rw-r--r--digital/asserv/src/asserv/pwm.avr.c4
-rw-r--r--digital/asserv/src/asserv/pwm.h13
-rw-r--r--digital/asserv/src/asserv/pwm_mp.avr.c10
-rw-r--r--digital/asserv/src/asserv/pwm_ocr.avr.c32
-rw-r--r--digital/asserv/src/asserv/simu.host.c73
-rw-r--r--digital/asserv/src/asserv/speed.c10
-rw-r--r--digital/asserv/src/asserv/speed.h2
-rw-r--r--digital/asserv/src/asserv/state.c2
-rw-r--r--digital/asserv/src/asserv/state.h4
-rw-r--r--digital/asserv/src/asserv/test_counter.c3
-rw-r--r--digital/asserv/src/asserv/twi_proto.c16
-rw-r--r--digital/asserv/tools/asserv/asserv.py80
-rw-r--r--digital/asserv/tools/asserv/mex.py6
28 files changed, 413 insertions, 269 deletions
diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c
index 05ada060..f8665fbb 100644
--- a/digital/asserv/src/asserv/aux.c
+++ b/digital/asserv/src/asserv/aux.c
@@ -38,8 +38,8 @@
# include "simu.host.h"
#endif
-/** Motor state. */
-struct aux_t aux0;
+/** Motors states. */
+struct aux_t aux[AC_ASSERV_AUX_NB];
/** Trajectory modes. */
enum
@@ -62,18 +62,24 @@ enum
void
aux_init (void)
{
- aux0.state = &state_aux0;
- aux0.speed = &speed_aux0;
- aux0.zero_pin = &PINC;
- aux0.zero_bv = _BV (5);
+ aux[0].state = &state_aux[0];
+ aux[0].speed = &speed_aux[0];
+ aux[0].zero_pin = &PINC;
+ aux[0].zero_bv = _BV (5);
+ aux[1].state = &state_aux[1];
+ aux[1].speed = &speed_aux[1];
+ aux[1].zero_pin = &PINC;
+ aux[1].zero_bv = _BV (6);
}
/** Update positions. */
void
aux_pos_update (void)
{
+ uint8_t i;
/* Easy... */
- aux0.pos += counter_aux0_diff;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ aux[i].pos += counter_aux_diff[i];
}
/** Goto position. */
@@ -190,6 +196,8 @@ aux_traj_update_single (struct aux_t *aux)
void
aux_traj_update (void)
{
- aux_traj_update_single (&aux0);
+ uint8_t i;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ aux_traj_update_single (&aux[i]);
}
diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h
index 9460e87f..5a95bdaf 100644
--- a/digital/asserv/src/asserv/aux.h
+++ b/digital/asserv/src/asserv/aux.h
@@ -46,7 +46,7 @@ struct aux_t
uint8_t zero_bv;
};
-extern struct aux_t aux0;
+extern struct aux_t aux[AC_ASSERV_AUX_NB];
void
aux_init (void);
diff --git a/digital/asserv/src/asserv/avrconfig.h b/digital/asserv/src/asserv/avrconfig.h
index b8451a09..f95f74f8 100644
--- a/digital/asserv/src/asserv/avrconfig.h
+++ b/digital/asserv/src/asserv/avrconfig.h
@@ -84,6 +84,8 @@
#define AC_PROTO_QUOTE 1
/* asserv. */
+/** Number of auxiliary motors. */
+#define AC_ASSERV_AUX_NB 2
/** Use external counters. */
#define AC_ASSERV_COUNTER_EXTERNAL 1
/** TWI address. */
diff --git a/digital/asserv/src/asserv/counter.h b/digital/asserv/src/asserv/counter.h
index c5eae18b..45b7392f 100644
--- a/digital/asserv/src/asserv/counter.h
+++ b/digital/asserv/src/asserv/counter.h
@@ -25,9 +25,11 @@
*
* }}} */
-extern uint16_t counter_left, counter_right, counter_aux0;
+extern uint16_t counter_left, counter_right,
+ counter_aux[AC_ASSERV_AUX_NB];
extern uint32_t counter_right_correction;
-extern int16_t counter_left_diff, counter_right_diff, counter_aux0_diff;
+extern int16_t counter_left_diff, counter_right_diff,
+ counter_aux_diff[AC_ASSERV_AUX_NB];
void
counter_init (void);
diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c
index 4f8cad3b..16463aba 100644
--- a/digital/asserv/src/asserv/counter_ext.avr.c
+++ b/digital/asserv/src/asserv/counter_ext.avr.c
@@ -43,6 +43,8 @@
#define COUNTER_RIGHT 1
/** Define the first auxiliary counter address. */
#define COUNTER_AUX0 2
+/** Define the second auxiliary counter address. */
+#define COUNTER_AUX1 3
/** Define to 1 to reverse the left counter. */
#define COUNTER_LEFT_REVERSE 1
@@ -50,22 +52,27 @@
#define COUNTER_RIGHT_REVERSE 0
/** Define to 1 to reverse the first auxiliary counter. */
#define COUNTER_AUX0_REVERSE 0
+/** Define to 1 to reverse the second auxiliary counter. */
+#define COUNTER_AUX1_REVERSE 0
/** Define to 1 to use the AVR External Memory system, or 0 to use hand made
* signals. */
#define COUNTER_USE_XMEM 1
/** Last values. */
-static uint8_t counter_left_old, counter_right_old, counter_aux0_old;
+static uint8_t counter_left_old, counter_right_old,
+ counter_aux_old[AC_ASSERV_AUX_NB];
/** Overall counter values. */
-uint16_t counter_left, counter_right, counter_aux0;
+uint16_t counter_left, counter_right,
+ counter_aux[AC_ASSERV_AUX_NB];
/** Overall uncorrected counter values. */
static int32_t counter_right_raw;
/** Correction factor (f8.24). */
uint32_t counter_right_correction = 1L << 24;
/** Counter differences since last update.
* Maximum of 9 significant bits, sign included. */
-int16_t counter_left_diff, counter_right_diff, counter_aux0_diff;
+int16_t counter_left_diff, counter_right_diff,
+ counter_aux_diff[AC_ASSERV_AUX_NB];
#if !COUNTER_USE_XMEM
# define COUNTER_ALE _BV (2)
@@ -117,18 +124,20 @@ counter_init (void)
/* Begin with safe values. */
counter_left_old = counter_read (COUNTER_LEFT);
counter_right_old = counter_read (COUNTER_RIGHT);
- counter_aux0_old = counter_read (COUNTER_AUX0);
+ counter_aux_old[0] = counter_read (COUNTER_AUX0);
+ counter_aux_old[1] = counter_read (COUNTER_AUX1);
}
/** Update overall counter values and compute diffs. */
void
counter_update (void)
{
- uint8_t left, right, aux0;
+ uint8_t left, right, aux0, aux1;
/* Sample counters. */
left = counter_read (COUNTER_LEFT);
right = counter_read (COUNTER_RIGHT);
aux0 = counter_read (COUNTER_AUX0);
+ aux1 = counter_read (COUNTER_AUX1);
/* Left counter. */
#if !COUNTER_LEFT_REVERSE
counter_left_diff = (int8_t) (left - counter_left_old);
@@ -152,11 +161,19 @@ counter_update (void)
counter_right = right_new;
/* First auxiliary counter. */
#if !COUNTER_AUX0_REVERSE
- counter_aux0_diff = (int8_t) (aux0 - counter_aux0_old);
+ counter_aux_diff[0] = (int8_t) (aux0 - counter_aux_old[0]);
#else
- counter_aux0_diff = (int8_t) (counter_aux0_old - aux0);
+ counter_aux_diff[0] = (int8_t) (counter_aux_old[0] - aux0);
#endif
- counter_aux0_old = aux0;
- counter_aux0 += counter_aux0_diff;
+ counter_aux_old[0] = aux0;
+ counter_aux[0] += counter_aux_diff[0];
+ /* Second auxiliary counter. */
+#if !COUNTER_AUX1_REVERSE
+ counter_aux_diff[1] = (int8_t) (aux1 - counter_aux_old[1]);
+#else
+ counter_aux_diff[1] = (int8_t) (counter_aux_old[1] - aux1);
+#endif
+ counter_aux_old[1] = aux1;
+ counter_aux[1] += counter_aux_diff[1];
}
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index 0cb04aae..cbb92b33 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -73,26 +73,32 @@ eeprom_read_params (void)
return;
speed_theta.max = eeprom_read_byte (p8++);
speed_alpha.max = eeprom_read_byte (p8++);
- speed_aux0.max = eeprom_read_byte (p8++);
+ speed_aux[0].max = eeprom_read_byte (p8++);
+ speed_aux[1].max = eeprom_read_byte (p8++);
speed_theta.slow = eeprom_read_byte (p8++);
speed_alpha.slow = eeprom_read_byte (p8++);
- speed_aux0.slow = eeprom_read_byte (p8++);
+ speed_aux[0].slow = eeprom_read_byte (p8++);
+ speed_aux[1].slow = eeprom_read_byte (p8++);
pwm_set_reverse (eeprom_read_byte (p8++));
counter_right_correction = eeprom_read_32 (p8); p8 += 4;
p16 = (uint16_t *) p8;
postrack_set_footing (eeprom_read_word (p16++));
speed_theta.acc = eeprom_read_word (p16++);
speed_alpha.acc = eeprom_read_word (p16++);
- speed_aux0.acc = eeprom_read_word (p16++);
+ speed_aux[0].acc = eeprom_read_word (p16++);
+ speed_aux[1].acc = eeprom_read_word (p16++);
pos_theta.kp = eeprom_read_word (p16++);
pos_alpha.kp = eeprom_read_word (p16++);
- pos_aux0.kp = eeprom_read_word (p16++);
+ pos_aux[0].kp = eeprom_read_word (p16++);
+ pos_aux[1].kp = eeprom_read_word (p16++);
pos_theta.ki = eeprom_read_word (p16++);
pos_alpha.ki = eeprom_read_word (p16++);
- pos_aux0.ki = eeprom_read_word (p16++);
+ pos_aux[0].ki = eeprom_read_word (p16++);
+ pos_aux[1].ki = eeprom_read_word (p16++);
pos_theta.kd = eeprom_read_word (p16++);
pos_alpha.kd = eeprom_read_word (p16++);
- pos_aux0.kd = eeprom_read_word (p16++);
+ pos_aux[0].kd = eeprom_read_word (p16++);
+ pos_aux[1].kd = eeprom_read_word (p16++);
pos_e_sat = eeprom_read_word (p16++);
pos_i_sat = eeprom_read_word (p16++);
pos_d_sat = eeprom_read_word (p16++);
@@ -113,26 +119,32 @@ eeprom_write_params (void)
eeprom_write_byte (p8++, EEPROM_KEY);
eeprom_write_byte (p8++, speed_theta.max);
eeprom_write_byte (p8++, speed_alpha.max);
- eeprom_write_byte (p8++, speed_aux0.max);
+ eeprom_write_byte (p8++, speed_aux[0].max);
+ eeprom_write_byte (p8++, speed_aux[1].max);
eeprom_write_byte (p8++, speed_theta.slow);
eeprom_write_byte (p8++, speed_alpha.slow);
- eeprom_write_byte (p8++, speed_aux0.slow);
+ eeprom_write_byte (p8++, speed_aux[0].slow);
+ eeprom_write_byte (p8++, speed_aux[1].slow);
eeprom_write_byte (p8++, pwm_reverse);
eeprom_write_32 (p8, counter_right_correction); p8 += 4;
p16 = (uint16_t *) p8;
eeprom_write_word (p16++, postrack_footing);
eeprom_write_word (p16++, speed_theta.acc);
eeprom_write_word (p16++, speed_alpha.acc);
- eeprom_write_word (p16++, speed_aux0.acc);
+ eeprom_write_word (p16++, speed_aux[0].acc);
+ eeprom_write_word (p16++, speed_aux[1].acc);
eeprom_write_word (p16++, pos_theta.kp);
eeprom_write_word (p16++, pos_alpha.kp);
- eeprom_write_word (p16++, pos_aux0.kp);
+ eeprom_write_word (p16++, pos_aux[0].kp);
+ eeprom_write_word (p16++, pos_aux[1].kp);
eeprom_write_word (p16++, pos_theta.ki);
eeprom_write_word (p16++, pos_alpha.ki);
- eeprom_write_word (p16++, pos_aux0.ki);
+ eeprom_write_word (p16++, pos_aux[0].ki);
+ eeprom_write_word (p16++, pos_aux[1].ki);
eeprom_write_word (p16++, pos_theta.kd);
eeprom_write_word (p16++, pos_alpha.kd);
- eeprom_write_word (p16++, pos_aux0.kd);
+ eeprom_write_word (p16++, pos_aux[0].kd);
+ eeprom_write_word (p16++, pos_aux[1].kd);
eeprom_write_word (p16++, pos_e_sat);
eeprom_write_word (p16++, pos_i_sat);
eeprom_write_word (p16++, pos_d_sat);
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index 30d59780..052f37de 100644
--- a/digital/asserv/src/asserv/eeprom.h
+++ b/digital/asserv/src/asserv/eeprom.h
@@ -26,7 +26,7 @@
* }}} */
/** Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x4c
+#define EEPROM_KEY 0x4d
void
eeprom_read_params (void);
diff --git a/digital/asserv/src/asserv/init b/digital/asserv/src/asserv/init
deleted file mode 100644
index 6ec03ce4..00000000
--- a/digital/asserv/src/asserv/init
+++ /dev/null
@@ -1,14 +0,0 @@
-!p'f0dd1
-!p'p01000200
-!p'i00000000
-!p'd10001000
-!p'E03ff
-!p'D01ff
-!p'a00800080
-!p's60302010
-!p'p0100
-!p'i0000
-!p'd1000
-!p'a0080
-!p's0a05
-!Aff
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index f6e07126..647d8777 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -69,6 +69,9 @@ uint8_t main_stat_speed, main_stat_speed_cpt;
/** Statistics about shaft position control. */
uint8_t main_stat_pos, main_stat_pos_cpt;
+/** Statistics about auxiliary shaft position control. */
+uint8_t main_stat_pos_aux, main_stat_pos_aux_cpt;
+
/** Statistics about pwm values. */
uint8_t main_stat_pwm, main_stat_pwm_cpt;
@@ -147,16 +150,19 @@ main_loop (void)
/* Stats. */
if (main_sequence_ack
&& (state_main.sequence_ack != state_main.sequence_finish
- || state_aux0.sequence_ack != state_aux0.sequence_finish)
+ || state_aux[0].sequence_ack != state_aux[0].sequence_finish
+ || state_aux[1].sequence_ack != state_aux[1].sequence_finish)
&& !--main_sequence_ack_cpt)
{
- proto_send2b ('A', state_main.sequence_finish,
- state_aux0.sequence_finish);
+ proto_send3b ('A', state_main.sequence_finish,
+ state_aux[0].sequence_finish,
+ state_aux[1].sequence_finish);
main_sequence_ack_cpt = main_sequence_ack;
}
if (main_stat_counter && !--main_stat_counter_cpt)
{
- proto_send3w ('C', counter_left, counter_right, counter_aux0);
+ proto_send4w ('C', counter_left, counter_right,
+ counter_aux[0], counter_aux[1]);
main_stat_counter_cpt = main_stat_counter;
}
if (main_stat_postrack && !--main_stat_postrack_cpt)
@@ -166,22 +172,27 @@ main_loop (void)
}
if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
{
- proto_send1w ('Y', aux0.pos);
+ proto_send2w ('Y', aux[0].pos, aux[1].pos);
main_stat_aux_pos_cpt = main_stat_aux_pos;
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send3b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8,
- speed_aux0.cur >> 8);
+ proto_send4b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8,
+ speed_aux[0].cur >> 8, speed_aux[1].cur >> 8);
main_stat_speed_cpt = main_stat_speed;
}
if (main_stat_pos && !--main_stat_pos_cpt)
{
- proto_send6w ('P', pos_theta.e_old, pos_theta.i,
- pos_alpha.e_old, pos_alpha.i,
- pos_aux0.e_old, pos_aux0.i);
+ proto_send4w ('P', pos_theta.e_old, pos_theta.i,
+ pos_alpha.e_old, pos_alpha.i);
main_stat_pos_cpt = main_stat_pos;
}
+ if (main_stat_pos_aux && !--main_stat_pos_aux_cpt)
+ {
+ proto_send4w ('Q', pos_aux[0].e_old, pos_aux[0].i,
+ pos_aux[1].e_old, pos_aux[1].i);
+ main_stat_pos_aux_cpt = main_stat_pos_aux;
+ }
#ifdef HOST
if (main_simu && !--main_simu_cpt)
{
@@ -193,7 +204,8 @@ main_loop (void)
#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
- proto_send3w ('W', pwm_left.cur, pwm_right.cur, pwm_aux0.cur);
+ proto_send4w ('W', pwm_left.cur, pwm_right.cur,
+ pwm_aux[0].cur, pwm_aux[1].cur);
main_stat_pwm_cpt = main_stat_pwm;
}
if (main_stat_timer && !--main_stat_timer_cpt)
@@ -218,6 +230,21 @@ main_loop (void)
void
proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
+ /* Many commands use the first argument as a selector. */
+ struct aux_t *aux = 0;
+ struct pwm_t *pwm = 0;
+ struct pos_t *pos = 0;
+ struct speed_t *speed = 0;
+ struct state_t *state = 0;
+ if (args[0] < AC_ASSERV_AUX_NB)
+ {
+ aux = &aux[args[0]];
+ pwm = &pwm_aux[args[0]];
+ pos = &pos_aux[args[0]];
+ speed = &speed_aux[args[0]];
+ state = &state_aux[args[0]];
+ }
+ /* Decode command. */
#define c(cmd, size) (cmd << 8 | size)
switch (c (cmd, size))
{
@@ -234,6 +261,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pwm_set (&pwm_left, 0);
pwm_set (&pwm_right, 0);
break;
+ case c ('W', 0):
+ /* Set zero auxiliary pwm. */
+ pos_reset (&pos_aux[0]);
+ pos_reset (&pos_aux[1]);
+ state_aux[0].mode = MODE_PWM;
+ state_aux[1].mode = MODE_PWM;
+ pwm_set (&pwm_aux[0], 0);
+ pwm_set (&pwm_aux[1], 0);
+ break;
case c ('w', 4):
/* Set pwm.
* - w: left pwm.
@@ -244,12 +280,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
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):
+ case c ('W', 3):
/* Set auxiliary pwm.
+ * - b: aux index.
* - w: pwm. */
- pos_reset (&pos_aux0);
- state_aux0.mode = MODE_PWM;
- pwm_set (&pwm_aux0, v8_to_v16 (args[0], args[1]));
+ if (!aux) { proto_send0 ('?'); return; }
+ pos_reset (pos);
+ state->mode = MODE_PWM;
+ pwm_set (pwm, v8_to_v16 (args[1], args[2]));
break;
case c ('c', 4):
/* Add to position consign.
@@ -260,12 +298,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pos_theta.cons += v8_to_v16 (args[0], args[1]);
pos_alpha.cons += v8_to_v16 (args[2], args[3]);
break;
- case c ('c', 2):
+ case c ('C', 3):
/* Add to auxiliary position consign.
+ * - b: aux index.
* - w: consign offset. */
- state_aux0.mode = MODE_POS;
- state_aux0.variant = 0;
- pos_aux0.cons += v8_to_v16 (args[0], args[1]);
+ if (!aux) { proto_send0 ('?'); return; }
+ state->mode = MODE_POS;
+ state->variant = 0;
+ pos->cons += v8_to_v16 (args[1], args[2]);
break;
case c ('s', 0):
/* Stop (set zero speed). */
@@ -285,13 +325,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_theta.cons = args[0] << 8;
speed_alpha.cons = args[1] << 8;
break;
- case c ('s', 1):
+ case c ('S', 2):
/* Set auxiliary speed.
+ * - b: aux index.
* - b: speed. */
- state_aux0.mode = MODE_SPEED;
- state_aux0.variant = 0;
- speed_aux0.use_pos = 0;
- speed_aux0.cons = args[0] << 8;
+ if (!aux) { proto_send0 ('?'); return; }
+ state->mode = MODE_SPEED;
+ state->variant = 0;
+ speed->use_pos = 0;
+ speed->cons = args[1] << 8;
break;
case c ('s', 9):
/* Set speed controlled position consign.
@@ -307,17 +349,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]);
state_start (&state_main, MODE_SPEED, args[8]);
break;
- case c ('s', 5):
- /* Set auxiliary speed controlled position consign.
- * - d: consign offset.
- * - b: sequence number. */
- if (args[4] == state_aux0.sequence)
- break;
- speed_aux0.use_pos = 1;
- speed_aux0.pos_cons = pos_aux0.cons;
- speed_aux0.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
- state_start (&state_aux0, MODE_SPEED, args[4]);
- break;
case c ('l', 5):
/* Set linear speed controlled position consign.
* - d: consign offset.
@@ -339,6 +370,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
traj_angle_offset_start (v8_to_v32 (args[0], args[1], args[2],
args[3]), args[4]);
break;
+ case c ('S', 6):
+ /* Set auxiliary speed controlled position consign.
+ * - b: aux index.
+ * - d: consign offset.
+ * - b: sequence number. */
+ if (!aux) { proto_send0 ('?'); return; }
+ if (args[5] == state->sequence)
+ break;
+ speed->use_pos = 1;
+ speed->pos_cons = pos->cons;
+ speed->pos_cons += v8_to_v32 (args[1], args[2], args[3], args[4]);
+ state_start (state, MODE_SPEED, args[5]);
+ break;
case c ('f', 1):
/* Go to the wall.
* - b: sequence number. */
@@ -412,24 +456,30 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
case c ('y', 3):
/* Auxiliary go to position.
+ * - b: aux index.
* - w: pos, i16.
* - b: sequence number. */
- if (args[2] == state_aux0.sequence)
+ if (!aux) { proto_send0 ('?'); return; }
+ if (args[3] == state->sequence)
break;
- aux_traj_goto_start (&aux0, v8_to_v16 (args[0], args[1]), args[2]);
+ aux_traj_goto_start (aux, v8_to_v16 (args[1], args[2]), args[3]);
break;
case c ('y', 1):
/* Auxiliary find zero.
+ * - b: aux index.
* - b: sequence number. */
- if (args[0] == state_aux0.sequence)
+ if (!aux) { proto_send0 ('?'); return; }
+ if (args[1] == state->sequence)
break;
- aux_traj_find_zero_start (&aux0, args[0]);
+ aux_traj_find_zero_start (aux, args[1]);
break;
- case c ('a', 2):
- /* Set both acknoledge.
+ case c ('a', 3):
+ /* Set all acknoledge.
* - b: main ack sequence number.
- * - b: auxiliary ack sequence number. */
- state_acknowledge (&state_aux0, args[1]);
+ * - b: first auxiliary ack sequence number.
+ * - b: second auxiliary ack sequence number. */
+ state_acknowledge (&state_aux[0], args[1]);
+ state_acknowledge (&state_aux[1], args[2]);
/* no break; */
case c ('a', 1):
/* Set main acknoledge.
@@ -462,6 +512,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Motor position control stats. */
main_stat_pos_cpt = main_stat_pos = args[0];
break;
+ case c ('Q', 1):
+ /* Auxiliary motor position control stats. */
+ main_stat_pos_aux_cpt = main_stat_pos_aux = args[0];
+ break;
case c ('W', 1):
/* Pwm stats. */
main_stat_pwm_cpt = main_stat_pwm = args[0];
@@ -484,6 +538,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Params. */
if (cmd == 'p')
{
+ /* Many commands use the first argument as a selector. */
+ switch (args[1])
+ {
+ case 't':
+ pos = &pos_theta;
+ speed = &speed_theta;
+ break;
+ case 'a':
+ pos = &pos_alpha;
+ speed = &speed_alpha;
+ break;
+ case 0:
+ case 1:
+ pos = &pos_aux[args[1]];
+ speed = &speed_aux[args[1]];
+ break;
+ default:
+ pos = 0;
+ speed = 0;
+ break;
+ }
switch (c (args[0], size))
{
case c ('X', 1):
@@ -518,56 +593,42 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: footing. */
postrack_set_footing (v8_to_v16 (args[1], args[2]));
break;
- case c ('a', 5):
- /* Set main acceleration.
- * - w: theta.
- * - w: alpha. */
- speed_theta.acc = v8_to_v16 (args[1], args[2]);
- speed_alpha.acc = v8_to_v16 (args[3], args[4]);
- break;
- case c ('a', 3):
- /* Set auxiliary acceleration.
+ case c ('a', 4):
+ /* Set acceleration.
+ * - b: index.
* - w: acceleration. */
- speed_aux0.acc = v8_to_v16 (args[1], args[2]);
- break;
- case c ('s', 5):
- /* Set main maximum and slow speed.
- * - b: theta max.
- * - b: alpha max.
- * - b: theta slow.
- * - b: alpha slow. */
- speed_theta.max = args[1];
- speed_alpha.max = args[2];
- speed_theta.slow = args[3];
- speed_alpha.slow = args[4];
- break;
- case c ('s', 3):
- /* Set auxiliary maximum and slow speed.
- * - b: speed max.
- * - b: speed slow. */
- speed_aux0.max = args[1];
- speed_aux0.slow = args[2];
- break;
- case c ('p', 3):
- pos_aux0.kp = v8_to_v16 (args[1], args[2]);
- break;
- case c ('p', 5):
- pos_theta.kp = v8_to_v16 (args[1], args[2]);
- pos_alpha.kp = v8_to_v16 (args[3], args[4]);
+ if (!speed) { proto_send0 ('?'); return; }
+ speed->acc = v8_to_v16 (args[2], args[3]);
break;
- case c ('i', 3):
- pos_aux0.ki = v8_to_v16 (args[1], args[2]);
+ case c ('s', 4):
+ /* Set maximum and slow speed.
+ * - b: index.
+ * - b: max speed.
+ * - b: slow speed. */
+ if (!speed) { proto_send0 ('?'); return; }
+ speed->max = args[2];
+ speed->slow = args[3];
break;
- case c ('i', 5):
- pos_theta.ki = v8_to_v16 (args[1], args[2]);
- pos_alpha.ki = v8_to_v16 (args[3], args[4]);
+ case c ('p', 4):
+ /* Set proportional coefficient.
+ * - b: index.
+ * - w: P coefficient. */
+ if (!pos) { proto_send0 ('?'); return; }
+ pos->kp = v8_to_v16 (args[2], args[3]);
break;
- case c ('d', 3):
- pos_aux0.kd = v8_to_v16 (args[1], args[2]);
+ case c ('i', 4):
+ /* Set integral coefficient.
+ * - b: index.
+ * - w: I coefficient. */
+ if (!pos) { proto_send0 ('?'); return; }
+ pos->ki = v8_to_v16 (args[2], args[3]);
break;
- case c ('d', 5):
- pos_theta.kd = v8_to_v16 (args[1], args[2]);
- pos_alpha.kd = v8_to_v16 (args[3], args[4]);
+ case c ('d', 4):
+ /* Set differential coefficient.
+ * - b: index.
+ * - w: D coefficient. */
+ if (!pos) { proto_send0 ('?'); return; }
+ pos->kd = v8_to_v16 (args[2], args[3]);
break;
case c ('E', 3):
pos_e_sat = v8_to_v16 (args[1], args[2]);
@@ -592,7 +653,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
case c ('w', 2):
/* Set PWM direction.
- * - b: bits: 0000[aux0][right][left]. */
+ * - b: bits: 000[aux1][aux0][right][left]. */
pwm_set_reverse (args[1]);
break;
case c ('E', 2):
@@ -609,16 +670,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send1d ('c', counter_right_correction);
proto_send1w ('f', postrack_footing);
proto_send2w ('a', speed_theta.acc, speed_alpha.acc);
- proto_send4b ('s', speed_theta.max, speed_alpha.max,
- speed_theta.slow, speed_alpha.slow);
+ proto_send2b ('s', speed_theta.max, speed_theta.slow);
+ proto_send2b ('s', speed_alpha.max, speed_alpha.slow);
proto_send2w ('p', pos_theta.kp, pos_alpha.kp);
proto_send2w ('i', pos_theta.ki, pos_alpha.ki);
proto_send2w ('d', pos_theta.kd, pos_alpha.kd);
- proto_send1w ('a', speed_aux0.acc);
- proto_send2b ('s', speed_aux0.max, speed_aux0.slow);
- proto_send1w ('p', pos_aux0.kp);
- proto_send1w ('i', pos_aux0.ki);
- proto_send1w ('d', pos_aux0.kd);
+ proto_send2w ('a', speed_aux[0].acc, speed_aux[1].acc);
+ proto_send2b ('s', speed_aux[0].max, speed_aux[0].slow);
+ proto_send2b ('s', speed_aux[1].max, speed_aux[1].slow);
+ proto_send2w ('p', pos_aux[0].kp, pos_aux[1].kp);
+ proto_send2w ('i', pos_aux[0].ki, pos_aux[1].ki);
+ proto_send2w ('d', pos_aux[0].kd, pos_aux[1].kd);
proto_send1w ('E', pos_e_sat);
proto_send1w ('I', pos_i_sat);
proto_send1w ('D', pos_d_sat);
diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c
index b8197b60..97341626 100644
--- a/digital/asserv/src/asserv/models.host.c
+++ b/digital/asserv/src/asserv/models.host.c
@@ -96,7 +96,7 @@ static const struct robot_t gloubi_robot =
13.0, // approx
/* Whether the encoder is mounted on the main motor (false) or not (true). */
0,
- 0.0, 0.0, NULL, 0
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }
};
/* Taz, APBTeam/Efrei 2005. */
@@ -116,7 +116,7 @@ static const struct robot_t taz_robot =
0.0,
/* Whether the encoder is mounted on the main motor (false) or not (true). */
0,
- 0.0, 0.0, NULL, 0
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }
};
/* TazG, Taz with RE25G motors. */
@@ -136,7 +136,7 @@ static const struct robot_t tazg_robot =
0.0,
/* Whether the encoder is mounted on the main motor (false) or not (true). */
0,
- 0.0, 0.0, NULL, 0
+ 0.0, 0.0, { NULL, NULL }, { 0, 0 }
};
/* Giboulée arm model, with a RE25CLL and a 1:10 ratio gearbox. */
@@ -179,10 +179,10 @@ static const struct robot_t giboulee_robot =
0.063 / 2,
/** Distance between the encoders wheels (m). */
0.28,
- /** First auxiliary motor or NULL if none. */
- &giboulee_arm_model,
- /** Number of steps on the first auxiliary motor encoder. */
- 500,
+ /** Auxiliary motors, NULL if not present. */
+ { &giboulee_arm_model, NULL },
+ /** Number of steps for each auxiliary motor encoder. */
+ { 500, 0 },
};
/* Table of models. */
@@ -214,8 +214,9 @@ models_get (const char *name)
/** Initialise simulation models. */
void
models_init (const struct robot_t *robot, struct motor_t *main_motor_left,
- struct motor_t *main_motor_right, struct motor_t *aux0_motor)
+ struct motor_t *main_motor_right, struct motor_t aux_motor[])
{
+ int i;
if (main_motor_left)
{
main_motor_left->m = *robot->main_motor;
@@ -232,11 +233,17 @@ models_init (const struct robot_t *robot, struct motor_t *main_motor_left,
main_motor_right->h = ECHANT_PERIOD;
main_motor_right->d = 1000;
}
- if (aux0_motor && robot->aux0_motor)
+ if (aux_motor)
{
- aux0_motor->m = *robot->aux0_motor;
- aux0_motor->h = ECHANT_PERIOD;
- aux0_motor->d = 1000;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ if (robot->aux_motor[i])
+ {
+ aux_motor[i].m = *robot->aux_motor[i];
+ aux_motor[i].h = ECHANT_PERIOD;
+ aux_motor[i].d = 1000;
+ }
+ }
}
}
diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h
index 36099e64..ae2bc21d 100644
--- a/digital/asserv/src/asserv/models.host.h
+++ b/digital/asserv/src/asserv/models.host.h
@@ -49,10 +49,10 @@ struct robot_t
double encoder_wheel_r;
/** Distance between the encoders wheels (m). */
double encoder_footing;
- /** First auxiliary motor or NULL if none. */
- const struct motor_def_t *aux0_motor;
- /** Number of steps on the first auxiliary motor encoder. */
- int aux0_encoder_steps;
+ /** Auxiliary motors, NULL if not present. */
+ const struct motor_def_t *aux_motor[AC_ASSERV_AUX_NB];
+ /** Number of steps for each auxiliary motor encoder. */
+ int aux_encoder_steps[AC_ASSERV_AUX_NB];
};
/** Get a pointer to a model by name, or return 0. */
@@ -62,6 +62,6 @@ models_get (const char *name);
/** Initialise simulation models. */
void
models_init (const struct robot_t *robot, struct motor_t *main_motor_left,
- struct motor_t *main_motor_right, struct motor_t *aux0_motor);
+ struct motor_t *main_motor_right, struct motor_t aux_motor[]);
#endif /* models_host_h */
diff --git a/digital/asserv/src/asserv/move_bot_blue b/digital/asserv/src/asserv/move_bot_blue
deleted file mode 100644
index ec72254b..00000000
--- a/digital/asserv/src/asserv/move_bot_blue
+++ /dev/null
@@ -1 +0,0 @@
-!h'X00C807EE12D7
diff --git a/digital/asserv/src/asserv/move_bot_red b/digital/asserv/src/asserv/move_bot_red
deleted file mode 100644
index 28d8ef29..00000000
--- a/digital/asserv/src/asserv/move_bot_red
+++ /dev/null
@@ -1 +0,0 @@
-!h'X0AF007EE12D7
diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
index eafd2877..0679de40 100644
--- a/digital/asserv/src/asserv/pos.c
+++ b/digital/asserv/src/asserv/pos.c
@@ -44,7 +44,7 @@
struct pos_t pos_theta, pos_alpha;
/** Auxiliaries control states. */
-struct pos_t pos_aux0;
+struct pos_t pos_aux[AC_ASSERV_AUX_NB];
/** Error saturation. */
int32_t pos_e_sat = 1023;
@@ -183,10 +183,13 @@ pos_update_single (struct state_t *state, struct pos_t *pos,
void
pos_update (void)
{
+ uint8_t i;
pos_update_polar (&state_main, &pos_theta, &pos_alpha,
counter_left_diff, counter_right_diff,
&pwm_left, &pwm_right);
- pos_update_single (&state_aux0, &pos_aux0, counter_aux0_diff, &pwm_aux0);
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ pos_update_single (&state_aux[i], &pos_aux[i], counter_aux_diff[i],
+ &pwm_aux[i]);
}
/** Reset position control state. To be called when the position control is
diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h
index cf02cda3..4ced49b8 100644
--- a/digital/asserv/src/asserv/pos.h
+++ b/digital/asserv/src/asserv/pos.h
@@ -45,7 +45,7 @@ struct pos_t
};
extern struct pos_t pos_theta, pos_alpha;
-extern struct pos_t pos_aux0;
+extern struct pos_t pos_aux[AC_ASSERV_AUX_NB];
extern int32_t pos_e_sat, pos_i_sat, pos_d_sat;
extern int32_t pos_blocked_error_limit, pos_blocked_speed_limit,
diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c
index 6a5b0ff4..cf8df769 100644
--- a/digital/asserv/src/asserv/pwm.avr.c
+++ b/digital/asserv/src/asserv/pwm.avr.c
@@ -30,7 +30,9 @@
/** 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);
+struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = {
+ PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1)
+};
/** PWM reverse directions. */
uint8_t pwm_reverse;
diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h
index 7877d4eb..7b834c05 100644
--- a/digital/asserv/src/asserv/pwm.h
+++ b/digital/asserv/src/asserv/pwm.h
@@ -41,16 +41,25 @@ struct pwm_t
int16_t min;
};
-extern struct pwm_t pwm_left, pwm_right, pwm_aux0;
+extern struct pwm_t pwm_left, pwm_right, pwm_aux[AC_ASSERV_AUX_NB];
extern uint8_t pwm_reverse;
+/** Define current PWM value for each output. */
+#define PWM_VALUE(x) PWM_VALUE_ (x)
+#define PWM_VALUE_(x) PWM_MAX_FOR_ ## x
+#define PWM_VALUE_pwm_left pwm_left.cur
+#define PWM_VALUE_pwm_right pwm_right.cur
+#define PWM_VALUE_pwm_aux0 pwm_aux[0].cur
+#define PWM_VALUE_pwm_aux1 pwm_aux[1].cur
+
/** 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 PWM_MAX_FOR_pwm_aux1 (PWM_MAX / 2)
/** Define minimum PWM value for each output, if the value is less than the
* minimum, use 0. */
@@ -59,6 +68,7 @@ extern uint8_t pwm_reverse;
#define PWM_MIN_FOR_pwm_left 0x10
#define PWM_MIN_FOR_pwm_right 0x10
#define PWM_MIN_FOR_pwm_aux0 0x10
+#define PWM_MIN_FOR_pwm_aux1 0x10
/** Define which bit controls the PWM inversion. */
#define PWM_REVERSE_BIT(x) PWM_REVERSE_BIT_ (x)
@@ -66,6 +76,7 @@ extern uint8_t pwm_reverse;
#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 PWM_REVERSE_BIT_pwm_aux1 _BV (3)
/** State init macro. */
#define PWM_INIT_FOR(x) \
diff --git a/digital/asserv/src/asserv/pwm_mp.avr.c b/digital/asserv/src/asserv/pwm_mp.avr.c
index 47b50763..62ef2190 100644
--- a/digital/asserv/src/asserv/pwm_mp.avr.c
+++ b/digital/asserv/src/asserv/pwm_mp.avr.c
@@ -34,7 +34,7 @@
#define PWM1 pwm_left
#define PWM2 pwm_right
#define PWM3 pwm_aux0
-#undef PWM4
+#define PWM4 pwm_aux1
#include "pwm_config.h"
@@ -98,8 +98,8 @@ void
pwm_mp_update (void)
{
#if PWM1or2 || PWM3or4
- if (PWM1c (PWM1.cur) || PWM2c (PWM2.cur)
- || PWM3c (PWM3.cur) || PWM4c (PWM4.cur))
+ if (PWM1c (PWM_VALUE (PWM1)) || PWM2c (PWM_VALUE (PWM2))
+ || PWM3c (PWM_VALUE (PWM3)) || PWM4c (PWM_VALUE (PWM4)))
pwm_mp_go = 1;
if (!pwm_mp_go)
return;
@@ -107,7 +107,7 @@ pwm_mp_update (void)
#if PWM1or2
/* Chip enable. */
PORTB &= ~_BV (0);
- pwm_mp_send (PWM1c (PWM1.cur), PWM2c (PWM2.cur),
+ pwm_mp_send (PWM1c (PWM_VALUE (PWM1)), PWM2c (PWM_VALUE (PWM2)),
PWM1c (pwm_reverse & PWM_REVERSE_BIT (PWM1)),
PWM2c (pwm_reverse & PWM_REVERSE_BIT (PWM2)));
/* Chip disable. */
@@ -116,7 +116,7 @@ pwm_mp_update (void)
#if PWM3or4
/* Chip enable. */
PORTE &= ~_BV (4);
- pwm_mp_send (PWM3c (PWM3.cur), PWM4c (PWM4.cur),
+ pwm_mp_send (PWM3c (PWM_VALUE (PWM3)), PWM4c (PWM_VALUE (PWM4)),
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 75857f10..25f2cfc7 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.cur == 0)
+ if (PWM_VALUE (PWM1) == 0)
{
pwm1 = 0;
}
- else if (PWM1.cur < 0)
+ else if (PWM_VALUE (PWM1) < 0)
{
- pwm1 = -PWM1.cur;
+ pwm1 = -PWM_VALUE (PWM1);
}
else
{
dir_b |= _BV (PWM1_DIR);
- pwm1 = PWM1.cur;
+ pwm1 = PWM_VALUE (PWM1);
}
# endif /* PWM1 */
# ifdef PWM2
uint16_t pwm2;
/* Set PWM2. */
- if (PWM2.cur == 0)
+ if (PWM_VALUE (PWM2) == 0)
{
pwm2 = 0;
}
- else if (PWM2.cur < 0)
+ else if (PWM_VALUE (PWM2) < 0)
{
- pwm2 = -PWM2.cur;
+ pwm2 = -PWM_VALUE (PWM2);
}
else
{
dir_b |= _BV (PWM2_DIR);
- pwm2 = PWM2.cur;
+ pwm2 = PWM_VALUE (PWM2);
}
# endif /* PWM2 */
#endif /* PWM1or2 */
@@ -134,35 +134,35 @@ pwm_ocr_update (void)
# ifdef PWM3
uint16_t pwm3;
/* Set PWM3. */
- if (PWM3.cur == 0)
+ if (PWM_VALUE (PWM3) == 0)
{
pwm3 = 0;
}
- else if (PWM3.cur < 0)
+ else if (PWM_VALUE (PWM3) < 0)
{
- pwm3 = -PWM3.cur;
+ pwm3 = -PWM_VALUE (PWM3);
}
else
{
dir_e |= _BV (PWM3_DIR);
- pwm3 = PWM3.cur;
+ pwm3 = PWM_VALUE (PWM3);
}
# endif /* PWM3 */
# ifdef PWM4
uint16_t pwm4;
/* Set PWM4. */
- if (PWM4.cur == 0)
+ if (PWM_VALUE (PWM4) == 0)
{
pwm4 = 0;
}
- else if (PWM4.cur < 0)
+ else if (PWM_VALUE (PWM4) < 0)
{
- pwm4 = -PWM4.cur;
+ pwm4 = -PWM_VALUE (PWM4);
}
else
{
dir_e |= _BV (PWM4_DIR);
- pwm4 = PWM4.cur;
+ pwm4 = PWM_VALUE (PWM4);
}
# endif /* PWM4 */
#endif /* PWM3or4 */
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index d496b61e..2ae3c3d4 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -38,6 +38,7 @@
#include <string.h>
#include "pwm.h"
+#include "aux.h"
#include "motor_model.host.h"
#include "models.host.h"
@@ -46,10 +47,12 @@
uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC;
/** Overall counter values. */
-uint16_t counter_left, counter_right, counter_aux0;
+uint16_t counter_left, counter_right,
+ counter_aux[AC_ASSERV_AUX_NB];
/** Counter differences since last update.
* Maximum of 9 significant bits, sign included. */
-int16_t counter_left_diff, counter_right_diff, counter_aux0_diff;
+int16_t counter_left_diff, counter_right_diff,
+ counter_aux_diff[AC_ASSERV_AUX_NB];
/** Overall uncorrected counter values. */
static int32_t counter_right_raw;
/** Correction factor (f8.24). */
@@ -58,7 +61,9 @@ uint32_t counter_right_correction = 1L << 24;
/** 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);
+struct pwm_t pwm_aux[AC_ASSERV_AUX_NB] = {
+ PWM_INIT_FOR (pwm_aux0), PWM_INIT_FOR (pwm_aux1)
+};
/** PWM reverse directions. */
uint8_t pwm_reverse;
@@ -66,13 +71,15 @@ uint8_t pwm_reverse;
const struct robot_t *simu_robot;
/** Motor models. */
-struct motor_t simu_left_model, simu_right_model, simu_aux0_model;
+struct motor_t simu_left_model, simu_right_model,
+ simu_aux_model[AC_ASSERV_AUX_NB];
/** Computed simulated position (mm, rad). */
double simu_pos_x, simu_pos_y, simu_pos_a;
/** Full counter values. */
-uint32_t simu_counter_left, simu_counter_right, simu_counter_aux0;
+uint32_t simu_counter_left, simu_counter_right,
+ simu_counter_aux[AC_ASSERV_AUX_NB];
double simu_counter_left_th, simu_counter_right_th;
/** Use mex. */
@@ -103,7 +110,7 @@ simu_init (void)
exit (1);
}
models_init (simu_robot, &simu_left_model, &simu_right_model,
- &simu_aux0_model);
+ simu_aux_model);
simu_pos_x = simu_pos_y = simu_pos_a = 0;
}
@@ -158,31 +165,40 @@ simu_sensor_update (void)
PINC |= sensors_bit[i];
}
/** Top zero sensor. */
- double aa = simu_aux0_model.th / simu_aux0_model.m.i_G * 3;
- if (!(cos (aa) > 0 && fabs (sin (aa)) * 80.0 < 7.5))
- PINC |= _BV (5);
+ double aa;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ aa = simu_aux_model[i].th / simu_aux_model[i].m.i_G * 3;
+ if (!(cos (aa) > 0 && fabs (sin (aa)) * 80.0 < 7.5))
+ *aux[i].zero_pin |= aux[i].zero_bv;
+ }
}
/** Do a simulation step. */
static void
simu_step (void)
{
- double old_left_th, old_right_th, old_aux0_th;
+ int i;
+ double old_left_th, old_right_th, old_aux_th[AC_ASSERV_AUX_NB];
/* Convert pwm value into voltage. */
simu_left_model.u = simu_left_model.m.u_max
* ((double) pwm_left.cur / (PWM_MAX + 1));
simu_right_model.u = simu_right_model.m.u_max
* ((double) pwm_right.cur / (PWM_MAX + 1));
- simu_aux0_model.u = simu_aux0_model.m.u_max
- * ((double) pwm_aux0.cur / (PWM_MAX + 1));
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ simu_aux_model[i].u = simu_aux_model[i].m.u_max
+ * ((double) pwm_aux[i].cur / (PWM_MAX + 1));
/* Make one step. */
old_left_th = simu_left_model.th;
old_right_th = simu_right_model.th;
- old_aux0_th = simu_aux0_model.th;
motor_model_step (&simu_left_model);
motor_model_step (&simu_right_model);
- if (simu_robot->aux0_motor)
- motor_model_step (&simu_aux0_model);
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ {
+ old_aux_th[i] = simu_aux_model[i].th;
+ if (simu_robot->aux_motor[i])
+ motor_model_step (&simu_aux_model[i]);
+ }
/* Modify counters. */
uint32_t counter_left_new;
uint32_t counter_right_new;
@@ -224,13 +240,16 @@ simu_step (void)
counter_right = right_new;
simu_counter_right = counter_right_new;
/* Update auxiliary counter. */
- if (simu_robot->aux0_motor)
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
{
- uint32_t counter_aux0_new = simu_aux0_model.th / (2*M_PI)
- * simu_robot->aux0_encoder_steps;
- counter_aux0_diff = counter_aux0_new - simu_counter_aux0;
- counter_aux0 += counter_aux0_diff;
- simu_counter_aux0 = counter_aux0_new;
+ if (simu_robot->aux_motor[i])
+ {
+ uint32_t counter_aux_new = simu_aux_model[i].th / (2*M_PI)
+ * simu_robot->aux_encoder_steps[i];
+ counter_aux_diff[i] = counter_aux_new - simu_counter_aux[i];
+ counter_aux[i] += counter_aux_diff[i];
+ simu_counter_aux[i] = counter_aux_new;
+ }
}
/* Update position. */
simu_pos_update ((simu_left_model.th - old_left_th)
@@ -246,6 +265,7 @@ simu_step (void)
static void
simu_send (void)
{
+ int i;
mex_msg_t *m;
/* Send position. */
m = mex_msg_new (0xa0);
@@ -254,12 +274,15 @@ simu_send (void)
mex_node_send (m);
/* Send PWM. */
m = mex_msg_new (0xa1);
- mex_msg_push (m, "hhh", pwm_left.cur, pwm_right.cur, pwm_aux0.cur);
+ mex_msg_push (m, "hh", pwm_left.cur, pwm_right.cur);
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ mex_msg_push (m, "h", pwm_aux[i].cur);
mex_node_send (m);
- /* Send Arm position. */
+ /* Send Aux position. */
m = mex_msg_new (0xa8);
- mex_msg_push (m, "l", (int32_t) (1024.0 * simu_aux0_model.th /
- simu_aux0_model.m.i_G));
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ mex_msg_push (m, "l", (int32_t) (1024.0 * simu_aux_model[i].th
+ / simu_aux_model[i].m.i_G));
mex_node_send (m);
}
diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c
index b9e6a714..5c221b7e 100644
--- a/digital/asserv/src/asserv/speed.c
+++ b/digital/asserv/src/asserv/speed.c
@@ -41,15 +41,17 @@
struct speed_t speed_theta, speed_alpha;
/** Auxiliaries speed control states. */
-struct speed_t speed_aux0;
+struct speed_t speed_aux[AC_ASSERV_AUX_NB];
/** Initialise speed control states. */
void
speed_init (void)
{
+ uint8_t i;
speed_theta.pos = &pos_theta;
speed_alpha.pos = &pos_alpha;
- speed_aux0.pos = &pos_aux0;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ speed_aux[i].pos = &pos_aux[i];
}
/** Update current speed according to a speed consign. */
@@ -147,7 +149,9 @@ speed_update_single (struct state_t *state, struct speed_t *speed)
void
speed_update (void)
{
+ uint8_t i;
speed_update_double (&state_main, &speed_theta, &speed_alpha);
- speed_update_single (&state_aux0, &speed_aux0);
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ speed_update_single (&state_aux[i], &speed_aux[i]);
}
diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h
index 3ed02393..66ad53e2 100644
--- a/digital/asserv/src/asserv/speed.h
+++ b/digital/asserv/src/asserv/speed.h
@@ -47,7 +47,7 @@ struct speed_t
};
extern struct speed_t speed_theta, speed_alpha;
-extern struct speed_t speed_aux0;
+extern struct speed_t speed_aux[AC_ASSERV_AUX_NB];
void
speed_init (void);
diff --git a/digital/asserv/src/asserv/state.c b/digital/asserv/src/asserv/state.c
index 21b994d1..712b445c 100644
--- a/digital/asserv/src/asserv/state.c
+++ b/digital/asserv/src/asserv/state.c
@@ -27,5 +27,5 @@
struct state_t state_main;
-struct state_t state_aux0;
+struct state_t state_aux[AC_ASSERV_AUX_NB];
diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h
index ebfa11dd..7967ed65 100644
--- a/digital/asserv/src/asserv/state.h
+++ b/digital/asserv/src/asserv/state.h
@@ -79,8 +79,8 @@ struct state_t
/** Main motors state. */
extern struct state_t state_main;
-/** First auxiliary motor state. */
-extern struct state_t state_aux0;
+/** Auxiliary motor states. */
+extern struct state_t state_aux[AC_ASSERV_AUX_NB];
/** Start a new command execution. */
static inline void
diff --git a/digital/asserv/src/asserv/test_counter.c b/digital/asserv/src/asserv/test_counter.c
index 002d08ec..548cc3ee 100644
--- a/digital/asserv/src/asserv/test_counter.c
+++ b/digital/asserv/src/asserv/test_counter.c
@@ -85,7 +85,8 @@ main (void)
}
if (count && !--count_cpt)
{
- proto_send3w ('C', counter_left, counter_right, counter_aux0);
+ proto_send4w ('C', counter_left, counter_right,
+ counter_aux[0], counter_aux[1]);
count_cpt = count;
}
while (uart0_poll ())
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 13cc441d..f1baecb6 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -73,8 +73,8 @@ twi_proto_update (void)
u8 status[12];
status[0] = (speed_theta.cur < 0 ? (1 << 5) : 0)
| (speed_theta.cur > 0 ? (1 << 4) : 0)
- | (state_aux0.blocked << 3)
- | (state_aux0.finished << 2)
+ | (state_aux[0].blocked << 3)
+ | (state_aux[0].finished << 2)
| (state_main.blocked << 1)
| (state_main.finished << 0);
status[1] = twi_proto.seq;
@@ -86,8 +86,8 @@ twi_proto_update (void)
status[7] = v32_to_v8 (postrack_y, 1);
status[8] = v32_to_v8 (postrack_a, 2);
status[9] = v32_to_v8 (postrack_a, 1);
- status[10] = v16_to_v8 (aux0.pos, 1);
- status[11] = v16_to_v8 (aux0.pos, 0);
+ status[10] = v16_to_v8 (aux[0].pos, 1);
+ status[11] = v16_to_v8 (aux[0].pos, 0);
twi_sl_update (status, sizeof (status));
}
@@ -190,14 +190,14 @@ twi_proto_callback (u8 *buf, u8 size)
/* Move the arm.
* - w: new position.
* - b: speed. */
- speed_aux0.max = buf[4];
- aux_traj_goto_start (&aux0, v8_to_v16 (buf[2], buf[3]), 0);
+ speed_aux[0].max = buf[4];
+ aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0);
break;
case c ('B', 1):
/* Find the zero position of the arm.
* - b: speed. */
- speed_aux0.max = buf[2];
- aux_traj_find_zero_start (&aux0, 0);
+ speed_aux[0].max = buf[2];
+ aux_traj_find_zero_start (&aux[0], 0);
break;
case c ('p', x):
/* Set parameters. */
diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py
index 5a16f735..74ea9132 100644
--- a/digital/asserv/tools/asserv/asserv.py
+++ b/digital/asserv/tools/asserv/asserv.py
@@ -35,34 +35,40 @@ class Proto:
interface."""
stats_format = {
- 'C': 'HHH',
- 'Z': 'H',
- 'S': 'bbb',
- 'P': 'hhhhhh',
- 'W': 'hhh',
+ 'C': 'HHHH',
+ 'S': 'bbbb',
+ 'P': 'hhhh',
+ 'Q': 'hhhh',
+ 'W': 'hhhh',
}
# The last occuring stats will increment stats_count, so they have to
# be in the same order than in asserv program.
- stats_order = 'CZSPW'
+ stats_order = 'CSPW'
stats_items = {
'lc': ('C', 0),
'rc': ('C', 1),
'a0c': ('C', 2),
- 'a0z': ('Z', 0),
+ 'a1c': ('C', 3),
'ts': ('S', 0),
'as': ('S', 1),
'a0s': ('S', 2),
+ 'a1s': ('S', 3),
'te': ('P', 0),
'ti': ('P', 1),
'ae': ('P', 2),
'ai': ('P', 3),
- 'a0e': ('P', 4),
- 'a0i': ('P', 5),
+ 'a0e': ('Q', 0),
+ 'a0i': ('Q', 1),
+ 'a1e': ('Q', 2),
+ 'a1i': ('Q', 3),
'lw': ('W', 0),
'rw': ('W', 1),
'a0w': ('W', 2),
+ 'a1w': ('W', 3),
}
+ _index = dict (t = ord ('t'), a = ord ('a'), a0 = 0, a1 = 1)
+
class Position (Observable):
"""An observable position. To be used with register_pos.
@@ -88,9 +94,9 @@ class Proto:
self.async = False
self.mseq = 0
self.mseq_ack = 0
- self.a0seq = 0
- self.a0seq_ack = 0
- self.proto.register ('A', 'BB', self.__handle_ack)
+ self.aseq = [ 0, 0 ]
+ self.aseq_ack = [ 0, 0 ]
+ self.proto.register ('A', 'BBB', self.__handle_ack)
def make_handle (s):
return lambda *args: self.__handle_stats (s, *args)
for (s, f) in self.stats_format.iteritems ():
@@ -99,12 +105,15 @@ class Proto:
self.param = dict (
scale = 1,
tkp = 0, tki = 0, tkd = 0,
+ ta = 1, tsm = 0, tss = 0,
akp = 0, aki = 0, akd = 0,
+ aa = 1, asm = 0, ass = 0,
a0kp = 0, a0ki = 0, a0kd = 0,
+ a0a = 1, a0sm = 0, a0ss = 0,
+ a1kp = 0, a1ki = 0, a1kd = 0,
+ a1a = 1, a1sm = 0, a1ss = 0,
E = 1023, I = 1023, D = 1023,
be = 2048, bs = 0x10, bc = 20,
- ta = 1, aa = 1, a0a = 1,
- tsm = 0, asm = 0, tss = 0, ass = 0, a0sm = 0, a0ss = 0,
c = 1, f = 0x1000,
l = 0x2000,
)
@@ -168,8 +177,7 @@ class Proto:
elif w == 'a':
self.proto.send ('c', 'hh', 0, c)
else:
- assert w == 'a0'
- self.proto.send ('c', 'h', c)
+ self.proto.send ('C', 'Bh', self._index[w], c)
def speed (self, w, s):
"""Speed consign."""
@@ -178,8 +186,7 @@ class Proto:
elif w == 'a':
self.proto.send ('s', 'bb', 0, s)
else:
- assert w == 'a0'
- self.proto.send ('s', 'b', s)
+ self.proto.send ('S', 'Bb', self._index[w], s)
def speed_pos (self, w, offset):
"""Speed controlled position consign."""
@@ -190,9 +197,9 @@ class Proto:
self.mseq += 1
self.proto.send ('s', 'llB', 0, self.dist (offset), self.mseq)
else:
- assert w == 'a0'
- self.a0seq += 1
- self.proto.send ('s', 'lB', offset, self.a0seq)
+ i = self._index[w]
+ self.aseq[i] += 1
+ self.proto.send ('S', 'BlB', i, offset, self.aseq[i])
self.wait (self.finished, auto = True)
def speed_angle (self, w, angle):
@@ -258,21 +265,17 @@ class Proto:
return int (round (x * (1 << 8)))
def f824 (x):
return int (round (x * (1 << 24)))
- self.proto.send ('p', 'cHH', 'p', f88 (p['tkp']), f88 (p['akp']))
- self.proto.send ('p', 'cHH', 'i', f88 (p['tki']), f88 (p['aki']))
- self.proto.send ('p', 'cHH', 'd', f88 (p['tkd']), f88 (p['akd']))
- self.proto.send ('p', 'cH', 'p', f88 (p['a0kp']))
- self.proto.send ('p', 'cH', 'i', f88 (p['a0ki']))
- self.proto.send ('p', 'cH', 'd', f88 (p['a0kd']))
+ for m in ('t', 'a', 'a0', 'a1'):
+ index = self._index [m]
+ self.proto.send ('p', 'cBH', 'p', index, f88 (p[m + 'kp']))
+ self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + 'ki']))
+ self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + 'kd']))
+ self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + 'a']))
+ self.proto.send ('p', 'cBBB', 's', index, p[m + 'sm'], p[m + 'ss'])
self.proto.send ('p', 'cH', 'E', p['E'])
self.proto.send ('p', 'cH', 'I', p['I'])
self.proto.send ('p', 'cH', 'D', p['D'])
self.proto.send ('p', 'cHHH', 'b', p['be'], p['bs'], p['bc'])
- self.proto.send ('p', 'cHH', 'a', f88 (p['ta']), f88 (p['aa']))
- self.proto.send ('p', 'cH', 'a', f88 (p['a0a']))
- self.proto.send ('p', 'cBBBB', 's', p['tsm'], p['asm'], p['tss'],
- p['ass'])
- self.proto.send ('p', 'cBB', 's', p['a0sm'], p['a0ss'])
self.proto.send ('p', 'cL', 'c', f824 (p['c']))
self.proto.send ('p', 'cH', 'f', p['f'])
self.proto.send ('p', 'cH', 'l', p['l'])
@@ -292,11 +295,12 @@ class Proto:
self.stats_line = [ ]
self.stats_count += 1
- def __handle_ack (self, mseq, a0seq):
+ def __handle_ack (self, mseq, a0seq, a1seq):
"""Record current acknowledge level and acknowledge reception."""
self.mseq_ack = mseq & 0x7f
- self.a0seq_ack = a0seq & 0x7f
- self.proto.send ('a', 'BB', mseq, a0seq)
+ self.aseq_ack[0] = a0seq & 0x7f
+ self.aseq_ack[1] = a1seq & 0x7f
+ self.proto.send ('a', 'BBB', mseq, a0seq, a1seq)
def __handle_pos (self, x, y, a):
"""Handle position report."""
@@ -319,7 +323,9 @@ class Proto:
def finished (self):
"""Return True if movement commands have been acknowledged."""
- return self.mseq == self.mseq_ack and self.a0seq == self.a0seq_ack
+ return (self.mseq == self.mseq_ack
+ and self.aseq[0] == self.aseq_ack[0]
+ and self.aseq[1] == self.aseq_ack[1])
def free (self):
"""Coast motors."""
@@ -328,7 +334,7 @@ class Proto:
def reset (self):
"""Coast all motors and reset asserv."""
self.proto.send ('w')
- self.proto.send ('w', 'H', 0)
+ self.proto.send ('W')
self.proto.send ('z')
self.proto.send ('z')
diff --git a/digital/asserv/tools/asserv/mex.py b/digital/asserv/tools/asserv/mex.py
index c673e8e0..b8fb7c31 100644
--- a/digital/asserv/tools/asserv/mex.py
+++ b/digital/asserv/tools/asserv/mex.py
@@ -55,7 +55,7 @@ class Mex:
class PWM (Observable):
"""Motor PWM.
- - pwm: current PWM value (hardware unit).
+ - pwm: current PWM values (hardware unit).
"""
@@ -65,7 +65,7 @@ class Mex:
node.register (ID_PWM, self.__handle)
def __handle (self, msg):
- self.pwm = msg.pop ('hhh')
+ self.pwm = msg.pop ('hhhh')
self.notify ()
class Aux (Observable):
@@ -95,6 +95,6 @@ class Mex:
def __init__ (self, node):
self.position = self.Position (node)
self.pwm = self.PWM (node)
- self.aux = (self.Aux (), )
+ self.aux = (self.Aux (), self.Aux ())
self.__aux_pack = self.Aux.Pack (node, self.aux)