summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv/src/asserv/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv/src/asserv/main.c')
-rw-r--r--digital/asserv/src/asserv/main.c353
1 files changed, 179 insertions, 174 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 677fea84..5d91d90e 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -32,16 +32,14 @@
#include "io.h"
#include "misc.h"
-#include "state.h"
-#include "counter.h"
-#include "pwm.h"
-#include "pos.h"
-#include "speed.h"
+#include "cs.h"
#include "postrack.h"
#include "traj.h"
#include "aux.h"
+#include "seq.h"
+
#include "twi_proto.h"
#include "eeprom.h"
@@ -105,13 +103,11 @@ main (int argc, char **argv)
PORTF = 0xfc;
PORTG = 0x18;
LED_SETUP;
- pwm_init ();
timer_init ();
- counter_init ();
uart0_init ();
twi_proto_init ();
postrack_init ();
- speed_init ();
+ cs_init ();
traj_init ();
aux_init ();
eeprom_read_params ();
@@ -126,43 +122,49 @@ main (int argc, char **argv)
static void
main_loop (void)
{
+ main_timer[3] = timer_read ();
+ /* Compute absolute position. */
+ postrack_update ();
+ aux_pos_update ();
+ main_timer[4] = timer_read ();
+ /* Compute trajectory. */
+ traj_update ();
+ aux_traj_update ();
+ /* Prepare control system. */
+ cs_update_prepare ();
main_timer[5] = timer_read ();
+ /* Wait for next cycle. */
timer_wait ();
- /* Counter update. */
- counter_update ();
+ /* Encoder update. */
+ encoder_update ();
+ encoder_corrector_update (&encoder_right_corrector, &encoder_right);
main_timer[0] = timer_read ();
- /* Position control. */
- pos_update ();
+ /* Control system update. */
+ cs_update ();
main_timer[1] = timer_read ();
/* Pwm setup. */
- pwm_update ();
+ output_update ();
main_timer[2] = timer_read ();
- /* Compute absolute position. */
- postrack_update ();
- aux_pos_update ();
- /* Compute trajectory. */
- if (state_main.mode >= MODE_TRAJ)
- traj_update ();
- aux_traj_update ();
- /* Speed control. */
- speed_update ();
- main_timer[3] = timer_read ();
+ /* Sequences. */
+ seq_update (&seq_main, &cs_main.state);
+ seq_update (&seq_aux[0], &cs_aux[0].state);
+ seq_update (&seq_aux[1], &cs_aux[1].state);
/* Stats. */
if (main_sequence_ack
- && (state_main.sequence_ack != state_main.sequence_finish
- || state_aux[0].sequence_ack != state_aux[0].sequence_finish
- || state_aux[1].sequence_ack != state_aux[1].sequence_finish)
+ && (seq_main.ack != seq_main.finish
+ || seq_aux[0].ack != seq_aux[0].finish
+ || seq_aux[1].ack != seq_aux[1].finish)
&& !--main_sequence_ack_cpt)
{
- proto_send3b ('A', state_main.sequence_finish,
- state_aux[0].sequence_finish,
- state_aux[1].sequence_finish);
+ proto_send3b ('A', seq_main.finish,
+ 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', counter_left, counter_right,
- counter_aux[0], counter_aux[1]);
+ proto_send4w ('C', encoder_left.cur, encoder_right.cur,
+ encoder_aux[0].cur, encoder_aux[1].cur);
main_stat_counter_cpt = main_stat_counter;
}
if (main_stat_postrack && !--main_stat_postrack_cpt)
@@ -177,20 +179,26 @@ main_loop (void)
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send4b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8,
- speed_aux[0].cur >> 8, speed_aux[1].cur >> 8);
+ 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);
main_stat_speed_cpt = main_stat_speed;
}
if (main_stat_pos && !--main_stat_pos_cpt)
{
- proto_send4w ('P', pos_theta.e_old, pos_theta.i,
- pos_alpha.e_old, pos_alpha.i);
+ proto_send4w ('P', cs_main.pos_theta.last_error,
+ cs_main.pos_theta.i,
+ cs_main.pos_alpha.last_error,
+ cs_main.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);
+ proto_send4w ('Q', cs_aux[0].pos.last_error,
+ cs_aux[0].pos.i,
+ cs_aux[1].pos.last_error,
+ cs_aux[1].pos.i);
main_stat_pos_aux_cpt = main_stat_pos_aux;
}
#ifdef HOST
@@ -204,8 +212,8 @@ main_loop (void)
#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
- proto_send4w ('W', pwm_left.cur, pwm_right.cur,
- pwm_aux[0].cur, pwm_aux[1].cur);
+ proto_send4w ('W', output_left.cur, output_right.cur,
+ output_aux[0].cur, output_aux[1].cur);
main_stat_pwm_cpt = main_stat_pwm;
}
if (main_stat_timer && !--main_stat_timer_cpt)
@@ -232,17 +240,20 @@ 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;
- struct pwm_t *pwm = 0;
- struct pos_t *pos = 0;
- struct speed_t *speed = 0;
- struct state_t *state = 0;
+ pos_control_t *pos = 0;
+ speed_control_t *speed = 0;
+ control_state_t *state = 0;
+ blocking_detection_t *bd = 0;
+ output_t *output = 0;
+ seq_t *seq = 0;
if (args[0] < AC_ASSERV_AUX_NB)
{
auxp = &aux[args[0]];
- pwm = &pwm_aux[args[0]];
- pos = &pos_aux[args[0]];
- speed = &speed_aux[args[0]];
- state = &state_aux[args[0]];
+ pos = &cs_aux[args[0]].pos;
+ speed = &cs_aux[args[0]].speed;
+ state = &cs_aux[args[0]].state;
+ output = &output_aux[args[0]];
+ seq = &seq_aux[args[0]];
}
/* Decode command. */
#define c(cmd, size) (cmd << 8 | size)
@@ -255,120 +266,105 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Commands. */
case c ('w', 0):
/* Set zero pwm. */
- pos_reset (&pos_theta);
- pos_reset (&pos_alpha);
- state_main.mode = MODE_PWM;
- pwm_set (&pwm_left, 0);
- pwm_set (&pwm_right, 0);
+ output_set (&output_left, 0);
+ output_set (&output_right, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_NONE, 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);
+ output_set (&output_aux[0], 0);
+ output_set (&output_aux[1], 0);
+ 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;
case c ('w', 4):
/* Set pwm.
* - w: left pwm.
* - w: right pwm. */
- 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]));
+ output_set (&output_left, v8_to_v16 (args[0], args[1]));
+ output_set (&output_right, v8_to_v16 (args[2], args[3]));
+ control_state_set_mode (&cs_main.state, CS_MODE_NONE, 0);
break;
case c ('W', 3):
/* Set auxiliary pwm.
* - b: aux index.
* - w: pwm. */
if (!auxp) { proto_send0 ('?'); return; }
- pos_reset (pos);
- state->mode = MODE_PWM;
- pwm_set (pwm, v8_to_v16 (args[1], args[2]));
+ output_set (output, v8_to_v16 (args[1], args[2]));
+ control_state_set_mode (state, CS_MODE_NONE, 0);
break;
case c ('c', 4):
/* Add to position consign.
* - w: theta consign offset.
* - w: alpha consign offset. */
- state_main.mode = MODE_POS;
- state_main.variant = 0;
- pos_theta.cons += v8_to_v16 (args[0], args[1]);
- pos_alpha.cons += v8_to_v16 (args[2], args[3]);
+ cs_main.pos_theta.cons += v8_to_v16 (args[0], args[1]);
+ 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;
case c ('C', 3):
/* Add to auxiliary position consign.
* - b: aux index.
* - w: consign offset. */
if (!auxp) { proto_send0 ('?'); return; }
- state->mode = MODE_POS;
- state->variant = 0;
pos->cons += v8_to_v16 (args[1], args[2]);
+ control_state_set_mode (state, CS_MODE_POS_CONTROL, 0);
break;
case c ('s', 0):
/* Stop (set zero speed). */
- state_main.mode = MODE_SPEED;
- state_main.variant = 0;
- speed_theta.use_pos = speed_alpha.use_pos = 0;
- speed_theta.cons = 0;
- speed_alpha.cons = 0;
+ speed_control_set_speed (&cs_main.speed_theta, 0);
+ speed_control_set_speed (&cs_main.speed_alpha, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
break;
case c ('s', 2):
/* Set speed.
* - b: theta speed.
* - b: alpha speed. */
- state_main.mode = MODE_SPEED;
- state_main.variant = 0;
- speed_theta.use_pos = speed_alpha.use_pos = 0;
- speed_theta.cons = args[0] << 8;
- speed_alpha.cons = args[1] << 8;
+ speed_control_set_speed (&cs_main.speed_theta, args[0]);
+ speed_control_set_speed (&cs_main.speed_alpha, args[1]);
+ control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
break;
case c ('S', 2):
/* Set auxiliary speed.
* - b: aux index.
* - b: speed. */
if (!auxp) { proto_send0 ('?'); return; }
- state->mode = MODE_SPEED;
- state->variant = 0;
- speed->use_pos = 0;
- speed->cons = args[1] << 8;
+ speed_control_set_speed (speed, args[1]);
+ control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 0);
break;
case c ('s', 9):
/* Set speed controlled position consign.
* - d: theta consign offset.
* - d: alpha consign offset.
* - b: sequence number. */
- if (args[8] == state_main.sequence)
+ if (!seq_start (&seq_main, args[8]))
break;
- speed_theta.use_pos = speed_alpha.use_pos = 1;
- speed_theta.pos_cons = pos_theta.cons;
- speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
- speed_alpha.pos_cons = pos_alpha.cons;
- speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]);
- state_start (&state_main, MODE_SPEED, args[8]);
+ speed_control_pos_offset (&cs_main.speed_theta,
+ v8_to_v32 (args[0], args[1], args[2],
+ args[3]));
+ speed_control_pos_offset (&cs_main.speed_alpha,
+ v8_to_v32 (args[4], args[5], args[6],
+ args[7]));
+ control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
break;
case c ('l', 5):
/* Set linear speed controlled position consign.
* - d: consign offset.
* - b: sequence number. */
- if (args[4] == state_main.sequence)
+ if (!seq_start (&seq_main, args[4]))
break;
- speed_theta.use_pos = speed_alpha.use_pos = 1;
- speed_theta.pos_cons = pos_theta.cons;
- speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
- speed_alpha.pos_cons = pos_alpha.cons;
- state_start (&state_main, MODE_SPEED, args[4]);
+ speed_control_pos_offset (&cs_main.speed_theta,
+ v8_to_v32 (args[0], args[1], args[2], args[3]));
+ speed_control_pos_offset (&cs_main.speed_alpha, 0);
+ traj_speed_start ();
break;
case c ('a', 5):
/* Set angular speed controlled position consign.
* - d: angle offset.
* - b: sequence number. */
- if (args[4] == state_main.sequence)
+ if (!seq_start (&seq_main, args[4]))
break;
traj_angle_offset_start (v8_to_v32 (args[0], args[1], args[2],
- args[3]), args[4]);
+ args[3]));
break;
case c ('S', 6):
/* Set auxiliary speed controlled position consign.
@@ -376,29 +372,28 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - d: consign offset.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[5] == state->sequence)
+ if (!seq_start (seq, args[5]))
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]);
+ speed_control_pos_offset (speed, v8_to_v32 (args[1], args[2], args[3],
+ args[4]));
+ aux_traj_speed_start (auxp);
break;
case c ('f', 2):
/* Go to the wall.
* - b: 0: forward, 1: backward.
* - b: sequence number. */
- if (args[1] == state_main.sequence)
+ if (!seq_start (&seq_main, args[1]))
break;
- traj_ftw_start (args[0], args[1]);
+ traj_ftw_start (args[0]);
break;
case c ('f', 3):
/* Go to the wall, using center with a delay.
* - b: 0: forward, 1: backward.
* - b: delay.
* - b: sequence number. */
- if (args[1] == state_main.sequence)
+ if (!seq_start (&seq_main, args[2]))
break;
- traj_ftw_start_center (args[0], args[1], args[2]);
+ traj_ftw_start_center (args[0], args[1]);
break;
case c ('f', 12):
/* Push the wall.
@@ -408,7 +403,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: init_a, f0.16.
* - b: sequence number. */
{
- if (args[11] == state_main.sequence)
+ if (!seq_start (&seq_main, args[11]))
break;
int32_t angle;
if (args[9] == 0xff && args[10] == 0xff)
@@ -418,15 +413,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
traj_ptw_start (args[0],
v8_to_v32 (args[1], args[2], args[3], args[4]),
v8_to_v32 (args[5], args[6], args[7], args[8]),
- angle, args[11]);
+ angle);
}
break;
case c ('F', 1):
/* Go to the dispenser.
* - b: sequence number. */
- if (args[0] == state_main.sequence)
+ if (!seq_start (&seq_main, args[0]))
break;
- traj_gtd_start (args[0]);
+ traj_gtd_start ();
break;
case c ('x', 10):
/* Go to position.
@@ -434,20 +429,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - d: y, f24.8.
* - b: backward (see traj.h).
* - b: sequence number. */
- if (args[9] == state_main.sequence)
+ if (!seq_start (&seq_main, args[9]))
break;
traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
v8_to_v32 (args[4], args[5], args[6], args[7]),
- args[8], args[9]);
+ args[8]);
break;
case c ('x', 3):
/* Go to angle.
* - d: a, f0.16.
* - b: sequence number. */
- if (args[2] == state_main.sequence)
+ if (!seq_start (&seq_main, args[2]))
break;
- traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0),
- args[2]);
+ traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0));
break;
case c ('x', 12):
/* Go to position, then angle.
@@ -456,12 +450,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: a, f0.16.
* - b: backward (see traj.h).
* - b: sequence number. */
- if (args[11] == state_main.sequence)
+ if (!seq_start (&seq_main, args[11]))
break;
traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
v8_to_v32 (args[4], args[5], args[6], args[7]),
v8_to_v32 (0, args[8], args[9], 0),
- args[10], args[11]);
+ args[10]);
break;
case c ('y', 4):
/* Auxiliary go to position.
@@ -469,9 +463,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: pos, i16.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[3] == state->sequence)
+ if (!seq_start (seq, args[3]))
break;
- aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]);
+ aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]));
break;
case c ('y', 3):
/* Auxiliary find zero.
@@ -479,25 +473,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - b: speed.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[2] == state->sequence)
+ if (!seq_start (seq, args[2]))
break;
if (args[0] == 0)
- aux_traj_find_limit_start (auxp, args[1], args[2]);
+ aux_traj_find_limit_start (auxp, args[1]);
else
- aux_traj_find_zero_reverse_start (auxp, args[1], args[2]);
+ aux_traj_find_zero_reverse_start (auxp, args[1]);
break;
case c ('a', 3):
/* Set all acknoledge.
* - b: main ack sequence number.
* - 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]);
+ seq_acknowledge (&seq_aux[0], args[1]);
+ seq_acknowledge (&seq_aux[1], args[2]);
/* no break; */
case c ('a', 1):
/* Set main acknoledge.
* - b: main ack sequence number. */
- state_acknowledge (&state_main, args[0]);
+ seq_acknowledge (&seq_main, args[0]);
break;
/* Stats.
* - b: interval between stats. */
@@ -555,21 +549,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
switch (args[1])
{
case 't':
- pos = &pos_theta;
- speed = &speed_theta;
+ pos = &cs_main.pos_theta;
+ speed = &cs_main.speed_theta;
+ bd = &cs_main.blocking_detection_theta;
break;
case 'a':
- pos = &pos_alpha;
- speed = &speed_alpha;
+ pos = &cs_main.pos_alpha;
+ speed = &cs_main.speed_alpha;
+ bd = &cs_main.blocking_detection_alpha;
break;
case 0:
case 1:
- pos = &pos_aux[args[1]];
- speed = &speed_aux[args[1]];
+ pos = &cs_aux[args[1]].pos;
+ speed = &cs_aux[args[1]].speed;
+ bd = &cs_aux[args[1]].blocking_detection;
break;
default:
pos = 0;
speed = 0;
+ bd = 0;
break;
}
switch (c (args[0], size))
@@ -598,8 +596,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('c', 5):
/* Set right counter correction factor.
* - d: factor (f8.24). */
- counter_right_correction = v8_to_v32 (args[1], args[2],
- args[3], args[4]);
+ encoder_corrector_set_correction (&encoder_right_corrector,
+ v8_to_v32 (args[1], args[2],
+ args[3], args[4]));
break;
case c ('f', 3):
/* Set footing.
@@ -649,19 +648,31 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - w: error limit.
* - w: speed limit.
* - b: counter limit. */
- if (!pos) { proto_send0 ('?'); return; }
- pos->blocked_error_limit = v8_to_v16 (args[2], args[3]);
- pos->blocked_speed_limit = v8_to_v16 (args[4], args[5]);
- pos->blocked_counter_limit = args[6];
+ if (!bd) { proto_send0 ('?'); return; }
+ bd->error_limit = v8_to_v16 (args[2], args[3]);
+ bd->speed_limit = v8_to_v16 (args[4], args[5]);
+ bd->counter_limit = args[6];
break;
case c ('E', 3):
- pos_e_sat = v8_to_v16 (args[1], args[2]);
+ cs_main.pos_theta.e_sat =
+ cs_main.pos_alpha.e_sat =
+ cs_aux[0].pos.e_sat =
+ cs_aux[1].pos.e_sat =
+ v8_to_v16 (args[1], args[2]);
break;
case c ('I', 3):
- pos_i_sat = v8_to_v16 (args[1], args[2]);
+ cs_main.pos_theta.i_sat =
+ cs_main.pos_alpha.i_sat =
+ cs_aux[0].pos.i_sat =
+ cs_aux[1].pos.i_sat =
+ v8_to_v16 (args[1], args[2]);
break;
case c ('D', 3):
- pos_d_sat = v8_to_v16 (args[1], args[2]);
+ cs_main.pos_theta.d_sat =
+ cs_main.pos_alpha.d_sat =
+ cs_aux[0].pos.d_sat =
+ cs_aux[1].pos.d_sat =
+ v8_to_v16 (args[1], args[2]);
break;
case c ('e', 5):
traj_eps = v8_to_v16 (args[1], args[2]);
@@ -673,7 +684,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('w', 2):
/* Set PWM direction.
* - b: bits: 0000[aux1][aux0][right][left]. */
- pwm_set_reverse (args[1]);
+ output_set_reverse (&output_left, (args[1] & 1) ? 1 : 0);
+ output_set_reverse (&output_right, (args[1] & 2) ? 1 : 0);
+ output_set_reverse (&output_aux[0], (args[1] & 4) ? 1 : 0);
+ output_set_reverse (&output_aux[1], (args[1] & 8) ? 1 : 0);
break;
case c ('E', 2):
/* Write to eeprom.
@@ -686,38 +700,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('P', 1):
/* Print current settings. */
proto_send1b ('E', EEPROM_KEY);
- proto_send1d ('c', counter_right_correction);
+ proto_send1d ('c', encoder_right_corrector.correction);
proto_send1w ('f', postrack_footing);
- proto_send2w ('a', speed_theta.acc, speed_alpha.acc);
- proto_send2b ('s', speed_theta.max, speed_theta.slow);
- proto_send2b ('s', speed_alpha.max, speed_alpha.slow);
- proto_send3w ('b', pos_theta.blocked_error_limit,
- pos_theta.blocked_speed_limit,
- pos_theta.blocked_counter_limit);
- proto_send3w ('b', pos_alpha.blocked_error_limit,
- pos_alpha.blocked_speed_limit,
- pos_alpha.blocked_counter_limit);
- 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_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_send3w ('b', pos_aux[0].blocked_error_limit,
- pos_aux[0].blocked_speed_limit,
- pos_aux[0].blocked_counter_limit);
- proto_send3w ('b', pos_aux[1].blocked_error_limit,
- pos_aux[1].blocked_speed_limit,
- pos_aux[1].blocked_counter_limit);
- 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);
proto_send2w ('e', traj_eps, traj_aeps);
proto_send1w ('l', traj_angle_limit);
- proto_send1b ('w', pwm_reverse);
+ 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));
+ break;
+ case c ('P', 2):
+ /* Print current settings for selected control.
+ * - b: index. */
+ proto_send1b ('E', EEPROM_KEY);
+ proto_send1w ('a', speed->acc);
+ proto_send2b ('s', speed->max, speed->slow);
+ proto_send3w ('b', bd->error_limit, bd->speed_limit,
+ bd->counter_limit);
+ proto_send1w ('p', pos->kp);
+ proto_send1w ('i', pos->ki);
+ proto_send1w ('d', pos->kd);
+ proto_send1w ('E', pos->e_sat);
+ proto_send1w ('I', pos->i_sat);
+ proto_send1w ('D', pos->d_sat);
break;
default:
proto_send0 ('?');