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"
62 #include "../common/i2c_commands.h"
63 #include "i2c_protocol.h"
65 /**********************************************************/
66 /* Traj_Speeds for trajectory_manager */
68 /* this structure is filled when cmd_traj_speed is parsed successfully */
69 struct cmd_traj_speed_result {
75 /* function called when cmd_traj_speed is parsed successfully */
76 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
78 struct cmd_traj_speed_result * res = parsed_result;
80 if (!strcmp_P(res->arg1, PSTR("angle"))) {
81 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
83 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
84 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
86 /* else it is a "show" */
88 printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
89 mainboard.traj.a_speed,
90 mainboard.traj.d_speed);
93 prog_char str_traj_speed_arg0[] = "traj_speed";
94 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
95 prog_char str_traj_speed_arg1[] = "angle#distance";
96 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
97 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT);
99 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
100 parse_pgm_inst_t cmd_traj_speed = {
101 .f = cmd_traj_speed_parsed, /* function to call */
102 .data = NULL, /* 2nd arg of func */
103 .help_str = help_traj_speed,
104 .tokens = { /* token list, NULL terminated */
105 (prog_void *)&cmd_traj_speed_arg0,
106 (prog_void *)&cmd_traj_speed_arg1,
107 (prog_void *)&cmd_traj_speed_s,
114 prog_char str_traj_speed_show_arg[] = "show";
115 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
117 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
118 parse_pgm_inst_t cmd_traj_speed_show = {
119 .f = cmd_traj_speed_parsed, /* function to call */
120 .data = NULL, /* 2nd arg of func */
121 .help_str = help_traj_speed_show,
122 .tokens = { /* token list, NULL terminated */
123 (prog_void *)&cmd_traj_speed_arg0,
124 (prog_void *)&cmd_traj_speed_show_arg,
129 /**********************************************************/
130 /* Traj_Accs for trajectory_manager */
132 /* this structure is filled when cmd_traj_acc is parsed successfully */
133 struct cmd_traj_acc_result {
139 /* function called when cmd_traj_acc is parsed successfully */
140 static void cmd_traj_acc_parsed(void *parsed_result, void *data)
142 struct cmd_traj_acc_result * res = parsed_result;
144 if (!strcmp_P(res->arg1, PSTR("angle"))) {
145 trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s);
147 else if (!strcmp_P(res->arg1, PSTR("distance"))) {
148 trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc);
150 /* else it is a "show" */
152 printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
153 mainboard.traj.a_acc,
154 mainboard.traj.d_acc);
157 prog_char str_traj_acc_arg0[] = "traj_acc";
158 parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0);
159 prog_char str_traj_acc_arg1[] = "angle#distance";
160 parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1);
161 parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT);
163 prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager";
164 parse_pgm_inst_t cmd_traj_acc = {
165 .f = cmd_traj_acc_parsed, /* function to call */
166 .data = NULL, /* 2nd arg of func */
167 .help_str = help_traj_acc,
168 .tokens = { /* token list, NULL terminated */
169 (prog_void *)&cmd_traj_acc_arg0,
170 (prog_void *)&cmd_traj_acc_arg1,
171 (prog_void *)&cmd_traj_acc_s,
178 prog_char str_traj_acc_show_arg[] = "show";
179 parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg);
181 prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager";
182 parse_pgm_inst_t cmd_traj_acc_show = {
183 .f = cmd_traj_acc_parsed, /* function to call */
184 .data = NULL, /* 2nd arg of func */
185 .help_str = help_traj_acc_show,
186 .tokens = { /* token list, NULL terminated */
187 (prog_void *)&cmd_traj_acc_arg0,
188 (prog_void *)&cmd_traj_acc_show_arg,
193 /**********************************************************/
194 /* circle coef configuration */
196 /* this structure is filled when cmd_circle_coef is parsed successfully */
197 struct cmd_circle_coef_result {
204 /* function called when cmd_circle_coef is parsed successfully */
205 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
207 struct cmd_circle_coef_result *res = parsed_result;
209 if (!strcmp_P(res->arg1, PSTR("set"))) {
210 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
213 printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
216 prog_char str_circle_coef_arg0[] = "circle_coef";
217 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
218 prog_char str_circle_coef_arg1[] = "set";
219 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
220 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
222 prog_char help_circle_coef[] = "Set circle coef";
223 parse_pgm_inst_t cmd_circle_coef = {
224 .f = cmd_circle_coef_parsed, /* function to call */
225 .data = NULL, /* 2nd arg of func */
226 .help_str = help_circle_coef,
227 .tokens = { /* token list, NULL terminated */
228 (prog_void *)&cmd_circle_coef_arg0,
229 (prog_void *)&cmd_circle_coef_arg1,
230 (prog_void *)&cmd_circle_coef_val,
237 prog_char str_circle_coef_show_arg[] = "show";
238 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
240 prog_char help_circle_coef_show[] = "Show circle coef";
241 parse_pgm_inst_t cmd_circle_coef_show = {
242 .f = cmd_circle_coef_parsed, /* function to call */
243 .data = NULL, /* 2nd arg of func */
244 .help_str = help_circle_coef_show,
245 .tokens = { /* token list, NULL terminated */
246 (prog_void *)&cmd_circle_coef_arg0,
247 (prog_void *)&cmd_circle_coef_show_arg,
252 /**********************************************************/
253 /* trajectory window configuration */
255 /* this structure is filled when cmd_trajectory is parsed successfully */
256 struct cmd_trajectory_result {
265 /* function called when cmd_trajectory is parsed successfully */
266 static void cmd_trajectory_parsed(void * parsed_result, void * data)
268 struct cmd_trajectory_result * res = parsed_result;
270 if (!strcmp_P(res->arg1, PSTR("set"))) {
271 trajectory_set_windows(&mainboard.traj, res->d_win,
272 res->a_win, res->a_start);
275 printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
276 DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
279 prog_char str_trajectory_arg0[] = "trajectory";
280 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
281 prog_char str_trajectory_arg1[] = "set";
282 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
283 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
284 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
285 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
287 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
288 parse_pgm_inst_t cmd_trajectory = {
289 .f = cmd_trajectory_parsed, /* function to call */
290 .data = NULL, /* 2nd arg of func */
291 .help_str = help_trajectory,
292 .tokens = { /* token list, NULL terminated */
293 (prog_void *)&cmd_trajectory_arg0,
294 (prog_void *)&cmd_trajectory_arg1,
295 (prog_void *)&cmd_trajectory_d,
296 (prog_void *)&cmd_trajectory_a,
297 (prog_void *)&cmd_trajectory_as,
304 prog_char str_trajectory_show_arg[] = "show";
305 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
307 prog_char help_trajectory_show[] = "Show trajectory window configuration";
308 parse_pgm_inst_t cmd_trajectory_show = {
309 .f = cmd_trajectory_parsed, /* function to call */
310 .data = NULL, /* 2nd arg of func */
311 .help_str = help_trajectory_show,
312 .tokens = { /* token list, NULL terminated */
313 (prog_void *)&cmd_trajectory_arg0,
314 (prog_void *)&cmd_trajectory_show_arg,
319 /**********************************************************/
320 /* rs_gains configuration */
322 /* this structure is filled when cmd_rs_gains is parsed successfully */
323 struct cmd_rs_gains_result {
330 /* function called when cmd_rs_gains is parsed successfully */
331 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
334 printf("not implemented\n");
336 struct cmd_rs_gains_result * res = parsed_result;
338 if (!strcmp_P(res->arg1, PSTR("set"))) {
339 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
340 LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
341 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
342 RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
344 printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
345 mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
349 prog_char str_rs_gains_arg0[] = "rs_gains";
350 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
351 prog_char str_rs_gains_arg1[] = "set";
352 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
353 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
354 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
356 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
357 parse_pgm_inst_t cmd_rs_gains = {
358 .f = cmd_rs_gains_parsed, /* function to call */
359 .data = NULL, /* 2nd arg of func */
360 .help_str = help_rs_gains,
361 .tokens = { /* token list, NULL terminated */
362 (prog_void *)&cmd_rs_gains_arg0,
363 (prog_void *)&cmd_rs_gains_arg1,
364 (prog_void *)&cmd_rs_gains_l,
365 (prog_void *)&cmd_rs_gains_r,
372 prog_char str_rs_gains_show_arg[] = "show";
373 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
375 prog_char help_rs_gains_show[] = "Show rs_gains";
376 parse_pgm_inst_t cmd_rs_gains_show = {
377 .f = cmd_rs_gains_parsed, /* function to call */
378 .data = NULL, /* 2nd arg of func */
379 .help_str = help_rs_gains_show,
380 .tokens = { /* token list, NULL terminated */
381 (prog_void *)&cmd_rs_gains_arg0,
382 (prog_void *)&cmd_rs_gains_show_arg,
387 /**********************************************************/
388 /* track configuration */
390 /* this structure is filled when cmd_track is parsed successfully */
391 struct cmd_track_result {
397 /* function called when cmd_track is parsed successfully */
398 static void cmd_track_parsed(void * parsed_result, void * data)
400 struct cmd_track_result * res = parsed_result;
402 if (!strcmp_P(res->arg1, PSTR("set"))) {
403 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
405 printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
408 prog_char str_track_arg0[] = "track";
409 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
410 prog_char str_track_arg1[] = "set";
411 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
412 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
414 prog_char help_track[] = "Set track in mm";
415 parse_pgm_inst_t cmd_track = {
416 .f = cmd_track_parsed, /* function to call */
417 .data = NULL, /* 2nd arg of func */
418 .help_str = help_track,
419 .tokens = { /* token list, NULL terminated */
420 (prog_void *)&cmd_track_arg0,
421 (prog_void *)&cmd_track_arg1,
422 (prog_void *)&cmd_track_val,
429 prog_char str_track_show_arg[] = "show";
430 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
432 prog_char help_track_show[] = "Show track";
433 parse_pgm_inst_t cmd_track_show = {
434 .f = cmd_track_parsed, /* function to call */
435 .data = NULL, /* 2nd arg of func */
436 .help_str = help_track_show,
437 .tokens = { /* token list, NULL terminated */
438 (prog_void *)&cmd_track_arg0,
439 (prog_void *)&cmd_track_show_arg,
446 /**********************************************************/
447 /* Pt_Lists for testing traj */
449 #define PT_LIST_SIZE 10
450 static struct xy_point pt_list[PT_LIST_SIZE];
451 static uint16_t pt_list_len = 0;
453 /* this structure is filled when cmd_pt_list is parsed successfully */
454 struct cmd_pt_list_result {
462 /* function called when cmd_pt_list is parsed successfully */
463 static void cmd_pt_list_parsed(void * parsed_result, void * data)
465 struct cmd_pt_list_result * res = parsed_result;
468 if (!strcmp_P(res->arg1, PSTR("append"))) {
469 res->arg2 = pt_list_len;
471 if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
472 printf_P(PSTR("removed\r\n"));
476 if (!strcmp_P(res->arg1, PSTR("insert")) ||
477 !strcmp_P(res->arg1, PSTR("append"))) {
478 if (res->arg2 > pt_list_len) {
479 printf_P(PSTR("Index too large\r\n"));
482 if (pt_list_len >= PT_LIST_SIZE) {
483 printf_P(PSTR("List is too large\r\n"));
486 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
487 PT_LIST_SIZE-1-res->arg2);
488 pt_list[res->arg2].x = res->arg3;
489 pt_list[res->arg2].y = res->arg4;
492 else if (!strcmp_P(res->arg1, PSTR("del"))) {
493 if (pt_list_len <= 0) {
494 printf_P(PSTR("Error: list empty\r\n"));
497 if (res->arg2 > pt_list_len) {
498 printf_P(PSTR("Index too large\r\n"));
501 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
502 (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
505 else if (!strcmp_P(res->arg1, PSTR("reset"))) {
509 /* else it is a "show" or a "start" */
510 if (pt_list_len == 0) {
511 printf_P(PSTR("List empty\r\n"));
515 for (i=0 ; i<pt_list_len ; i++) {
516 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
517 if (!strcmp_P(res->arg1, PSTR("start"))) {
518 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
519 why = wait_traj_end(0xFF); /* all */
521 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
522 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
523 why = wait_traj_end(0xFF); /* all */
526 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
528 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
529 printf("next point\r\n");
530 if (why != END_OBSTACLE)
535 if (why & (~(END_TRAJ | END_NEAR)))
536 trajectory_stop(&mainboard.traj);
542 if (!strcmp_P(res->arg1, PSTR("loop_start")))
546 prog_char str_pt_list_arg0[] = "pt_list";
547 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
548 prog_char str_pt_list_arg1[] = "insert";
549 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
550 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
551 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
552 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
554 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
555 parse_pgm_inst_t cmd_pt_list = {
556 .f = cmd_pt_list_parsed, /* function to call */
557 .data = NULL, /* 2nd arg of func */
558 .help_str = help_pt_list,
559 .tokens = { /* token list, NULL terminated */
560 (prog_void *)&cmd_pt_list_arg0,
561 (prog_void *)&cmd_pt_list_arg1,
562 (prog_void *)&cmd_pt_list_arg2,
563 (prog_void *)&cmd_pt_list_arg3,
564 (prog_void *)&cmd_pt_list_arg4,
571 prog_char str_pt_list_arg1_append[] = "append";
572 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
574 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
575 parse_pgm_inst_t cmd_pt_list_append = {
576 .f = cmd_pt_list_parsed, /* function to call */
577 .data = NULL, /* 2nd arg of func */
578 .help_str = help_pt_list_append,
579 .tokens = { /* token list, NULL terminated */
580 (prog_void *)&cmd_pt_list_arg0,
581 (prog_void *)&cmd_pt_list_arg1_append,
582 (prog_void *)&cmd_pt_list_arg3,
583 (prog_void *)&cmd_pt_list_arg4,
590 prog_char str_pt_list_del_arg[] = "del";
591 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
593 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
594 parse_pgm_inst_t cmd_pt_list_del = {
595 .f = cmd_pt_list_parsed, /* function to call */
596 .data = NULL, /* 2nd arg of func */
597 .help_str = help_pt_list_del,
598 .tokens = { /* token list, NULL terminated */
599 (prog_void *)&cmd_pt_list_arg0,
600 (prog_void *)&cmd_pt_list_del_arg,
601 (prog_void *)&cmd_pt_list_arg2,
607 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
608 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
610 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
611 parse_pgm_inst_t cmd_pt_list_show = {
612 .f = cmd_pt_list_parsed, /* function to call */
613 .data = NULL, /* 2nd arg of func */
614 .help_str = help_pt_list_show,
615 .tokens = { /* token list, NULL terminated */
616 (prog_void *)&cmd_pt_list_arg0,
617 (prog_void *)&cmd_pt_list_show_arg,
624 /**********************************************************/
627 /* this structure is filled when cmd_goto is parsed successfully */
628 struct cmd_goto_result {
636 /* function called when cmd_goto is parsed successfully */
637 static void cmd_goto_parsed(void * parsed_result, void * data)
639 struct cmd_goto_result * res = parsed_result;
643 interrupt_traj_reset();
644 if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
645 trajectory_a_rel(&mainboard.traj, res->arg2);
647 else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
648 trajectory_d_rel(&mainboard.traj, res->arg2);
650 else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
651 trajectory_a_abs(&mainboard.traj, res->arg2);
653 else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
654 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
656 else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
657 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
659 else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
660 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
662 else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
663 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
665 else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
667 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
668 if (err != END_TRAJ && err != END_NEAR)
671 printf_P(PSTR("not implemented\r\n"));
675 else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
677 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
678 if (err != END_TRAJ && err != END_NEAR)
681 printf_P(PSTR("not implemented\r\n"));
685 else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
686 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
688 else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
689 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
691 else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
692 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
695 while ((err = test_traj_end(0xFF)) == 0) {
697 if (t2 - t1 > 200000) {
698 dump_cs_debug("angle", &mainboard.angle.cs);
699 dump_cs_debug("distance", &mainboard.distance.cs);
703 if (err != END_TRAJ && err != END_NEAR)
705 printf_P(PSTR("returned %s\r\n"), get_err(err));
708 prog_char str_goto_arg0[] = "goto";
709 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
710 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
711 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
712 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
715 prog_char help_goto1[] = "Change orientation of the mainboard";
716 parse_pgm_inst_t cmd_goto1 = {
717 .f = cmd_goto_parsed, /* function to call */
718 .data = NULL, /* 2nd arg of func */
719 .help_str = help_goto1,
720 .tokens = { /* token list, NULL terminated */
721 (prog_void *)&cmd_goto_arg0,
722 (prog_void *)&cmd_goto_arg1_a,
723 (prog_void *)&cmd_goto_arg2,
728 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";
729 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
730 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
733 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
734 parse_pgm_inst_t cmd_goto2 = {
735 .f = cmd_goto_parsed, /* function to call */
736 .data = NULL, /* 2nd arg of func */
737 .help_str = help_goto2,
738 .tokens = { /* token list, NULL terminated */
739 (prog_void *)&cmd_goto_arg0,
740 (prog_void *)&cmd_goto_arg1_b,
741 (prog_void *)&cmd_goto_arg2,
742 (prog_void *)&cmd_goto_arg3,
747 /**********************************************************/
750 /* this structure is filled when cmd_position is parsed successfully */
751 struct cmd_position_result {
759 #define AUTOPOS_SPEED_FAST 200
760 static void auto_position(void)
763 uint16_t old_spdd, old_spda;
765 interrupt_traj_reset();
766 strat_get_speed(&old_spdd, &old_spda);
767 strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
769 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
772 strat_reset_pos(ROBOT_WIDTH/2 + 100,
773 COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
777 trajectory_d_rel(&mainboard.traj, -180);
778 err = wait_traj_end(END_INTR|END_TRAJ);
782 trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
783 err = wait_traj_end(END_INTR|END_TRAJ);
787 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
790 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
795 trajectory_d_rel(&mainboard.traj, -170);
796 err = wait_traj_end(END_INTR|END_TRAJ);
801 trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
802 err = wait_traj_end(END_INTR|END_TRAJ);
807 strat_set_speed(old_spdd, old_spda);
812 strat_set_speed(old_spdd, old_spda);
815 /* function called when cmd_position is parsed successfully */
816 static void cmd_position_parsed(void * parsed_result, void * data)
818 struct cmd_position_result * res = parsed_result;
820 /* display raw position values */
821 if (!strcmp_P(res->arg1, PSTR("reset"))) {
822 position_set(&mainboard.pos, 0, 0, 0);
824 else if (!strcmp_P(res->arg1, PSTR("set"))) {
825 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
827 else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
828 mainboard.our_color = I2C_COLOR_BLUE;
830 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
831 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
835 else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
836 mainboard.our_color = I2C_COLOR_YELLOW;
838 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
839 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
844 /* else it's just a "show" */
845 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
846 position_get_x_double(&mainboard.pos),
847 position_get_y_double(&mainboard.pos),
848 DEG(position_get_a_rad_double(&mainboard.pos)));
851 prog_char str_position_arg0[] = "position";
852 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
853 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
854 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
856 prog_char help_position[] = "Show/reset (x,y,a) position";
857 parse_pgm_inst_t cmd_position = {
858 .f = cmd_position_parsed, /* function to call */
859 .data = NULL, /* 2nd arg of func */
860 .help_str = help_position,
861 .tokens = { /* token list, NULL terminated */
862 (prog_void *)&cmd_position_arg0,
863 (prog_void *)&cmd_position_arg1,
869 prog_char str_position_arg1_set[] = "set";
870 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
871 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
872 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
873 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
875 prog_char help_position_set[] = "Set (x,y,a) position";
876 parse_pgm_inst_t cmd_position_set = {
877 .f = cmd_position_parsed, /* function to call */
878 .data = NULL, /* 2nd arg of func */
879 .help_str = help_position_set,
880 .tokens = { /* token list, NULL terminated */
881 (prog_void *)&cmd_position_arg0,
882 (prog_void *)&cmd_position_arg1_set,
883 (prog_void *)&cmd_position_arg2,
884 (prog_void *)&cmd_position_arg3,
885 (prog_void *)&cmd_position_arg4,
891 /**********************************************************/
892 /* strat configuration */
894 /* this structure is filled when cmd_strat_infos is parsed successfully */
895 struct cmd_strat_infos_result {
900 /* function called when cmd_strat_infos is parsed successfully */
901 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
903 struct cmd_strat_infos_result *res = parsed_result;
905 if (!strcmp_P(res->arg1, PSTR("reset"))) {
908 strat_infos.dump_enabled = 1;
909 strat_dump_infos(__FUNCTION__);
912 prog_char str_strat_infos_arg0[] = "strat_infos";
913 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
914 prog_char str_strat_infos_arg1[] = "show#reset";
915 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
917 prog_char help_strat_infos[] = "reset/show strat_infos";
918 parse_pgm_inst_t cmd_strat_infos = {
919 .f = cmd_strat_infos_parsed, /* function to call */
920 .data = NULL, /* 2nd arg of func */
921 .help_str = help_strat_infos,
922 .tokens = { /* token list, NULL terminated */
923 (prog_void *)&cmd_strat_infos_arg0,
924 (prog_void *)&cmd_strat_infos_arg1,
929 /**********************************************************/
930 /* strat configuration */
932 /* this structure is filled when cmd_strat_conf is parsed successfully */
933 struct cmd_strat_conf_result {
938 /* function called when cmd_strat_conf is parsed successfully */
939 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
941 // struct cmd_strat_conf_result *res = parsed_result;
943 strat_infos.dump_enabled = 1;
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_infos.conf.flags |= bit;
1007 strat_infos.conf.flags &= (~bit);
1009 strat_infos.dump_enabled = 1;
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_infos.conf.scan_opp_min_time = res->arg2;
1055 else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1058 strat_infos.conf.delay_between_opp_scan = res->arg2;
1060 else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1063 strat_infos.conf.scan_our_min_time = res->arg2;
1065 else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1068 strat_infos.conf.delay_between_our_scan = res->arg2;
1070 else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1071 strat_infos.conf.wait_opponent = res->arg2;
1073 else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1074 strat_infos.conf.lintel_min_time = res->arg2;
1077 strat_infos.dump_enabled = 1;
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,