summaryrefslogtreecommitdiff
path: root/n/asserv/src/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/main.c')
-rw-r--r--n/asserv/src/main.c73
1 files changed, 27 insertions, 46 deletions
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 72d4b39..61ae706 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -69,10 +69,6 @@ uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3;
static void
main_loop (void);
-/** Handle incoming messages. */
-static void
-proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]);
-
/* -AutoDec */
/* Entry point. */
@@ -86,7 +82,6 @@ main (void)
speed_init ();
postrack_init ();
rs232_init ();
- proto_init (proto_callback, rs232_putc);
proto_send0 ('z');
sei ();
main_loop ();
@@ -119,45 +114,31 @@ main_loop (void)
/* Stats. */
if (motor_stat_speed && !--motor_stat_speed_cpt)
{
- proto_send4 ('U',
- speed_left_e_old >> 8, speed_left_e_old,
- speed_left_int >> 8, speed_left_int);
- proto_send4 ('V',
- speed_right_e_old >> 8, speed_right_e_old,
- speed_right_int >> 8, speed_right_int);
+ proto_send4w ('U', speed_left_e_old, speed_left_int,
+ speed_right_e_old, speed_right_int);
motor_stat_speed_cpt = motor_stat_speed;
}
if (motor_stat_pwm && !--motor_stat_pwm_cpt)
{
- proto_send4 ('W',
- pwm_left >> 8, pwm_left,
- pwm_right >> 8, pwm_right);
+ proto_send2w ('W', pwm_left, pwm_right);
motor_stat_pwm_cpt = motor_stat_pwm;
}
if (motor_stat_timer && !--motor_stat_timer_cpt)
{
- proto_send4 ('T', motor_timer_3, motor_timer_2, motor_timer_1,
- motor_timer_0);
+ proto_send4b ('T', motor_timer_3, motor_timer_2, motor_timer_1,
+ motor_timer_0);
motor_stat_timer_cpt = motor_stat_timer;
}
if (motor_stat_counter && !--motor_stat_counter_cpt)
{
- proto_send4 ('C',
- counter_left >> 8, counter_left,
- counter_right >> 8, counter_right);
+ proto_send2w ('C', counter_left, counter_right);
motor_stat_counter_cpt = motor_stat_counter;
}
if (motor_stat_postrack && !--motor_stat_postrack_cpt)
{
- proto_send4 ('X',
- postrack_x >> 24, postrack_x >> 16,
- postrack_x >> 8, postrack_x);
- proto_send4 ('Y',
- postrack_y >> 24, postrack_y >> 16,
- postrack_y >> 8, postrack_y);
- proto_send4 ('A',
- postrack_a >> 24, postrack_a >> 16,
- postrack_a >> 8, postrack_a);
+ proto_send1d ('X', postrack_x);
+ proto_send1d ('Y', postrack_y);
+ proto_send1d ('A', postrack_a);
motor_stat_postrack_cpt = motor_stat_postrack;
}
/* Misc. */
@@ -171,11 +152,11 @@ main_loop (void)
}
/** Handle incoming messages. */
-static void
-proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[])
+void
+proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
-#define c(cmd, argc) (cmd << 8 | argc)
- switch (c (cmd, argc))
+#define c(cmd, size) (cmd << 8 | size)
+ switch (c (cmd, size))
{
case c ('z', 0):
reset ();
@@ -189,44 +170,44 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[])
case c ('w', 4):
speed_restart ();
motor_mode = 0;
- pwm_left = argv[0] << 8 | argv[1];
- pwm_right = argv[2] << 8 | argv[3];
+ pwm_left = args[0] << 8 | args[1];
+ pwm_right = args[2] << 8 | args[3];
break;
case c ('s', 2):
motor_mode = 1;
- speed_left_aim = argv[0];
- speed_right_aim = argv[1];
+ speed_left_aim = args[0];
+ speed_right_aim = args[1];
break;
case c ('a', 1):
- speed_acc_cpt = speed_acc = argv[0];
+ speed_acc_cpt = speed_acc = args[0];
break;
case c ('p', 2):
- speed_kp = argv[0] << 8 | argv[1];
+ speed_kp = args[0] << 8 | args[1];
break;
case c ('i', 2):
- speed_ki = argv[0] << 8 | argv[1];
+ speed_ki = args[0] << 8 | args[1];
break;
case c ('f', 2):
- postrack_set_footing (argv[0] << 8 | argv[1]);
+ postrack_set_footing (args[0] << 8 | args[1]);
break;
case c ('m', 1):
- motor_stat_speed_cpt = motor_stat_speed = argv[0];
- motor_stat_pwm_cpt = motor_stat_pwm = argv[0];
+ motor_stat_speed_cpt = motor_stat_speed = args[0];
+ motor_stat_pwm_cpt = motor_stat_pwm = args[0];
break;
case c ('c', 1):
- motor_stat_counter_cpt = motor_stat_counter = argv[0];
+ motor_stat_counter_cpt = motor_stat_counter = args[0];
break;
case c ('t', 1):
- motor_stat_timer_cpt = motor_stat_timer = argv[0];
+ motor_stat_timer_cpt = motor_stat_timer = args[0];
break;
case c ('T', 1):
- motor_stat_postrack_cpt = motor_stat_postrack = argv[0];
+ motor_stat_postrack_cpt = motor_stat_postrack = args[0];
break;
default:
proto_send0 ('e');
return;
}
- proto_send (cmd, argc, argv);
+ proto_send (cmd, size, args);
#undef c
}