summaryrefslogtreecommitdiff
path: root/digital/asserv/src/asserv/cs.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv/src/asserv/cs.c')
-rw-r--r--digital/asserv/src/asserv/cs.c34
1 files changed, 22 insertions, 12 deletions
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
}