2 * Copyright Droids Corporation (2009)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
29 #include <aversive/error.h>
34 #include <clock_time.h>
36 #include <encoders_spi.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <trajectory_manager_utils.h>
43 #include <vect_base.h>
46 #include <obstacle_avoidance.h>
47 #include <blocking_detection_manager.h>
48 #include <robot_system.h>
49 #include <position_manager.h>
53 #include <parse_string.h>
54 #include <parse_num.h>
59 #include "strat_utils.h"
60 #include "strat_base.h"
63 #include "../common/i2c_commands.h"
64 #include "i2c_protocol.h"
66 /**********************************************************/
67 /* Traj_Speeds for trajectory_manager */
69 /* this structure is filled when cmd_traj_speed is parsed successfully */
70 struct cmd_traj_speed_result {
76 /* function called when cmd_traj_speed is parsed successfully */
77 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
79 struct cmd_traj_speed_result * res = parsed_result;
81 if (!strcmp_P(res->arg1, PSTR("angle"))) {
82 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
84 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
85 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
87 /* else it is a "show" */
89 printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
90 mainboard.traj.a_speed,
91 mainboard.traj.d_speed);
94 prog_char str_traj_speed_arg0[] = "traj_speed";
95 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
96 prog_char str_traj_speed_arg1[] = "angle#distance";
97 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
98 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT);
100 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
101 parse_pgm_inst_t cmd_traj_speed = {
102 .f = cmd_traj_speed_parsed, /* function to call */
103 .data = NULL, /* 2nd arg of func */
104 .help_str = help_traj_speed,
105 .tokens = { /* token list, NULL terminated */
106 (prog_void *)&cmd_traj_speed_arg0,
107 (prog_void *)&cmd_traj_speed_arg1,
108 (prog_void *)&cmd_traj_speed_s,
115 prog_char str_traj_speed_show_arg[] = "show";
116 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
118 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
119 parse_pgm_inst_t cmd_traj_speed_show = {
120 .f = cmd_traj_speed_parsed, /* function to call */
121 .data = NULL, /* 2nd arg of func */
122 .help_str = help_traj_speed_show,
123 .tokens = { /* token list, NULL terminated */
124 (prog_void *)&cmd_traj_speed_arg0,
125 (prog_void *)&cmd_traj_speed_show_arg,
130 /**********************************************************/
131 /* Traj_Accs for trajectory_manager */
133 /* this structure is filled when cmd_traj_acc is parsed successfully */
134 struct cmd_traj_acc_result {
140 /* function called when cmd_traj_acc is parsed successfully */
141 static void cmd_traj_acc_parsed(void *parsed_result, void *data)
143 struct cmd_traj_acc_result * res = parsed_result;
145 if (!strcmp_P(res->arg1, PSTR("angle"))) {
146 trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s);
148 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
149 trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc);
151 /* else it is a "show" */
153 printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
154 mainboard.traj.a_acc,
155 mainboard.traj.d_acc);
158 prog_char str_traj_acc_arg0[] = "traj_acc";
159 parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0);
160 prog_char str_traj_acc_arg1[] = "angle#distance";
161 parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1);
162 parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT);
164 prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager";
165 parse_pgm_inst_t cmd_traj_acc = {
166 .f = cmd_traj_acc_parsed, /* function to call */
167 .data = NULL, /* 2nd arg of func */
168 .help_str = help_traj_acc,
169 .tokens = { /* token list, NULL terminated */
170 (prog_void *)&cmd_traj_acc_arg0,
171 (prog_void *)&cmd_traj_acc_arg1,
172 (prog_void *)&cmd_traj_acc_s,
179 prog_char str_traj_acc_show_arg[] = "show";
180 parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg);
182 prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager";
183 parse_pgm_inst_t cmd_traj_acc_show = {
184 .f = cmd_traj_acc_parsed, /* function to call */
185 .data = NULL, /* 2nd arg of func */
186 .help_str = help_traj_acc_show,
187 .tokens = { /* token list, NULL terminated */
188 (prog_void *)&cmd_traj_acc_arg0,
189 (prog_void *)&cmd_traj_acc_show_arg,
194 /**********************************************************/
195 /* circle coef configuration */
197 /* this structure is filled when cmd_circle_coef is parsed successfully */
198 struct cmd_circle_coef_result {
205 /* function called when cmd_circle_coef is parsed successfully */
206 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
208 struct cmd_circle_coef_result *res = parsed_result;
210 if (!strcmp_P(res->arg1, PSTR("set"))) {
211 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
214 printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
217 prog_char str_circle_coef_arg0[] = "circle_coef";
218 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
219 prog_char str_circle_coef_arg1[] = "set";
220 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
221 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
223 prog_char help_circle_coef[] = "Set circle coef";
224 parse_pgm_inst_t cmd_circle_coef = {
225 .f = cmd_circle_coef_parsed, /* function to call */
226 .data = NULL, /* 2nd arg of func */
227 .help_str = help_circle_coef,
228 .tokens = { /* token list, NULL terminated */
229 (prog_void *)&cmd_circle_coef_arg0,
230 (prog_void *)&cmd_circle_coef_arg1,
231 (prog_void *)&cmd_circle_coef_val,
238 prog_char str_circle_coef_show_arg[] = "show";
239 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
241 prog_char help_circle_coef_show[] = "Show circle coef";
242 parse_pgm_inst_t cmd_circle_coef_show = {
243 .f = cmd_circle_coef_parsed, /* function to call */
244 .data = NULL, /* 2nd arg of func */
245 .help_str = help_circle_coef_show,
246 .tokens = { /* token list, NULL terminated */
247 (prog_void *)&cmd_circle_coef_arg0,
248 (prog_void *)&cmd_circle_coef_show_arg,
253 /**********************************************************/
254 /* trajectory window configuration */
256 /* this structure is filled when cmd_trajectory is parsed successfully */
257 struct cmd_trajectory_result {
266 /* function called when cmd_trajectory is parsed successfully */
267 static void cmd_trajectory_parsed(void * parsed_result, void * data)
269 struct cmd_trajectory_result * res = parsed_result;
271 if (!strcmp_P(res->arg1, PSTR("set"))) {
272 trajectory_set_windows(&mainboard.traj, res->d_win,
273 res->a_win, res->a_start);
276 printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
277 DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
280 prog_char str_trajectory_arg0[] = "trajectory";
281 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
282 prog_char str_trajectory_arg1[] = "set";
283 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
284 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
285 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
286 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
288 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
289 parse_pgm_inst_t cmd_trajectory = {
290 .f = cmd_trajectory_parsed, /* function to call */
291 .data = NULL, /* 2nd arg of func */
292 .help_str = help_trajectory,
293 .tokens = { /* token list, NULL terminated */
294 (prog_void *)&cmd_trajectory_arg0,
295 (prog_void *)&cmd_trajectory_arg1,
296 (prog_void *)&cmd_trajectory_d,
297 (prog_void *)&cmd_trajectory_a,
298 (prog_void *)&cmd_trajectory_as,
305 prog_char str_trajectory_show_arg[] = "show";
306 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
308 prog_char help_trajectory_show[] = "Show trajectory window configuration";
309 parse_pgm_inst_t cmd_trajectory_show = {
310 .f = cmd_trajectory_parsed, /* function to call */
311 .data = NULL, /* 2nd arg of func */
312 .help_str = help_trajectory_show,
313 .tokens = { /* token list, NULL terminated */
314 (prog_void *)&cmd_trajectory_arg0,
315 (prog_void *)&cmd_trajectory_show_arg,
320 /**********************************************************/
321 /* rs_gains configuration */
323 /* this structure is filled when cmd_rs_gains is parsed successfully */
324 struct cmd_rs_gains_result {
331 /* function called when cmd_rs_gains is parsed successfully */
332 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
335 printf("not implemented\n");
337 struct cmd_rs_gains_result * res = parsed_result;
339 if (!strcmp_P(res->arg1, PSTR("set"))) {
340 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
341 LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
342 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
343 RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
345 printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
346 mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
350 prog_char str_rs_gains_arg0[] = "rs_gains";
351 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
352 prog_char str_rs_gains_arg1[] = "set";
353 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
354 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
355 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
357 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
358 parse_pgm_inst_t cmd_rs_gains = {
359 .f = cmd_rs_gains_parsed, /* function to call */
360 .data = NULL, /* 2nd arg of func */
361 .help_str = help_rs_gains,
362 .tokens = { /* token list, NULL terminated */
363 (prog_void *)&cmd_rs_gains_arg0,
364 (prog_void *)&cmd_rs_gains_arg1,
365 (prog_void *)&cmd_rs_gains_l,
366 (prog_void *)&cmd_rs_gains_r,
373 prog_char str_rs_gains_show_arg[] = "show";
374 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
376 prog_char help_rs_gains_show[] = "Show rs_gains";
377 parse_pgm_inst_t cmd_rs_gains_show = {
378 .f = cmd_rs_gains_parsed, /* function to call */
379 .data = NULL, /* 2nd arg of func */
380 .help_str = help_rs_gains_show,
381 .tokens = { /* token list, NULL terminated */
382 (prog_void *)&cmd_rs_gains_arg0,
383 (prog_void *)&cmd_rs_gains_show_arg,
388 /**********************************************************/
389 /* track configuration */
391 /* this structure is filled when cmd_track is parsed successfully */
392 struct cmd_track_result {
398 /* function called when cmd_track is parsed successfully */
399 static void cmd_track_parsed(void * parsed_result, void * data)
401 struct cmd_track_result * res = parsed_result;
403 if (!strcmp_P(res->arg1, PSTR("set"))) {
404 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
406 printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
409 prog_char str_track_arg0[] = "track";
410 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
411 prog_char str_track_arg1[] = "set";
412 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
413 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
415 prog_char help_track[] = "Set track in mm";
416 parse_pgm_inst_t cmd_track = {
417 .f = cmd_track_parsed, /* function to call */
418 .data = NULL, /* 2nd arg of func */
419 .help_str = help_track,
420 .tokens = { /* token list, NULL terminated */
421 (prog_void *)&cmd_track_arg0,
422 (prog_void *)&cmd_track_arg1,
423 (prog_void *)&cmd_track_val,
430 prog_char str_track_show_arg[] = "show";
431 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
433 prog_char help_track_show[] = "Show track";
434 parse_pgm_inst_t cmd_track_show = {
435 .f = cmd_track_parsed, /* function to call */
436 .data = NULL, /* 2nd arg of func */
437 .help_str = help_track_show,
438 .tokens = { /* token list, NULL terminated */
439 (prog_void *)&cmd_track_arg0,
440 (prog_void *)&cmd_track_show_arg,
447 /**********************************************************/
448 /* Pt_Lists for testing traj */
450 #define PT_LIST_SIZE 10
451 static struct xy_point pt_list[PT_LIST_SIZE];
452 static uint16_t pt_list_len = 0;
454 /* this structure is filled when cmd_pt_list is parsed successfully */
455 struct cmd_pt_list_result {
463 /* function called when cmd_pt_list is parsed successfully */
464 static void cmd_pt_list_parsed(void * parsed_result, void * data)
466 struct cmd_pt_list_result * res = parsed_result;
469 if (!strcmp_P(res->arg1, PSTR("append"))) {
470 res->arg2 = pt_list_len;
472 if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
473 printf_P(PSTR("removed\r\n"));
477 if (!strcmp_P(res->arg1, PSTR("insert")) ||
478 !strcmp_P(res->arg1, PSTR("append"))) {
479 if (res->arg2 > pt_list_len) {
480 printf_P(PSTR("Index too large\r\n"));
483 if (pt_list_len >= PT_LIST_SIZE) {
484 printf_P(PSTR("List is too large\r\n"));
487 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
488 PT_LIST_SIZE-1-res->arg2);
489 pt_list[res->arg2].x = res->arg3;
490 pt_list[res->arg2].y = res->arg4;
493 else if (!strcmp_P(res->arg1, PSTR("del"))) {
494 if (pt_list_len <= 0) {
495 printf_P(PSTR("Error: list empty\r\n"));
498 if (res->arg2 > pt_list_len) {
499 printf_P(PSTR("Index too large\r\n"));
502 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
503 (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
506 else if (!strcmp_P(res->arg1, PSTR("reset"))) {
510 /* else it is a "show" or a "start" */
511 if (pt_list_len == 0) {
512 printf_P(PSTR("List empty\r\n"));
516 for (i=0 ; i<pt_list_len ; i++) {
517 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
518 if (!strcmp_P(res->arg1, PSTR("start"))) {
519 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
520 why = wait_traj_end(0xFF); /* all */
522 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
523 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
524 why = wait_traj_end(0xFF); /* all */
527 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
529 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
530 printf("next point\r\n");
531 if (why != END_OBSTACLE)
536 if (why & (~(END_TRAJ | END_NEAR)))
537 trajectory_stop(&mainboard.traj);
543 if (!strcmp_P(res->arg1, PSTR("loop_start")))
547 prog_char str_pt_list_arg0[] = "pt_list";
548 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
549 prog_char str_pt_list_arg1[] = "insert";
550 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
551 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
552 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
553 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
555 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
556 parse_pgm_inst_t cmd_pt_list = {
557 .f = cmd_pt_list_parsed, /* function to call */
558 .data = NULL, /* 2nd arg of func */
559 .help_str = help_pt_list,
560 .tokens = { /* token list, NULL terminated */
561 (prog_void *)&cmd_pt_list_arg0,
562 (prog_void *)&cmd_pt_list_arg1,
563 (prog_void *)&cmd_pt_list_arg2,
564 (prog_void *)&cmd_pt_list_arg3,
565 (prog_void *)&cmd_pt_list_arg4,
572 prog_char str_pt_list_arg1_append[] = "append";
573 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
575 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
576 parse_pgm_inst_t cmd_pt_list_append = {
577 .f = cmd_pt_list_parsed, /* function to call */
578 .data = NULL, /* 2nd arg of func */
579 .help_str = help_pt_list_append,
580 .tokens = { /* token list, NULL terminated */
581 (prog_void *)&cmd_pt_list_arg0,
582 (prog_void *)&cmd_pt_list_arg1_append,
583 (prog_void *)&cmd_pt_list_arg3,
584 (prog_void *)&cmd_pt_list_arg4,
591 prog_char str_pt_list_del_arg[] = "del";
592 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
594 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
595 parse_pgm_inst_t cmd_pt_list_del = {
596 .f = cmd_pt_list_parsed, /* function to call */
597 .data = NULL, /* 2nd arg of func */
598 .help_str = help_pt_list_del,
599 .tokens = { /* token list, NULL terminated */
600 (prog_void *)&cmd_pt_list_arg0,
601 (prog_void *)&cmd_pt_list_del_arg,
602 (prog_void *)&cmd_pt_list_arg2,
608 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
609 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
611 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
612 parse_pgm_inst_t cmd_pt_list_show = {
613 .f = cmd_pt_list_parsed, /* function to call */
614 .data = NULL, /* 2nd arg of func */
615 .help_str = help_pt_list_show,
616 .tokens = { /* token list, NULL terminated */
617 (prog_void *)&cmd_pt_list_arg0,
618 (prog_void *)&cmd_pt_list_show_arg,
625 /**********************************************************/
628 /* this structure is filled when cmd_goto is parsed successfully */
629 struct cmd_goto_result {
637 /* function called when cmd_goto is parsed successfully */
638 static void cmd_goto_parsed(void * parsed_result, void * data)
640 struct cmd_goto_result * res = parsed_result;
644 interrupt_traj_reset();
645 if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
646 trajectory_a_rel(&mainboard.traj, res->arg2);
648 else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
649 trajectory_d_rel(&mainboard.traj, res->arg2);
651 else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
652 trajectory_a_abs(&mainboard.traj, res->arg2);
654 else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
655 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
657 else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
658 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
660 else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
661 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
663 else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
664 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
666 else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
668 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
669 if (err != END_TRAJ && err != END_NEAR)
672 printf_P(PSTR("not implemented\r\n"));
676 else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
678 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
679 if (err != END_TRAJ && err != END_NEAR)
682 printf_P(PSTR("not implemented\r\n"));
686 else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
687 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
689 else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
690 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
692 else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
693 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
696 while ((err = test_traj_end(0xFF)) == 0) {
698 if (t2 - t1 > 200000) {
699 dump_cs_debug("angle", &mainboard.angle.cs);
700 dump_cs_debug("distance", &mainboard.distance.cs);
704 if (err != END_TRAJ && err != END_NEAR)
706 printf_P(PSTR("returned %s\r\n"), get_err(err));
709 prog_char str_goto_arg0[] = "goto";
710 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
711 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
712 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
713 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
716 prog_char help_goto1[] = "Change orientation of the mainboard";
717 parse_pgm_inst_t cmd_goto1 = {
718 .f = cmd_goto_parsed, /* function to call */
719 .data = NULL, /* 2nd arg of func */
720 .help_str = help_goto1,
721 .tokens = { /* token list, NULL terminated */
722 (prog_void *)&cmd_goto_arg0,
723 (prog_void *)&cmd_goto_arg1_a,
724 (prog_void *)&cmd_goto_arg2,
729 prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#xy_abs_fow#xy_abs_back#da_rel#a_to_xy#avoid#avoid_bw#a_behind_xy";
730 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
731 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
734 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
735 parse_pgm_inst_t cmd_goto2 = {
736 .f = cmd_goto_parsed, /* function to call */
737 .data = NULL, /* 2nd arg of func */
738 .help_str = help_goto2,
739 .tokens = { /* token list, NULL terminated */
740 (prog_void *)&cmd_goto_arg0,
741 (prog_void *)&cmd_goto_arg1_b,
742 (prog_void *)&cmd_goto_arg2,
743 (prog_void *)&cmd_goto_arg3,
748 /**********************************************************/
751 /* this structure is filled when cmd_position is parsed successfully */
752 struct cmd_position_result {
760 #define AUTOPOS_SPEED_FAST 200
761 static void auto_position(void)
764 uint16_t old_spdd, old_spda;
766 interrupt_traj_reset();
767 strat_get_speed(&old_spdd, &old_spda);
768 strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
770 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
773 strat_reset_pos(ROBOT_WIDTH/2 + 100,
774 COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
778 trajectory_d_rel(&mainboard.traj, -180);
779 err = wait_traj_end(END_INTR|END_TRAJ);
783 trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
784 err = wait_traj_end(END_INTR|END_TRAJ);
788 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
791 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
796 trajectory_d_rel(&mainboard.traj, -170);
797 err = wait_traj_end(END_INTR|END_TRAJ);
802 trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
803 err = wait_traj_end(END_INTR|END_TRAJ);
808 strat_set_speed(old_spdd, old_spda);
813 strat_set_speed(old_spdd, old_spda);
816 /* function called when cmd_position is parsed successfully */
817 static void cmd_position_parsed(void * parsed_result, void * data)
819 struct cmd_position_result * res = parsed_result;
821 /* display raw position values */
822 if (!strcmp_P(res->arg1, PSTR("reset"))) {
823 position_set(&mainboard.pos, 0, 0, 0);
825 else if (!strcmp_P(res->arg1, PSTR("set"))) {
826 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
828 else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
829 mainboard.our_color = I2C_COLOR_BLUE;
831 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
832 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
836 else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
837 mainboard.our_color = I2C_COLOR_YELLOW;
839 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
840 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
845 /* else it's just a "show" */
846 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
847 position_get_x_double(&mainboard.pos),
848 position_get_y_double(&mainboard.pos),
849 DEG(position_get_a_rad_double(&mainboard.pos)));
852 prog_char str_position_arg0[] = "position";
853 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
854 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
855 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
857 prog_char help_position[] = "Show/reset (x,y,a) position";
858 parse_pgm_inst_t cmd_position = {
859 .f = cmd_position_parsed, /* function to call */
860 .data = NULL, /* 2nd arg of func */
861 .help_str = help_position,
862 .tokens = { /* token list, NULL terminated */
863 (prog_void *)&cmd_position_arg0,
864 (prog_void *)&cmd_position_arg1,
870 prog_char str_position_arg1_set[] = "set";
871 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
872 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
873 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
874 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
876 prog_char help_position_set[] = "Set (x,y,a) position";
877 parse_pgm_inst_t cmd_position_set = {
878 .f = cmd_position_parsed, /* function to call */
879 .data = NULL, /* 2nd arg of func */
880 .help_str = help_position_set,
881 .tokens = { /* token list, NULL terminated */
882 (prog_void *)&cmd_position_arg0,
883 (prog_void *)&cmd_position_arg1_set,
884 (prog_void *)&cmd_position_arg2,
885 (prog_void *)&cmd_position_arg3,
886 (prog_void *)&cmd_position_arg4,
892 /**********************************************************/
893 /* strat configuration */
895 /* this structure is filled when cmd_strat_db is parsed successfully */
896 struct cmd_strat_db_result {
901 /* function called when cmd_strat_db is parsed successfully */
902 static void cmd_strat_db_parsed(void *parsed_result, void *data)
904 struct cmd_strat_db_result *res = parsed_result;
906 if (!strcmp_P(res->arg1, PSTR("reset"))) {
909 strat_db.dump_enabled = 1;
910 strat_db_dump(__FUNCTION__);
913 prog_char str_strat_db_arg0[] = "strat_db";
914 parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0);
915 prog_char str_strat_db_arg1[] = "show#reset";
916 parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1);
918 prog_char help_strat_db[] = "reset/show strat_db";
919 parse_pgm_inst_t cmd_strat_db = {
920 .f = cmd_strat_db_parsed, /* function to call */
921 .data = NULL, /* 2nd arg of func */
922 .help_str = help_strat_db,
923 .tokens = { /* token list, NULL terminated */
924 (prog_void *)&cmd_strat_db_arg0,
925 (prog_void *)&cmd_strat_db_arg1,
930 /**********************************************************/
931 /* strat configuration */
933 /* this structure is filled when cmd_strat_conf is parsed successfully */
934 struct cmd_strat_conf_result {
939 /* function called when cmd_strat_conf is parsed successfully */
940 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
942 // struct cmd_strat_conf_result *res = parsed_result;
943 strat_conf.dump_enabled = 1;
944 strat_conf_dump(__FUNCTION__);
947 prog_char str_strat_conf_arg0[] = "strat_conf";
948 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
949 prog_char str_strat_conf_arg1[] = "show#base";
950 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
952 prog_char help_strat_conf[] = "configure strat options";
953 parse_pgm_inst_t cmd_strat_conf = {
954 .f = cmd_strat_conf_parsed, /* function to call */
955 .data = NULL, /* 2nd arg of func */
956 .help_str = help_strat_conf,
957 .tokens = { /* token list, NULL terminated */
958 (prog_void *)&cmd_strat_conf_arg0,
959 (prog_void *)&cmd_strat_conf_arg1,
964 /**********************************************************/
965 /* strat configuration */
967 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
968 struct cmd_strat_conf2_result {
974 /* function called when cmd_strat_conf2 is parsed successfully */
975 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
977 struct cmd_strat_conf2_result *res = parsed_result;
980 if (!strcmp_P(res->arg2, PSTR("on")))
986 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
987 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
988 else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
989 bit = STRAT_CONF_BYPASS_STATIC2;
990 else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
991 bit = STRAT_CONF_TAKE_ONE_LINTEL;
992 else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
993 bit = STRAT_CONF_TAKE_ONE_LINTEL;
994 else if (!strcmp_P(res->arg1, PSTR("store_static2")))
995 bit = STRAT_CONF_STORE_STATIC2;
996 else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
997 bit = STRAT_CONF_BIG_3_TEMPLE;
998 else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
999 bit = STRAT_CONF_EARLY_SCAN;
1000 else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
1001 bit = STRAT_CONF_PUSH_OPP_COLS;
1005 strat_conf.flags |= bit;
1007 strat_conf.flags &= (~bit);
1009 strat_conf.dump_enabled = 1;
1010 strat_conf_dump(__FUNCTION__);
1013 prog_char str_strat_conf2_arg0[] = "strat_conf";
1014 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1015 prog_char str_strat_conf2_arg1[] = "faux";
1016 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1017 prog_char str_strat_conf2_arg2[] = "on#off";
1018 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1021 prog_char help_strat_conf2[] = "configure strat options";
1022 parse_pgm_inst_t cmd_strat_conf2 = {
1023 .f = cmd_strat_conf2_parsed, /* function to call */
1024 .data = NULL, /* 2nd arg of func */
1025 .help_str = help_strat_conf2,
1026 .tokens = { /* token list, NULL terminated */
1027 (prog_void *)&cmd_strat_conf2_arg0,
1028 (prog_void *)&cmd_strat_conf2_arg1,
1029 (prog_void *)&cmd_strat_conf2_arg2,
1034 /**********************************************************/
1035 /* strat configuration */
1037 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1038 struct cmd_strat_conf3_result {
1039 fixed_string_t arg0;
1040 fixed_string_t arg1;
1044 /* function called when cmd_strat_conf3 is parsed successfully */
1045 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1048 struct cmd_strat_conf3_result *res = parsed_result;
1050 if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1053 strat_conf.scan_opp_min_time = res->arg2;
1055 else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1058 strat_conf.delay_between_opp_scan = res->arg2;
1060 else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1063 strat_conf.scan_our_min_time = res->arg2;
1065 else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1068 strat_conf.delay_between_our_scan = res->arg2;
1070 else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1071 strat_conf.wait_opponent = res->arg2;
1073 else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1074 strat_conf.lintel_min_time = res->arg2;
1077 strat_conf.dump_enabled = 1;
1078 strat_conf_dump(__FUNCTION__);
1081 prog_char str_strat_conf3_arg0[] = "strat_conf";
1082 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1083 prog_char str_strat_conf3_arg1[] = "faux2";
1084 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1085 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1087 prog_char help_strat_conf3[] = "configure strat options";
1088 parse_pgm_inst_t cmd_strat_conf3 = {
1089 .f = cmd_strat_conf3_parsed, /* function to call */
1090 .data = NULL, /* 2nd arg of func */
1091 .help_str = help_strat_conf3,
1092 .tokens = { /* token list, NULL terminated */
1093 (prog_void *)&cmd_strat_conf3_arg0,
1094 (prog_void *)&cmd_strat_conf3_arg1,
1095 (prog_void *)&cmd_strat_conf3_arg2,
1101 /**********************************************************/
1104 //////////////////////
1108 #define TEST_SPEED 400
1111 static void line2line(double line1x1, double line1y1,
1112 double line1x2, double line1y2,
1113 double line2x1, double line2y1,
1114 double line2x2, double line2y2,
1115 double radius, double dist)
1118 double speed_d, speed_a;
1119 double distance, angle;
1120 double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
1121 double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
1123 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1125 strat_set_speed(TEST_SPEED, TEST_SPEED);
1126 quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1128 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1129 &speed_d, &speed_a);
1130 trajectory_line_abs(&mainboard.traj,
1132 line1x2, line1y2, 150.);
1133 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1134 dist, TRAJ_FLAGS_NO_NEAR);
1136 strat_set_speed(speed_d, speed_a);
1137 angle = line2_angle - line1_angle;
1138 distance = angle * radius;
1140 distance = -distance;
1141 angle = simple_modulo_2pi(angle);
1143 printf_P(PSTR("(%d,%d,%d) "),
1144 position_get_x_s16(&mainboard.pos),
1145 position_get_y_s16(&mainboard.pos),
1146 position_get_a_deg_s16(&mainboard.pos));
1147 printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
1150 /* take some margin on dist to avoid deceleration */
1151 trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
1153 /* circle exit condition */
1154 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1155 TRAJ_FLAGS_NO_NEAR);
1157 strat_set_speed(500, 500);
1158 printf_P(PSTR("(%d,%d,%d) "),
1159 position_get_x_s16(&mainboard.pos),
1160 position_get_y_s16(&mainboard.pos),
1161 position_get_a_deg_s16(&mainboard.pos));
1162 printf_P(PSTR("line\r\n"));
1163 trajectory_line_abs(&mainboard.traj,
1165 line2x2, line2y2, 150.);
1168 static void halfturn(double line1x1, double line1y1,
1169 double line1x2, double line1y2,
1170 double line2x1, double line2y1,
1171 double line2x2, double line2y2,
1172 double radius, double dist, double dir)
1175 double speed_d, speed_a;
1176 double distance, angle;
1178 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1180 strat_set_speed(TEST_SPEED, TEST_SPEED);
1181 quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1183 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1184 &speed_d, &speed_a);
1185 trajectory_line_abs(&mainboard.traj,
1187 line1x2, line1y2, 150.);
1188 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1189 dist, TRAJ_FLAGS_NO_NEAR);
1191 strat_set_speed(speed_d, speed_a);
1192 angle = dir * M_PI/2.;
1193 distance = angle * radius;
1195 distance = -distance;
1196 angle = simple_modulo_2pi(angle);
1199 /* take some margin on dist to avoid deceleration */
1200 DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
1202 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1204 /* circle exit condition */
1205 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1206 TRAJ_FLAGS_NO_NEAR);
1208 DEBUG(E_USER_STRAT, "miniline");
1209 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
1210 dist, TRAJ_FLAGS_NO_NEAR);
1211 DEBUG(E_USER_STRAT, "circle2");
1212 /* take some margin on dist to avoid deceleration */
1213 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1215 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1216 TRAJ_FLAGS_NO_NEAR);
1218 strat_set_speed(500, 500);
1219 DEBUG(E_USER_STRAT, "line");
1220 trajectory_line_abs(&mainboard.traj,
1222 line2x2, line2y2, 150.);
1226 /* function called when cmd_test is parsed successfully */
1227 static void subtraj_test(void)
1230 strat_reset_pos(400, 400, 90);
1231 mainboard.angle.on = 1;
1232 mainboard.distance.on = 1;
1234 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1235 while (!cmdline_keypressed()) {
1238 #define DIST_HARD_TURN 260
1239 #define RADIUS_HARD_TURN 100
1240 #define DIST_EASY_TURN 190
1241 #define RADIUS_EASY_TURN 190
1242 #define DIST_HALF_TURN 225
1243 #define RADIUS_HALF_TURN 130
1246 line2line(375, 597, 375, 1847,
1247 375, 1847, 1050, 1472,
1248 RADIUS_HARD_TURN, DIST_HARD_TURN);
1250 /* easy left and easy right !*/
1251 line2line(825, 1596, 1050, 1472,
1252 1050, 1472, 1500, 1722,
1253 RADIUS_EASY_TURN, DIST_EASY_TURN);
1254 line2line(1050, 1472, 1500, 1722,
1255 1500, 1722, 2175, 1347,
1256 RADIUS_EASY_TURN, DIST_EASY_TURN);
1257 line2line(1500, 1722, 2175, 1347,
1258 2175, 1347, 2175, 847,
1259 RADIUS_EASY_TURN, DIST_EASY_TURN);
1262 halfturn(2175, 1347, 2175, 722,
1263 2625, 722, 2625, 1597,
1264 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1265 halfturn(2625, 847, 2625, 1722,
1266 2175, 1722, 2175, 1097,
1267 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1270 line2line(2175, 1597, 2175, 1097,
1271 2175, 1097, 1500, 722,
1272 RADIUS_EASY_TURN, DIST_EASY_TURN);
1273 line2line(2175, 1097, 1500, 722,
1274 1500, 722, 1050, 972,
1275 RADIUS_EASY_TURN, DIST_EASY_TURN);
1276 line2line(1500, 722, 1050, 972,
1277 1050, 972, 375, 597,
1278 RADIUS_EASY_TURN, DIST_EASY_TURN);
1281 line2line(1050, 972, 375, 597,
1282 375, 597, 375, 1097,
1283 RADIUS_HARD_TURN, DIST_HARD_TURN);
1288 line2line(375, 597, 375, 1097,
1289 375, 1097, 1050, 1472,
1290 RADIUS_EASY_TURN, DIST_EASY_TURN);
1293 line2line(375, 1097, 1050, 1472,
1294 1050, 1472, 375, 1847,
1295 RADIUS_HARD_TURN, DIST_HARD_TURN);
1298 line2line(1050, 1472, 375, 1847,
1299 375, 1847, 375, 1347,
1300 RADIUS_HARD_TURN, DIST_HARD_TURN);
1303 line2line(375, 1847, 375, 1347,
1304 375, 1347, 1050, 972,
1305 RADIUS_EASY_TURN, DIST_EASY_TURN);
1308 line2line(375, 1347, 1050, 972,
1309 1050, 972, 375, 597,
1310 RADIUS_HARD_TURN, DIST_HARD_TURN);
1313 line2line(1050, 972, 375, 597,
1314 375, 597, 375, 1847,
1315 RADIUS_HARD_TURN, DIST_HARD_TURN);
1318 trajectory_hardstop(&mainboard.traj);
1322 /* this structure is filled when cmd_subtraj is parsed successfully */
1323 struct cmd_subtraj_result {
1324 fixed_string_t arg0;
1325 fixed_string_t arg1;
1332 /* function called when cmd_subtraj is parsed successfully */
1333 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1335 struct cmd_subtraj_result *res = parsed_result;
1337 if (!strcmp_P(res->arg1, PSTR("test")))
1340 trajectory_hardstop(&mainboard.traj);
1343 prog_char str_subtraj_arg0[] = "subtraj";
1344 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1345 prog_char str_subtraj_arg1[] = "faux";
1346 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1347 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1348 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1349 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1350 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1352 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1353 parse_pgm_inst_t cmd_subtraj = {
1354 .f = cmd_subtraj_parsed, /* function to call */
1355 .data = NULL, /* 2nd arg of func */
1356 .help_str = help_subtraj,
1357 .tokens = { /* token list, NULL terminated */
1358 (prog_void *)&cmd_subtraj_arg0,
1359 (prog_void *)&cmd_subtraj_arg1,
1360 (prog_void *)&cmd_subtraj_arg2,
1361 (prog_void *)&cmd_subtraj_arg3,
1362 (prog_void *)&cmd_subtraj_arg4,
1363 (prog_void *)&cmd_subtraj_arg5,