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 trajectory_d_rel(&mainboard.traj, 300);
770 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
774 strat_reset_pos(ROBOT_WIDTH/2,
775 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 trajectory_d_rel(&mainboard.traj, 300);
789 err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
793 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
797 trajectory_d_rel(&mainboard.traj, -170);
798 err = wait_traj_end(END_INTR|END_TRAJ);
803 trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
804 err = wait_traj_end(END_INTR|END_TRAJ);
809 strat_set_speed(old_spdd, old_spda);
814 strat_set_speed(old_spdd, old_spda);
817 /* function called when cmd_position is parsed successfully */
818 static void cmd_position_parsed(void * parsed_result, void * data)
820 struct cmd_position_result * res = parsed_result;
822 /* display raw position values */
823 if (!strcmp_P(res->arg1, PSTR("reset"))) {
824 position_set(&mainboard.pos, 0, 0, 0);
826 else if (!strcmp_P(res->arg1, PSTR("set"))) {
827 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
829 else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
830 mainboard.our_color = I2C_COLOR_BLUE;
832 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
833 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
837 else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
838 mainboard.our_color = I2C_COLOR_YELLOW;
840 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
841 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
846 /* else it's just a "show" */
847 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
848 position_get_x_double(&mainboard.pos),
849 position_get_y_double(&mainboard.pos),
850 DEG(position_get_a_rad_double(&mainboard.pos)));
853 prog_char str_position_arg0[] = "position";
854 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
855 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
856 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
858 prog_char help_position[] = "Show/reset (x,y,a) position";
859 parse_pgm_inst_t cmd_position = {
860 .f = cmd_position_parsed, /* function to call */
861 .data = NULL, /* 2nd arg of func */
862 .help_str = help_position,
863 .tokens = { /* token list, NULL terminated */
864 (prog_void *)&cmd_position_arg0,
865 (prog_void *)&cmd_position_arg1,
871 prog_char str_position_arg1_set[] = "set";
872 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
873 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
874 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
875 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
877 prog_char help_position_set[] = "Set (x,y,a) position";
878 parse_pgm_inst_t cmd_position_set = {
879 .f = cmd_position_parsed, /* function to call */
880 .data = NULL, /* 2nd arg of func */
881 .help_str = help_position_set,
882 .tokens = { /* token list, NULL terminated */
883 (prog_void *)&cmd_position_arg0,
884 (prog_void *)&cmd_position_arg1_set,
885 (prog_void *)&cmd_position_arg2,
886 (prog_void *)&cmd_position_arg3,
887 (prog_void *)&cmd_position_arg4,
893 /**********************************************************/
894 /* strat configuration */
896 /* this structure is filled when cmd_strat_infos is parsed successfully */
897 struct cmd_strat_infos_result {
902 /* function called when cmd_strat_infos is parsed successfully */
903 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
905 struct cmd_strat_infos_result *res = parsed_result;
907 if (!strcmp_P(res->arg1, PSTR("reset"))) {
910 strat_infos.dump_enabled = 1;
911 strat_dump_infos(__FUNCTION__);
914 prog_char str_strat_infos_arg0[] = "strat_infos";
915 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
916 prog_char str_strat_infos_arg1[] = "show#reset";
917 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
919 prog_char help_strat_infos[] = "reset/show strat_infos";
920 parse_pgm_inst_t cmd_strat_infos = {
921 .f = cmd_strat_infos_parsed, /* function to call */
922 .data = NULL, /* 2nd arg of func */
923 .help_str = help_strat_infos,
924 .tokens = { /* token list, NULL terminated */
925 (prog_void *)&cmd_strat_infos_arg0,
926 (prog_void *)&cmd_strat_infos_arg1,
931 /**********************************************************/
932 /* strat configuration */
934 /* this structure is filled when cmd_strat_conf is parsed successfully */
935 struct cmd_strat_conf_result {
940 /* function called when cmd_strat_conf is parsed successfully */
941 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
943 // struct cmd_strat_conf_result *res = parsed_result;
945 strat_infos.dump_enabled = 1;
949 prog_char str_strat_conf_arg0[] = "strat_conf";
950 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
951 prog_char str_strat_conf_arg1[] = "show#base";
952 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
954 prog_char help_strat_conf[] = "configure strat options";
955 parse_pgm_inst_t cmd_strat_conf = {
956 .f = cmd_strat_conf_parsed, /* function to call */
957 .data = NULL, /* 2nd arg of func */
958 .help_str = help_strat_conf,
959 .tokens = { /* token list, NULL terminated */
960 (prog_void *)&cmd_strat_conf_arg0,
961 (prog_void *)&cmd_strat_conf_arg1,
966 /**********************************************************/
967 /* strat configuration */
969 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
970 struct cmd_strat_conf2_result {
976 /* function called when cmd_strat_conf2 is parsed successfully */
977 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
979 struct cmd_strat_conf2_result *res = parsed_result;
982 if (!strcmp_P(res->arg2, PSTR("on")))
988 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
989 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
990 else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
991 bit = STRAT_CONF_BYPASS_STATIC2;
992 else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
993 bit = STRAT_CONF_TAKE_ONE_LINTEL;
994 else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
995 bit = STRAT_CONF_TAKE_ONE_LINTEL;
996 else if (!strcmp_P(res->arg1, PSTR("store_static2")))
997 bit = STRAT_CONF_STORE_STATIC2;
998 else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
999 bit = STRAT_CONF_BIG_3_TEMPLE;
1000 else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
1001 bit = STRAT_CONF_EARLY_SCAN;
1002 else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
1003 bit = STRAT_CONF_PUSH_OPP_COLS;
1007 strat_infos.conf.flags |= bit;
1009 strat_infos.conf.flags &= (~bit);
1011 strat_infos.dump_enabled = 1;
1015 prog_char str_strat_conf2_arg0[] = "strat_conf";
1016 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1017 prog_char str_strat_conf2_arg1[] = "faux";
1018 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1019 prog_char str_strat_conf2_arg2[] = "on#off";
1020 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1023 prog_char help_strat_conf2[] = "configure strat options";
1024 parse_pgm_inst_t cmd_strat_conf2 = {
1025 .f = cmd_strat_conf2_parsed, /* function to call */
1026 .data = NULL, /* 2nd arg of func */
1027 .help_str = help_strat_conf2,
1028 .tokens = { /* token list, NULL terminated */
1029 (prog_void *)&cmd_strat_conf2_arg0,
1030 (prog_void *)&cmd_strat_conf2_arg1,
1031 (prog_void *)&cmd_strat_conf2_arg2,
1036 /**********************************************************/
1037 /* strat configuration */
1039 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1040 struct cmd_strat_conf3_result {
1041 fixed_string_t arg0;
1042 fixed_string_t arg1;
1046 /* function called when cmd_strat_conf3 is parsed successfully */
1047 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1050 struct cmd_strat_conf3_result *res = parsed_result;
1052 if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1055 strat_infos.conf.scan_opp_min_time = res->arg2;
1057 else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1060 strat_infos.conf.delay_between_opp_scan = res->arg2;
1062 else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1065 strat_infos.conf.scan_our_min_time = res->arg2;
1067 else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1070 strat_infos.conf.delay_between_our_scan = res->arg2;
1072 else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1073 strat_infos.conf.wait_opponent = res->arg2;
1075 else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1076 strat_infos.conf.lintel_min_time = res->arg2;
1079 strat_infos.dump_enabled = 1;
1083 prog_char str_strat_conf3_arg0[] = "strat_conf";
1084 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1085 prog_char str_strat_conf3_arg1[] = "faux2";
1086 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1087 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1089 prog_char help_strat_conf3[] = "configure strat options";
1090 parse_pgm_inst_t cmd_strat_conf3 = {
1091 .f = cmd_strat_conf3_parsed, /* function to call */
1092 .data = NULL, /* 2nd arg of func */
1093 .help_str = help_strat_conf3,
1094 .tokens = { /* token list, NULL terminated */
1095 (prog_void *)&cmd_strat_conf3_arg0,
1096 (prog_void *)&cmd_strat_conf3_arg1,
1097 (prog_void *)&cmd_strat_conf3_arg2,
1103 /**********************************************************/
1106 //////////////////////
1110 #define TEST_SPEED 400
1113 static void line2line(double line1x1, double line1y1,
1114 double line1x2, double line1y2,
1115 double line2x1, double line2y1,
1116 double line2x2, double line2y2,
1117 double radius, double dist)
1120 double speed_d, speed_a;
1121 double distance, angle;
1122 double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
1123 double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
1125 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1127 strat_set_speed(TEST_SPEED, TEST_SPEED);
1128 quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1130 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1131 &speed_d, &speed_a);
1132 trajectory_line_abs(&mainboard.traj,
1134 line1x2, line1y2, 150.);
1135 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1136 dist, TRAJ_FLAGS_NO_NEAR);
1138 strat_set_speed(speed_d, speed_a);
1139 angle = line2_angle - line1_angle;
1140 distance = angle * radius;
1142 distance = -distance;
1143 angle = simple_modulo_2pi(angle);
1145 printf_P(PSTR("(%d,%d,%d) "),
1146 position_get_x_s16(&mainboard.pos),
1147 position_get_y_s16(&mainboard.pos),
1148 position_get_a_deg_s16(&mainboard.pos));
1149 printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
1152 /* take some margin on dist to avoid deceleration */
1153 trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
1155 /* circle exit condition */
1156 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1157 TRAJ_FLAGS_NO_NEAR);
1159 strat_set_speed(500, 500);
1160 printf_P(PSTR("(%d,%d,%d) "),
1161 position_get_x_s16(&mainboard.pos),
1162 position_get_y_s16(&mainboard.pos),
1163 position_get_a_deg_s16(&mainboard.pos));
1164 printf_P(PSTR("line\r\n"));
1165 trajectory_line_abs(&mainboard.traj,
1167 line2x2, line2y2, 150.);
1170 static void halfturn(double line1x1, double line1y1,
1171 double line1x2, double line1y2,
1172 double line2x1, double line2y1,
1173 double line2x2, double line2y2,
1174 double radius, double dist, double dir)
1177 double speed_d, speed_a;
1178 double distance, angle;
1180 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1182 strat_set_speed(TEST_SPEED, TEST_SPEED);
1183 quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1185 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1186 &speed_d, &speed_a);
1187 trajectory_line_abs(&mainboard.traj,
1189 line1x2, line1y2, 150.);
1190 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1191 dist, TRAJ_FLAGS_NO_NEAR);
1193 strat_set_speed(speed_d, speed_a);
1194 angle = dir * M_PI/2.;
1195 distance = angle * radius;
1197 distance = -distance;
1198 angle = simple_modulo_2pi(angle);
1201 /* take some margin on dist to avoid deceleration */
1202 DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
1204 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1206 /* circle exit condition */
1207 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1208 TRAJ_FLAGS_NO_NEAR);
1210 DEBUG(E_USER_STRAT, "miniline");
1211 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
1212 dist, TRAJ_FLAGS_NO_NEAR);
1213 DEBUG(E_USER_STRAT, "circle2");
1214 /* take some margin on dist to avoid deceleration */
1215 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1217 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1218 TRAJ_FLAGS_NO_NEAR);
1220 strat_set_speed(500, 500);
1221 DEBUG(E_USER_STRAT, "line");
1222 trajectory_line_abs(&mainboard.traj,
1224 line2x2, line2y2, 150.);
1228 /* function called when cmd_test is parsed successfully */
1229 static void subtraj_test(void)
1232 strat_reset_pos(400, 400, 90);
1233 mainboard.angle.on = 1;
1234 mainboard.distance.on = 1;
1236 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1237 while (!cmdline_keypressed()) {
1240 #define DIST_HARD_TURN 260
1241 #define RADIUS_HARD_TURN 100
1242 #define DIST_EASY_TURN 190
1243 #define RADIUS_EASY_TURN 190
1244 #define DIST_HALF_TURN 225
1245 #define RADIUS_HALF_TURN 130
1248 line2line(375, 597, 375, 1847,
1249 375, 1847, 1050, 1472,
1250 RADIUS_HARD_TURN, DIST_HARD_TURN);
1252 /* easy left and easy right !*/
1253 line2line(825, 1596, 1050, 1472,
1254 1050, 1472, 1500, 1722,
1255 RADIUS_EASY_TURN, DIST_EASY_TURN);
1256 line2line(1050, 1472, 1500, 1722,
1257 1500, 1722, 2175, 1347,
1258 RADIUS_EASY_TURN, DIST_EASY_TURN);
1259 line2line(1500, 1722, 2175, 1347,
1260 2175, 1347, 2175, 847,
1261 RADIUS_EASY_TURN, DIST_EASY_TURN);
1264 halfturn(2175, 1347, 2175, 722,
1265 2625, 722, 2625, 1597,
1266 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1267 halfturn(2625, 847, 2625, 1722,
1268 2175, 1722, 2175, 1097,
1269 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1272 line2line(2175, 1597, 2175, 1097,
1273 2175, 1097, 1500, 722,
1274 RADIUS_EASY_TURN, DIST_EASY_TURN);
1275 line2line(2175, 1097, 1500, 722,
1276 1500, 722, 1050, 972,
1277 RADIUS_EASY_TURN, DIST_EASY_TURN);
1278 line2line(1500, 722, 1050, 972,
1279 1050, 972, 375, 597,
1280 RADIUS_EASY_TURN, DIST_EASY_TURN);
1283 line2line(1050, 972, 375, 597,
1284 375, 597, 375, 1097,
1285 RADIUS_HARD_TURN, DIST_HARD_TURN);
1290 line2line(375, 597, 375, 1097,
1291 375, 1097, 1050, 1472,
1292 RADIUS_EASY_TURN, DIST_EASY_TURN);
1295 line2line(375, 1097, 1050, 1472,
1296 1050, 1472, 375, 1847,
1297 RADIUS_HARD_TURN, DIST_HARD_TURN);
1300 line2line(1050, 1472, 375, 1847,
1301 375, 1847, 375, 1347,
1302 RADIUS_HARD_TURN, DIST_HARD_TURN);
1305 line2line(375, 1847, 375, 1347,
1306 375, 1347, 1050, 972,
1307 RADIUS_EASY_TURN, DIST_EASY_TURN);
1310 line2line(375, 1347, 1050, 972,
1311 1050, 972, 375, 597,
1312 RADIUS_HARD_TURN, DIST_HARD_TURN);
1315 line2line(1050, 972, 375, 597,
1316 375, 597, 375, 1847,
1317 RADIUS_HARD_TURN, DIST_HARD_TURN);
1320 trajectory_hardstop(&mainboard.traj);
1324 /* this structure is filled when cmd_subtraj is parsed successfully */
1325 struct cmd_subtraj_result {
1326 fixed_string_t arg0;
1327 fixed_string_t arg1;
1334 /* function called when cmd_subtraj is parsed successfully */
1335 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1337 struct cmd_subtraj_result *res = parsed_result;
1339 if (!strcmp_P(res->arg1, PSTR("test")))
1342 trajectory_hardstop(&mainboard.traj);
1345 prog_char str_subtraj_arg0[] = "subtraj";
1346 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1347 prog_char str_subtraj_arg1[] = "faux";
1348 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1349 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1350 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1351 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1352 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1354 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1355 parse_pgm_inst_t cmd_subtraj = {
1356 .f = cmd_subtraj_parsed, /* function to call */
1357 .data = NULL, /* 2nd arg of func */
1358 .help_str = help_subtraj,
1359 .tokens = { /* token list, NULL terminated */
1360 (prog_void *)&cmd_subtraj_arg0,
1361 (prog_void *)&cmd_subtraj_arg1,
1362 (prog_void *)&cmd_subtraj_arg2,
1363 (prog_void *)&cmd_subtraj_arg3,
1364 (prog_void *)&cmd_subtraj_arg4,
1365 (prog_void *)&cmd_subtraj_arg5,