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.c81
1 files changed, 60 insertions, 21 deletions
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 2d21804..d119e4b 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -38,8 +38,8 @@
#include "goto.c"
/** Motor mode :
- * 0 - pwm setup;
- * 1 - speed control;
+ * 0 - pwm setup.
+ * 1 - speed control.
* 2 - position control. */
int8_t motor_mode;
@@ -180,8 +180,23 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
reset ();
break;
/* Commands. */
+ case c ('l', 2):
+ motor_mode = 2;
+ goto_mode = 0;
+ goto_sign = args[0] >> 7;
+ goto_d = v8_to_v16 (args[0], args[1]);
+ if (goto_sign) goto_d = -goto_d;
+ goto_x = postrack_x;
+ goto_y = postrack_y;
+ break;
+ case c ('a', 1):
+ motor_mode = 2;
+ goto_mode = 1;
+ goto_a = args[0];
+ break;
case c ('g', 8):
motor_mode = 2;
+ goto_mode = 2;
goto_x = v8_to_v32 (args[0], args[1], args[2], args[3]);
goto_y = v8_to_v32 (args[4], args[5], args[6], args[7]);
break;
@@ -199,7 +214,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
motor_mode = 1;
speed_left_aim = args[0];
speed_right_aim = args[1];
- speed_max = args[2];
break;
case c ('w', 0):
speed_restart ();
@@ -213,22 +227,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pwm_left = v8_to_v16 (args[0], args[1]);
pwm_right = v8_to_v16 (args[2], args[3]);
break;
- /* Params. */
- case c ('e', 4):
- goto_eps = v8_to_v32 (args[0], args[1], args[2], args[3]);
- break;
- case c ('a', 1):
- speed_acc_cpt = speed_acc = args[0];
- break;
- case c ('p', 2):
- speed_kp = v8_to_v16 (args[0], args[1]);
- break;
- case c ('i', 2):
- speed_ki = v8_to_v16 (args[0], args[1]);
- break;
- case c ('f', 2):
- postrack_set_footing (v8_to_v16 (args[0], args[1]));
- break;
/* Stats. */
case c ('C', 1):
motor_stat_counter_cpt = motor_stat_counter = args[0];
@@ -249,8 +247,49 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
motor_stat_timer_cpt = motor_stat_timer = args[0];
break;
default:
- proto_send0 ('e');
- return;
+ /* Params. */
+ if (cmd == 'p')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('x', 5):
+ postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('y', 5):
+ postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('a', 5):
+ postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('f', 3):
+ postrack_set_footing (v8_to_v16 (args[1], args[2]));
+ break;
+ case c ('e', 5):
+ goto_eps = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('p', 3):
+ speed_kp = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('i', 3):
+ speed_ki = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('a', 2):
+ speed_acc_cpt = speed_acc = args[1];
+ break;
+ case c ('m', 2):
+ speed_max = args[1];
+ break;
+ default:
+ proto_send0 ('e');
+ return;
+ }
+ }
+ else
+ {
+ proto_send0 ('e');
+ return;
+ }
+ break;
}
proto_send (cmd, size, args);
#undef c