summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/aux.h27
-rw-r--r--digital/asserv/src/asserv/cs.c34
-rw-r--r--digital/asserv/src/asserv/cs.h10
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c4
-rw-r--r--digital/asserv/src/asserv/main.c73
-rw-r--r--digital/asserv/src/asserv/seq.c2
-rw-r--r--digital/asserv/src/asserv/seq.h2
-rw-r--r--digital/asserv/src/asserv/simu.host.c6
-rw-r--r--digital/asserv/src/asserv/twi_proto.c8
9 files changed, 132 insertions, 34 deletions
diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h
index 98a407b3..cf12aa7d 100644
--- a/digital/asserv/src/asserv/aux.h
+++ b/digital/asserv/src/asserv/aux.h
@@ -26,6 +26,11 @@
* }}} */
#include "cs.h"
+#if AC_ASSERV_AUX_NB
+
+# define AUX_IF(x...) x
+# define AUX_OR_0(x) (x)
+
/** Auxiliary motor informations. */
struct aux_t
{
@@ -73,4 +78,26 @@ aux_traj_find_limit_start (struct aux_t *aux, int8_t speed);
void
aux_traj_update (void);
+#else /* !AC_ASSERV_AUX_NB */
+
+# define AUX_IF(x...)
+# define AUX_OR_0(x) 0
+
+extern inline void
+aux_init (void)
+{
+}
+
+extern inline void
+aux_pos_update (void)
+{
+}
+
+extern inline void
+aux_traj_update (void)
+{
+}
+
+#endif /* !AC_ASSERV_AUX_NB */
+
#endif /* aux_h */
diff --git a/digital/asserv/src/asserv/cs.c b/digital/asserv/src/asserv/cs.c
index c8bebf8a..fe15c5d2 100644
--- a/digital/asserv/src/asserv/cs.c
+++ b/digital/asserv/src/asserv/cs.c
@@ -25,61 +25,71 @@
#include "common.h"
#include "cs.h"
-encoder_t encoder_left, encoder_right, encoder_aux[AC_ASSERV_AUX_NB];
+encoder_t encoder_left, encoder_right;
encoder_corrector_t encoder_right_corrector;
-output_t output_left, output_right, output_aux[AC_ASSERV_AUX_NB];
-
+output_t output_left, output_right;
control_system_polar_t cs_main;
+
+#if AC_ASSERV_AUX_NB
+encoder_t encoder_aux[AC_ASSERV_AUX_NB];
+output_t output_aux[AC_ASSERV_AUX_NB];
control_system_single_t cs_aux[AC_ASSERV_AUX_NB];
+#endif
void
cs_init (void)
{
- uint8_t i;
/* Initialise encoders. */
encoder_init (0, &encoder_left);
encoder_init (1, &encoder_right);
encoder_corrector_init (&encoder_right_corrector);
- encoder_init (2, &encoder_aux[0]);
- encoder_init (3, &encoder_aux[1]);
/* Initialise outputs. */
output_left.max = OUTPUT_MAX;
output_init (0, &output_left);
output_right.max = OUTPUT_MAX;
output_init (1, &output_right);
- output_aux[0].max = OUTPUT_MAX;
- output_init (2, &output_aux[0]);
- output_aux[1].max = OUTPUT_MAX;
- output_init (3, &output_aux[1]);
/* Initialise control system. */
control_system_polar_init (&cs_main);
cs_main.encoder_left = &encoder_left;
cs_main.encoder_right = &encoder_right;
cs_main.output_left = &output_left;
cs_main.output_right = &output_right;
+#if AC_ASSERV_AUX_NB
+ uint8_t i;
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
{
+ /* Initialise encoders. */
+ encoder_init (2 + i, &encoder_aux[i]);
+ /* Initialise outputs. */
+ output_aux[i].max = OUTPUT_MAX;
+ output_init (2 + i, &output_aux[i]);
+ /* Initialise control system. */
control_system_single_init (&cs_aux[i]);
cs_aux[i].encoder = &encoder_aux[i];
cs_aux[i].output = &output_aux[i];
}
+#endif
}
void
cs_update_prepare (void)
{
- uint8_t i;
control_system_polar_update_prepare (&cs_main);
+#if AC_ASSERV_AUX_NB
+ uint8_t i;
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
control_system_single_update_prepare (&cs_aux[i]);
+#endif
}
void
cs_update (void)
{
- uint8_t i;
control_system_polar_update (&cs_main);
+#if AC_ASSERV_AUX_NB
+ uint8_t i;
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
control_system_single_update (&cs_aux[i]);
+#endif
}
diff --git a/digital/asserv/src/asserv/cs.h b/digital/asserv/src/asserv/cs.h
index 5a1737ec..1887ec35 100644
--- a/digital/asserv/src/asserv/cs.h
+++ b/digital/asserv/src/asserv/cs.h
@@ -27,12 +27,16 @@
#include "modules/motor/control_system/control_system.h"
#include "modules/motor/encoder/encoder_corrector.h"
-extern encoder_t encoder_left, encoder_right, encoder_aux[AC_ASSERV_AUX_NB];
+extern encoder_t encoder_left, encoder_right;
extern encoder_corrector_t encoder_right_corrector;
-extern output_t output_left, output_right, output_aux[AC_ASSERV_AUX_NB];
-
+extern output_t output_left, output_right;
extern control_system_polar_t cs_main;
+
+#if AC_ASSERV_AUX_NB
+extern encoder_t encoder_aux[AC_ASSERV_AUX_NB];
+extern output_t output_aux[AC_ASSERV_AUX_NB];
extern control_system_single_t cs_aux[AC_ASSERV_AUX_NB];
+#endif
void
cs_init (void);
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index 70e45dca..0d1753ee 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -156,11 +156,13 @@ eeprom_read_params (void)
&cs_main.pos_alpha,
&cs_main.blocking_detection_alpha,
&output_right);
+#if AC_ASSERV_AUX_NB
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
eeprom_read_params_helper (&loaded, 2 + i, &cs_aux[i].speed,
&cs_aux[i].pos,
&cs_aux[i].blocking_detection,
&output_aux[i]);
+#endif
postrack_set_footing (loaded.postrack_footing);
traj_eps = loaded.traj_eps;
traj_aeps = loaded.traj_aeps;
@@ -205,11 +207,13 @@ eeprom_write_params (void)
&cs_main.pos_alpha,
&cs_main.blocking_detection_alpha,
&output_right);
+#if AC_ASSERV_AUX_NB
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
eeprom_write_params_helper (&p, 2 + i, &cs_aux[i].speed,
&cs_aux[i].pos,
&cs_aux[i].blocking_detection,
&output_aux[i]);
+#endif
p.postrack_footing = postrack_footing;
p.traj_eps = traj_eps;
p.traj_aeps = traj_aeps;
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 5b7d7fef..c1e495e3 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -58,8 +58,10 @@ uint8_t main_stat_counter, main_stat_counter_cpt;
/** Report of position. */
uint8_t main_stat_postrack, main_stat_postrack_cpt;
+#if AC_ASSERV_AUX_NB
/** Report of auxiliary position. */
uint8_t main_stat_aux_pos, main_stat_aux_pos_cpt;
+#endif
/** Statistics about speed control. */
uint8_t main_stat_speed, main_stat_speed_cpt;
@@ -67,8 +69,10 @@ uint8_t main_stat_speed, main_stat_speed_cpt;
/** Statistics about shaft position control. */
uint8_t main_stat_pos, main_stat_pos_cpt;
+#if AC_ASSERV_AUX_NB
/** Statistics about auxiliary shaft position control. */
uint8_t main_stat_pos_aux, main_stat_pos_aux_cpt;
+#endif
/** Statistics about pwm values. */
uint8_t main_stat_pwm, main_stat_pwm_cpt;
@@ -147,24 +151,25 @@ main_loop (void)
main_timer[2] = timer_read ();
/* Sequences. */
seq_update (&seq_main, &cs_main.state);
+#if AC_ASSERV_AUX_NB
seq_update (&seq_aux[0], &cs_aux[0].state);
seq_update (&seq_aux[1], &cs_aux[1].state);
+#endif
/* Stats. */
if (main_sequence_ack
&& (seq_main.ack != seq_main.finish
- || seq_aux[0].ack != seq_aux[0].finish
- || seq_aux[1].ack != seq_aux[1].finish)
+ || AUX_OR_0 (seq_aux[0].ack != seq_aux[0].finish
+ || seq_aux[1].ack != seq_aux[1].finish))
&& !--main_sequence_ack_cpt)
{
- proto_send3b ('A', seq_main.finish,
- seq_aux[0].finish,
- seq_aux[1].finish);
+ proto_sendb ('A', seq_main.finish
+ AUX_IF (, seq_aux[0].finish, seq_aux[1].finish));
main_sequence_ack_cpt = main_sequence_ack;
}
if (main_stat_counter && !--main_stat_counter_cpt)
{
- proto_send4w ('C', encoder_left.cur, encoder_right.cur,
- encoder_aux[0].cur, encoder_aux[1].cur);
+ proto_sendw ('C', encoder_left.cur, encoder_right.cur
+ AUX_IF (, encoder_aux[0].cur, encoder_aux[1].cur));
main_stat_counter_cpt = main_stat_counter;
}
if (main_stat_postrack && !--main_stat_postrack_cpt)
@@ -172,17 +177,19 @@ main_loop (void)
proto_send3d ('X', postrack_x, postrack_y, postrack_a);
main_stat_postrack_cpt = main_stat_postrack;
}
+#if AC_ASSERV_AUX_NB
if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
{
proto_send2w ('Y', aux[0].pos, aux[1].pos);
main_stat_aux_pos_cpt = main_stat_aux_pos;
}
+#endif
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send4b ('S', cs_main.speed_theta.cur >> 8,
- cs_main.speed_alpha.cur >> 8,
- cs_aux[0].speed.cur >> 8,
- cs_aux[1].speed.cur >> 8);
+ proto_sendb ('S', cs_main.speed_theta.cur >> 8,
+ cs_main.speed_alpha.cur >> 8
+ AUX_IF (, cs_aux[0].speed.cur >> 8,
+ cs_aux[1].speed.cur >> 8));
main_stat_speed_cpt = main_stat_speed;
}
if (main_stat_pos && !--main_stat_pos_cpt)
@@ -193,6 +200,7 @@ main_loop (void)
cs_main.pos_alpha.i);
main_stat_pos_cpt = main_stat_pos;
}
+#if AC_ASSERV_AUX_NB
if (main_stat_pos_aux && !--main_stat_pos_aux_cpt)
{
proto_send4w ('Q', cs_aux[0].pos.last_error,
@@ -201,6 +209,7 @@ main_loop (void)
cs_aux[1].pos.i);
main_stat_pos_aux_cpt = main_stat_pos_aux;
}
+#endif
#ifdef HOST
if (main_simu && !--main_simu_cpt)
{
@@ -212,8 +221,8 @@ main_loop (void)
#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
- proto_send4w ('W', output_left.cur, output_right.cur,
- output_aux[0].cur, output_aux[1].cur);
+ proto_sendw ('W', output_left.cur, output_right.cur
+ AUX_IF (, output_aux[0].cur, output_aux[1].cur));
main_stat_pwm_cpt = main_stat_pwm;
}
if (main_stat_timer && !--main_stat_timer_cpt)
@@ -238,12 +247,13 @@ 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 *auxp = 0;
pos_control_t *pos = 0;
speed_control_t *speed = 0;
- control_state_t *state = 0;
blocking_detection_t *bd = 0;
+#if AC_ASSERV_AUX_NB
+ /* Many commands use the first argument as a selector. */
+ struct aux_t *auxp = 0;
+ control_state_t *state = 0;
output_t *output = 0;
seq_t *seq = 0;
if (args[0] < AC_ASSERV_AUX_NB)
@@ -255,6 +265,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
output = &output_aux[args[0]];
seq = &seq_aux[args[0]];
}
+#endif
/* Decode command. */
#define c(cmd, size) (cmd << 8 | size)
switch (c (cmd, size))
@@ -270,6 +281,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
output_set (&output_right, 0);
control_state_set_mode (&cs_main.state, CS_MODE_NONE, 0);
break;
+#if AC_ASSERV_AUX_NB
case c ('W', 0):
/* Set zero auxiliary pwm. */
output_set (&output_aux[0], 0);
@@ -277,6 +289,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
control_state_set_mode (&cs_aux[0].state, CS_MODE_NONE, 0);
control_state_set_mode (&cs_aux[1].state, CS_MODE_NONE, 0);
break;
+#endif
case c ('w', 4):
/* Set pwm.
* - w: left pwm.
@@ -285,6 +298,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
output_set (&output_right, v8_to_v16 (args[2], args[3]));
control_state_set_mode (&cs_main.state, CS_MODE_NONE, 0);
break;
+#if AC_ASSERV_AUX_NB
case c ('W', 3):
/* Set auxiliary pwm.
* - b: aux index.
@@ -293,6 +307,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
output_set (output, v8_to_v16 (args[1], args[2]));
control_state_set_mode (state, CS_MODE_NONE, 0);
break;
+#endif
case c ('c', 4):
/* Add to position consign.
* - w: theta consign offset.
@@ -301,6 +316,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
cs_main.pos_alpha.cons += v8_to_v16 (args[2], args[3]);
control_state_set_mode (&cs_main.state, CS_MODE_POS_CONTROL, 0);
break;
+#if AC_ASSERV_AUX_NB
case c ('C', 3):
/* Add to auxiliary position consign.
* - b: aux index.
@@ -309,6 +325,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pos->cons += v8_to_v16 (args[1], args[2]);
control_state_set_mode (state, CS_MODE_POS_CONTROL, 0);
break;
+#endif
case c ('s', 0):
/* Stop (set zero speed). */
speed_control_set_speed (&cs_main.speed_theta, 0);
@@ -323,6 +340,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_control_set_speed (&cs_main.speed_alpha, args[1]);
control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
break;
+#if AC_ASSERV_AUX_NB
case c ('S', 2):
/* Set auxiliary speed.
* - b: aux index.
@@ -331,6 +349,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_control_set_speed (speed, args[1]);
control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 0);
break;
+#endif
case c ('s', 9):
/* Set speed controlled position consign.
* - d: theta consign offset.
@@ -366,6 +385,7 @@ 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]));
break;
+#if AC_ASSERV_AUX_NB
case c ('S', 6):
/* Set auxiliary speed controlled position consign.
* - b: aux index.
@@ -378,6 +398,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
args[4]));
aux_traj_speed_start (auxp);
break;
+#endif
case c ('f', 2):
/* Go to the wall.
* - b: 0: forward, 1: backward.
@@ -457,6 +478,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
v8_to_v32 (0, args[8], args[9], 0),
args[10]);
break;
+#if AC_ASSERV_AUX_NB
case c ('y', 4):
/* Auxiliary go to position.
* - b: aux index.
@@ -488,6 +510,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
seq_acknowledge (&seq_aux[0], args[1]);
seq_acknowledge (&seq_aux[1], args[2]);
/* no break; */
+#endif
case c ('a', 1):
/* Set main acknoledge.
* - b: main ack sequence number. */
@@ -507,10 +530,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Position stats. */
main_stat_postrack_cpt = main_stat_postrack = args[0];
break;
+#if AC_ASSERV_AUX_NB
case c ('Y', 1):
/* Auxiliary position stats. */
main_stat_aux_pos_cpt = main_stat_aux_pos = args[0];
break;
+#endif
case c ('S', 1):
/* Motor speed control stats. */
main_stat_speed_cpt = main_stat_speed = args[0];
@@ -519,10 +544,12 @@ 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;
+#if AC_ASSERV_AUX_NB
case c ('Q', 1):
/* Auxiliary motor position control stats. */
main_stat_pos_aux_cpt = main_stat_pos_aux = args[0];
break;
+#endif
case c ('W', 1):
/* Pwm stats. */
main_stat_pwm_cpt = main_stat_pwm = args[0];
@@ -558,12 +585,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed = &cs_main.speed_alpha;
bd = &cs_main.blocking_detection_alpha;
break;
+#if AC_ASSERV_AUX_NB
case 0:
case 1:
pos = &cs_aux[args[1]].pos;
speed = &cs_aux[args[1]].speed;
bd = &cs_aux[args[1]].blocking_detection;
break;
+#endif
default:
pos = 0;
speed = 0;
@@ -656,22 +685,28 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('E', 3):
cs_main.pos_theta.e_sat =
cs_main.pos_alpha.e_sat =
+#if AC_ASSERV_AUX_NB
cs_aux[0].pos.e_sat =
cs_aux[1].pos.e_sat =
+#endif
v8_to_v16 (args[1], args[2]);
break;
case c ('I', 3):
cs_main.pos_theta.i_sat =
cs_main.pos_alpha.i_sat =
+#if AC_ASSERV_AUX_NB
cs_aux[0].pos.i_sat =
cs_aux[1].pos.i_sat =
+#endif
v8_to_v16 (args[1], args[2]);
break;
case c ('D', 3):
cs_main.pos_theta.d_sat =
cs_main.pos_alpha.d_sat =
+#if AC_ASSERV_AUX_NB
cs_aux[0].pos.d_sat =
cs_aux[1].pos.d_sat =
+#endif
v8_to_v16 (args[1], args[2]);
break;
case c ('e', 5):
@@ -686,8 +721,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - b: bits: 0000[aux1][aux0][right][left]. */
output_set_reverse (&output_left, (args[1] & 1) ? 1 : 0);
output_set_reverse (&output_right, (args[1] & 2) ? 1 : 0);
+#if AC_ASSERV_AUX_NB
output_set_reverse (&output_aux[0], (args[1] & 4) ? 1 : 0);
output_set_reverse (&output_aux[1], (args[1] & 8) ? 1 : 0);
+#endif
break;
case c ('E', 2):
/* Write to eeprom.
@@ -706,8 +743,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send1w ('l', traj_angle_limit);
proto_send1b ('w', (output_left.reverse ? 1 : 0)
| (output_right.reverse ? 2 : 0)
- | (output_aux[0].reverse ? 4 : 0)
- | (output_aux[1].reverse ? 8 : 0));
+ | AUX_OR_0 ((output_aux[0].reverse ? 4 : 0)
+ | (output_aux[1].reverse ? 8 : 0)));
break;
case c ('P', 2):
/* Print current settings for selected control.
diff --git a/digital/asserv/src/asserv/seq.c b/digital/asserv/src/asserv/seq.c
index 6cbf596f..d9268acd 100644
--- a/digital/asserv/src/asserv/seq.c
+++ b/digital/asserv/src/asserv/seq.c
@@ -27,5 +27,7 @@
seq_t seq_main;
+#if AC_ASSERV_AUX_NB
seq_t seq_aux[AC_ASSERV_AUX_NB];
+#endif
diff --git a/digital/asserv/src/asserv/seq.h b/digital/asserv/src/asserv/seq.h
index 14e35cca..0029586b 100644
--- a/digital/asserv/src/asserv/seq.h
+++ b/digital/asserv/src/asserv/seq.h
@@ -58,8 +58,10 @@ typedef struct seq_t seq_t;
/** Main motors sequence state. */
extern seq_t seq_main;
+#if AC_ASSERV_AUX_NB
/** Auxiliary motor states. */
extern seq_t seq_aux[AC_ASSERV_AUX_NB];
+#endif
/** Start a new command execution, return non zero if this is a new
* sequence. */
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index 82f628ca..9029e002 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -292,9 +292,11 @@ simu_step (void)
* ((double) output_left.cur / (OUTPUT_MAX + 1));
simu_right_model.u = simu_robot->u_max
* ((double) output_right.cur / (OUTPUT_MAX + 1));
+#if AC_ASSERV_AUX_NB
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
simu_aux_model[i].u = simu_robot->u_max
* ((double) output_aux[i].cur / (OUTPUT_MAX + 1));
+#endif
/* Make one step. */
old_left_th = simu_left_model.th;
old_right_th = simu_right_model.th;
@@ -375,6 +377,7 @@ simu_step (void)
encoder_right.diff = encoder_right_new - simu_encoder_right;
encoder_right.cur += encoder_right.diff;
simu_encoder_right = encoder_right_new;
+#if AC_ASSERV_AUX_NB
/* Update auxiliary encoder. */
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
{
@@ -393,6 +396,7 @@ simu_step (void)
simu_encoder_aux[i] = 0;
}
}
+#endif
/* Update sensors. */
if (simu_robot->sensor_update)
simu_robot->sensor_update ();
@@ -432,8 +436,10 @@ simu_send (void)
{
m = mex_msg_new (simu_mex_pwm);
mex_msg_push (m, "hh", output_left.cur, output_right.cur);
+#if AC_ASSERV_AUX_NB
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
mex_msg_push (m, "h", output_aux[i].cur);
+#endif
mex_node_send (m);
output_left_sent = output_left.cur;
output_right_sent = output_right.cur;
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 0363d68f..c2931691 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -72,13 +72,15 @@ twi_proto_update (void)
while ((read_data = twi_slave_poll (buf, sizeof (buf))))
twi_proto_callback (buf, read_data);
/* Update status. */
- u8 status_with_crc[16];
+ u8 status_with_crc[12 + AC_ASSERV_AUX_NB * 2];
u8 *status = &status_with_crc[1];
status[0] = 0
+#if AC_ASSERV_AUX_NB
| (control_state_is_blocked (&cs_aux[1].state) ? (1 << 7) : 0)
| (control_state_is_finished (&cs_aux[1].state) ? (1 << 6) : 0)
| (control_state_is_blocked (&cs_aux[0].state) ? (1 << 5) : 0)
| (control_state_is_finished (&cs_aux[0].state) ? (1 << 4) : 0)
+#endif
| (cs_main.speed_theta.cur < 0 ? (1 << 3) : 0)
| (cs_main.speed_theta.cur > 0 ? (1 << 2) : 0)
| (control_state_is_blocked (&cs_main.state) ? (1 << 1) : 0)
@@ -93,10 +95,12 @@ twi_proto_update (void)
status[8] = v32_to_v8 (postrack_y, 1);
status[9] = v32_to_v8 (postrack_a, 2);
status[10] = v32_to_v8 (postrack_a, 1);
+#if AC_ASSERV_AUX_NB
status[11] = v16_to_v8 (aux[0].pos, 1);
status[12] = v16_to_v8 (aux[0].pos, 0);
status[13] = v16_to_v8 (aux[1].pos, 1);
status[14] = v16_to_v8 (aux[1].pos, 0);
+#endif
/* Compute CRC. */
status_with_crc[0] = crc_compute (&status_with_crc[1],
sizeof (status_with_crc) - 1);
@@ -213,6 +217,7 @@ twi_proto_callback (u8 *buf, u8 size)
v8_to_v32 (0, buf[8], buf[9], 0),
buf[10]);
break;
+#if AC_ASSERV_AUX_NB
case c ('b', 3):
/* Move the aux0.
* - w: new position.
@@ -249,6 +254,7 @@ twi_proto_callback (u8 *buf, u8 size)
else
buf[0] = 0;
break;
+#endif
case c ('p', x):
/* Set parameters. */
if (twi_proto_params (&buf[2], size - 2) != 0)