summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/avrconfig.h10
-rw-r--r--n/asserv/src/main.c73
-rw-r--r--n/asserv/src/test_dsp.c107
-rw-r--r--n/asserv/src/test_pwm.c11
4 files changed, 71 insertions, 130 deletions
diff --git a/n/asserv/src/avrconfig.h b/n/asserv/src/avrconfig.h
index bb19576..769257c 100644
--- a/n/asserv/src/avrconfig.h
+++ b/n/asserv/src/avrconfig.h
@@ -54,9 +54,11 @@
#define AC_RS232_PORT 1
/* proto - Protocol module. */
-/** Maximum argument number. */
-#define AC_PROTO_MAX_ARGS 4
-/** Protocol arguments type. */
-#define AC_PROTO_ARG_TYPE uint8_t
+/** Maximum argument size. */
+#define AC_PROTO_ARGS_MAX_SIZE 8
+/** Callback function name. */
+#define AC_PROTO_CALLBACK proto_callback
+/** Putchar function name. */
+#define AC_PROTO_PUTC rs232_putc
#endif /* avrconfig_h */
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
}
diff --git a/n/asserv/src/test_dsp.c b/n/asserv/src/test_dsp.c
index 27ddc9a..870980d 100644
--- a/n/asserv/src/test_dsp.c
+++ b/n/asserv/src/test_dsp.c
@@ -33,7 +33,7 @@
/* -AutoDec */
void
-proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
+proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
int16_t a;
uint16_t b;
@@ -43,21 +43,21 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
int32_t al, bl, rl;
uint32_t wl[] = { 0xa66a6aa6, 0xffffffff };
static int32_t sa, sb;
- switch (c | argc << 8)
+ switch (cmd | size << 8)
{
case 'a' | 4 << 8:
- sa = argv[0];
- sa = sa << 8 | argv[1];
- sa = sa << 8 | argv[2];
- sa = sa << 8 | argv[3];
- proto_send ('a', argc, argv);
+ sa = args[0];
+ sa = sa << 8 | args[1];
+ sa = sa << 8 | args[2];
+ sa = sa << 8 | args[3];
+ proto_send ('a', size, args);
break;
case 'b' | 4 << 8:
- sb = argv[0];
- sb = sb << 8 | argv[1];
- sb = sb << 8 | argv[2];
- sb = sb << 8 | argv[3];
- proto_send ('b', argc, argv);
+ sb = args[0];
+ sb = sb << 8 | args[1];
+ sb = sb << 8 | args[2];
+ sb = sb << 8 | args[3];
+ proto_send ('b', size, args);
break;
case 'm' | 0 << 8:
for (i = 16; i > 0; i--)
@@ -66,33 +66,23 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
for (j = 16; j >= 0; j--)
{
b = w >> j;
- proto_send4 ('m',
- (a >> 8) & 0xff, a & 0xff,
- (b >> 8) & 0xff, b & 0xff);
+ proto_send2w ('m', a, b);
r = dsp_mul_i16f88 (a, b);
- proto_send2 ('r',
- (r >> 8) & 0xff, r & 0xff);
+ proto_send1w ('r', r);
r = dsp_mul_i16f88 (-a, b);
- proto_send2 ('R',
- (r >> 8) & 0xff, r & 0xff);
+ proto_send1w ('R', r);
}
}
break;
case 'M' | 1 << 8:
rl = dsp_mul_f824 (sa, sb);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1d ('r', rl);
break;
case 'c' | 1 << 8:
rl = dsp_cos (sa);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1w ('r', rl);
rl = dsp_sin (sa);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1w ('r', rl);
break;
case 'M' | 0 << 8:
for (k = 0; k < 2; k++)
@@ -103,28 +93,16 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
for (j = 32; j >= 0; j--)
{
bl = wl[k] >> j;
- proto_send4 ('A',
- (al >> 24) & 0xff, (al >> 16) & 0xff,
- (al >> 8) & 0xff, al & 0xff);
- proto_send4 ('B',
- (bl >> 24) & 0xff, (bl >> 16) & 0xff,
- (bl >> 8) & 0xff, bl & 0xff);
+ proto_send1w ('A', al);
+ proto_send1w ('B', bl);
rl = dsp_mul_f824 (al, bl);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1w ('r', rl);
rl = dsp_mul_f824 (-al, bl);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1w ('r', rl);
rl = dsp_mul_f824 (al, -bl);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1w ('r', rl);
rl = dsp_mul_f824 (-al, -bl);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1w ('r', rl);
}
}
}
@@ -138,28 +116,16 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
for (j = 32; j >= 0; j--)
{
bl = wl[k] >> j;
- proto_send4 ('d',
- (al >> 24) & 0xff, (al >> 16) & 0xff,
- (al >> 8) & 0xff, al & 0xff);
- proto_send4 ('v',
- (bl >> 24) & 0xff, (bl >> 16) & 0xff,
- (bl >> 8) & 0xff, bl & 0xff);
+ proto_send1d ('d', al);
+ proto_send1d ('v', bl);
rl = dsp_div_f824 (al, bl);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1d ('r', rl);
rl = dsp_div_f824 (-al, bl);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1d ('r', rl);
rl = dsp_div_f824 (al, -bl);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1d ('r', rl);
rl = dsp_div_f824 (-al, -bl);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1d ('r', rl);
}
}
}
@@ -167,17 +133,11 @@ proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
case 'c' | 0 << 8:
for (al = 0; al < (1L << 24) + (1L << 21); al += 32 << 8)
{
- proto_send4 ('c',
- (al >> 24) & 0xff, (al >> 16) & 0xff,
- (al >> 8) & 0xff, al & 0xff);
+ proto_send1d ('c', al);
rl = dsp_cos (al);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1d ('r', rl);
rl = dsp_sin (al);
- proto_send4 ('r',
- (rl >> 24) & 0xff, (rl >> 16) & 0xff,
- (rl >> 8) & 0xff, rl & 0xff);
+ proto_send1d ('r', rl);
}
break;
case 'z' | 0 << 8:
@@ -192,7 +152,6 @@ int
main (void)
{
rs232_init ();
- proto_init (proto_callback, rs232_putc);
rs232_putc ('!');
rs232_putc ('z');
rs232_putc ('d');
diff --git a/n/asserv/src/test_pwm.c b/n/asserv/src/test_pwm.c
index 37d6adf..ef3b413 100644
--- a/n/asserv/src/test_pwm.c
+++ b/n/asserv/src/test_pwm.c
@@ -33,22 +33,22 @@
#include "pwm.c"
void
-proto_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
+proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
- switch (c | argc << 8)
+ switch (cmd | size << 8)
{
case 'l' | 2 << 8:
- pwm_left = argv[0] << 8 | argv[1];
+ pwm_left = args[0] << 8 | args[1];
break;
case 'r' | 2 << 8:
- pwm_right = argv[0] << 8 | argv[1];
+ pwm_right = args[0] << 8 | args[1];
break;
default:
proto_send0 ('e');
return;
}
pwm_update ();
- proto_send (c, argc, argv);
+ proto_send (cmd, size, args);
}
int
@@ -56,7 +56,6 @@ main (void)
{
rs232_init ();
pwm_init ();
- proto_init (proto_callback, rs232_putc);
rs232_putc ('!');
rs232_putc ('z');
rs232_putc ('\r');