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,
445 /**********************************************************/
446 /* centrifugal configuration */
448 /* this structure is filled when cmd_centrifugal is parsed successfully */
449 struct cmd_centrifugal_result {
455 /* function called when cmd_centrifugal is parsed successfully */
456 static void cmd_centrifugal_parsed(void * parsed_result, void * data)
458 struct cmd_centrifugal_result * res = parsed_result;
460 if (!strcmp_P(res->arg1, PSTR("set"))) {
461 position_set_centrifugal_coef(&mainboard.pos, res->val);
463 printf_P(PSTR("centrifugal set %f\r\n"), mainboard.pos.centrifugal_coef);
466 prog_char str_centrifugal_arg0[] = "centrifugal";
467 parse_pgm_token_string_t cmd_centrifugal_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg0, str_centrifugal_arg0);
468 prog_char str_centrifugal_arg1[] = "set";
469 parse_pgm_token_string_t cmd_centrifugal_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_arg1);
470 parse_pgm_token_num_t cmd_centrifugal_val = TOKEN_NUM_INITIALIZER(struct cmd_centrifugal_result, val, FLOAT);
472 prog_char help_centrifugal[] = "Set centrifugal coef";
473 parse_pgm_inst_t cmd_centrifugal = {
474 .f = cmd_centrifugal_parsed, /* function to call */
475 .data = NULL, /* 2nd arg of func */
476 .help_str = help_centrifugal,
477 .tokens = { /* token list, NULL terminated */
478 (prog_void *)&cmd_centrifugal_arg0,
479 (prog_void *)&cmd_centrifugal_arg1,
480 (prog_void *)&cmd_centrifugal_val,
487 prog_char str_centrifugal_show_arg[] = "show";
488 parse_pgm_token_string_t cmd_centrifugal_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_show_arg);
490 prog_char help_centrifugal_show[] = "Show centrifugal";
491 parse_pgm_inst_t cmd_centrifugal_show = {
492 .f = cmd_centrifugal_parsed, /* function to call */
493 .data = NULL, /* 2nd arg of func */
494 .help_str = help_centrifugal_show,
495 .tokens = { /* token list, NULL terminated */
496 (prog_void *)&cmd_centrifugal_arg0,
497 (prog_void *)&cmd_centrifugal_show_arg,
504 /**********************************************************/
505 /* Pt_Lists for testing traj */
507 #define PT_LIST_SIZE 10
508 static struct xy_point pt_list[PT_LIST_SIZE];
509 static uint16_t pt_list_len = 0;
511 /* this structure is filled when cmd_pt_list is parsed successfully */
512 struct cmd_pt_list_result {
520 /* function called when cmd_pt_list is parsed successfully */
521 static void cmd_pt_list_parsed(void * parsed_result, void * data)
523 struct cmd_pt_list_result * res = parsed_result;
526 if (!strcmp_P(res->arg1, PSTR("append"))) {
527 res->arg2 = pt_list_len;
529 if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
530 printf_P(PSTR("removed\r\n"));
534 if (!strcmp_P(res->arg1, PSTR("insert")) ||
535 !strcmp_P(res->arg1, PSTR("append"))) {
536 if (res->arg2 > pt_list_len) {
537 printf_P(PSTR("Index too large\r\n"));
540 if (pt_list_len >= PT_LIST_SIZE) {
541 printf_P(PSTR("List is too large\r\n"));
544 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
545 PT_LIST_SIZE-1-res->arg2);
546 pt_list[res->arg2].x = res->arg3;
547 pt_list[res->arg2].y = res->arg4;
550 else if (!strcmp_P(res->arg1, PSTR("del"))) {
551 if (pt_list_len <= 0) {
552 printf_P(PSTR("Error: list empty\r\n"));
555 if (res->arg2 > pt_list_len) {
556 printf_P(PSTR("Index too large\r\n"));
559 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
560 (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
563 else if (!strcmp_P(res->arg1, PSTR("reset"))) {
567 /* else it is a "show" or a "start" */
568 if (pt_list_len == 0) {
569 printf_P(PSTR("List empty\r\n"));
573 for (i=0 ; i<pt_list_len ; i++) {
574 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
575 if (!strcmp_P(res->arg1, PSTR("start"))) {
576 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
577 why = wait_traj_end(0xFF); /* all */
579 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
580 trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
581 why = wait_traj_end(0xFF); /* all */
584 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
586 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
587 printf("next point\r\n");
588 if (why != END_OBSTACLE)
593 if (why & (~(END_TRAJ | END_NEAR)))
594 trajectory_stop(&mainboard.traj);
600 if (!strcmp_P(res->arg1, PSTR("loop_start")))
604 prog_char str_pt_list_arg0[] = "pt_list";
605 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
606 prog_char str_pt_list_arg1[] = "insert";
607 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
608 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
609 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
610 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
612 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
613 parse_pgm_inst_t cmd_pt_list = {
614 .f = cmd_pt_list_parsed, /* function to call */
615 .data = NULL, /* 2nd arg of func */
616 .help_str = help_pt_list,
617 .tokens = { /* token list, NULL terminated */
618 (prog_void *)&cmd_pt_list_arg0,
619 (prog_void *)&cmd_pt_list_arg1,
620 (prog_void *)&cmd_pt_list_arg2,
621 (prog_void *)&cmd_pt_list_arg3,
622 (prog_void *)&cmd_pt_list_arg4,
629 prog_char str_pt_list_arg1_append[] = "append";
630 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
632 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
633 parse_pgm_inst_t cmd_pt_list_append = {
634 .f = cmd_pt_list_parsed, /* function to call */
635 .data = NULL, /* 2nd arg of func */
636 .help_str = help_pt_list_append,
637 .tokens = { /* token list, NULL terminated */
638 (prog_void *)&cmd_pt_list_arg0,
639 (prog_void *)&cmd_pt_list_arg1_append,
640 (prog_void *)&cmd_pt_list_arg3,
641 (prog_void *)&cmd_pt_list_arg4,
648 prog_char str_pt_list_del_arg[] = "del";
649 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
651 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
652 parse_pgm_inst_t cmd_pt_list_del = {
653 .f = cmd_pt_list_parsed, /* function to call */
654 .data = NULL, /* 2nd arg of func */
655 .help_str = help_pt_list_del,
656 .tokens = { /* token list, NULL terminated */
657 (prog_void *)&cmd_pt_list_arg0,
658 (prog_void *)&cmd_pt_list_del_arg,
659 (prog_void *)&cmd_pt_list_arg2,
665 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
666 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
668 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
669 parse_pgm_inst_t cmd_pt_list_show = {
670 .f = cmd_pt_list_parsed, /* function to call */
671 .data = NULL, /* 2nd arg of func */
672 .help_str = help_pt_list_show,
673 .tokens = { /* token list, NULL terminated */
674 (prog_void *)&cmd_pt_list_arg0,
675 (prog_void *)&cmd_pt_list_show_arg,
682 /**********************************************************/
685 /* this structure is filled when cmd_goto is parsed successfully */
686 struct cmd_goto_result {
694 /* function called when cmd_goto is parsed successfully */
695 static void cmd_goto_parsed(void * parsed_result, void * data)
697 struct cmd_goto_result * res = parsed_result;
701 interrupt_traj_reset();
702 if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
703 trajectory_a_rel(&mainboard.traj, res->arg2);
705 else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
706 trajectory_d_rel(&mainboard.traj, res->arg2);
708 else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
709 trajectory_a_abs(&mainboard.traj, res->arg2);
711 else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
712 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
714 else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
715 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
717 else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
718 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
720 else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
721 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
723 else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
725 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
726 if (err != END_TRAJ && err != END_NEAR)
729 printf_P(PSTR("not implemented\r\n"));
733 else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
735 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
736 if (err != END_TRAJ && err != END_NEAR)
739 printf_P(PSTR("not implemented\r\n"));
743 else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
744 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
746 else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
747 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
749 else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
750 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
753 while ((err = test_traj_end(0xFF)) == 0) {
755 if (t2 - t1 > 200000) {
756 dump_cs_debug("angle", &mainboard.angle.cs);
757 dump_cs_debug("distance", &mainboard.distance.cs);
761 if (err != END_TRAJ && err != END_NEAR)
763 printf_P(PSTR("returned %s\r\n"), get_err(err));
766 prog_char str_goto_arg0[] = "goto";
767 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
768 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
769 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
770 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
773 prog_char help_goto1[] = "Change orientation of the mainboard";
774 parse_pgm_inst_t cmd_goto1 = {
775 .f = cmd_goto_parsed, /* function to call */
776 .data = NULL, /* 2nd arg of func */
777 .help_str = help_goto1,
778 .tokens = { /* token list, NULL terminated */
779 (prog_void *)&cmd_goto_arg0,
780 (prog_void *)&cmd_goto_arg1_a,
781 (prog_void *)&cmd_goto_arg2,
786 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";
787 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
788 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
791 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
792 parse_pgm_inst_t cmd_goto2 = {
793 .f = cmd_goto_parsed, /* function to call */
794 .data = NULL, /* 2nd arg of func */
795 .help_str = help_goto2,
796 .tokens = { /* token list, NULL terminated */
797 (prog_void *)&cmd_goto_arg0,
798 (prog_void *)&cmd_goto_arg1_b,
799 (prog_void *)&cmd_goto_arg2,
800 (prog_void *)&cmd_goto_arg3,
805 /**********************************************************/
808 /* this structure is filled when cmd_position is parsed successfully */
809 struct cmd_position_result {
817 #define AUTOPOS_SPEED_FAST 200
818 static void auto_position(void)
821 uint16_t old_spdd, old_spda;
823 interrupt_traj_reset();
824 strat_get_speed(&old_spdd, &old_spda);
825 strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
827 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
830 strat_reset_pos(ROBOT_WIDTH/2 + 100,
831 COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
835 trajectory_d_rel(&mainboard.traj, -180);
836 err = wait_traj_end(END_INTR|END_TRAJ);
840 trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
841 err = wait_traj_end(END_INTR|END_TRAJ);
845 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
848 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
853 trajectory_d_rel(&mainboard.traj, -170);
854 err = wait_traj_end(END_INTR|END_TRAJ);
859 trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
860 err = wait_traj_end(END_INTR|END_TRAJ);
865 strat_set_speed(old_spdd, old_spda);
870 strat_set_speed(old_spdd, old_spda);
873 /* function called when cmd_position is parsed successfully */
874 static void cmd_position_parsed(void * parsed_result, void * data)
876 struct cmd_position_result * res = parsed_result;
878 /* display raw position values */
879 if (!strcmp_P(res->arg1, PSTR("reset"))) {
880 position_set(&mainboard.pos, 0, 0, 0);
882 else if (!strcmp_P(res->arg1, PSTR("set"))) {
883 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
885 else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
886 mainboard.our_color = I2C_COLOR_BLUE;
888 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
889 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
893 else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
894 mainboard.our_color = I2C_COLOR_YELLOW;
896 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
897 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
902 /* else it's just a "show" */
903 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
904 position_get_x_double(&mainboard.pos),
905 position_get_y_double(&mainboard.pos),
906 DEG(position_get_a_rad_double(&mainboard.pos)));
909 prog_char str_position_arg0[] = "position";
910 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
911 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
912 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
914 prog_char help_position[] = "Show/reset (x,y,a) position";
915 parse_pgm_inst_t cmd_position = {
916 .f = cmd_position_parsed, /* function to call */
917 .data = NULL, /* 2nd arg of func */
918 .help_str = help_position,
919 .tokens = { /* token list, NULL terminated */
920 (prog_void *)&cmd_position_arg0,
921 (prog_void *)&cmd_position_arg1,
927 prog_char str_position_arg1_set[] = "set";
928 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
929 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
930 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
931 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
933 prog_char help_position_set[] = "Set (x,y,a) position";
934 parse_pgm_inst_t cmd_position_set = {
935 .f = cmd_position_parsed, /* function to call */
936 .data = NULL, /* 2nd arg of func */
937 .help_str = help_position_set,
938 .tokens = { /* token list, NULL terminated */
939 (prog_void *)&cmd_position_arg0,
940 (prog_void *)&cmd_position_arg1_set,
941 (prog_void *)&cmd_position_arg2,
942 (prog_void *)&cmd_position_arg3,
943 (prog_void *)&cmd_position_arg4,
949 /**********************************************************/
950 /* strat configuration */
952 /* this structure is filled when cmd_strat_db is parsed successfully */
953 struct cmd_strat_db_result {
958 /* function called when cmd_strat_db is parsed successfully */
959 static void cmd_strat_db_parsed(void *parsed_result, void *data)
961 struct cmd_strat_db_result *res = parsed_result;
963 if (!strcmp_P(res->arg1, PSTR("reset"))) {
966 strat_db.dump_enabled = 1;
967 strat_db_dump(__FUNCTION__);
970 prog_char str_strat_db_arg0[] = "strat_db";
971 parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0);
972 prog_char str_strat_db_arg1[] = "show#reset";
973 parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1);
975 prog_char help_strat_db[] = "reset/show strat_db";
976 parse_pgm_inst_t cmd_strat_db = {
977 .f = cmd_strat_db_parsed, /* function to call */
978 .data = NULL, /* 2nd arg of func */
979 .help_str = help_strat_db,
980 .tokens = { /* token list, NULL terminated */
981 (prog_void *)&cmd_strat_db_arg0,
982 (prog_void *)&cmd_strat_db_arg1,
987 /**********************************************************/
988 /* strat configuration */
990 /* this structure is filled when cmd_strat_conf is parsed successfully */
991 struct cmd_strat_conf_result {
996 /* function called when cmd_strat_conf is parsed successfully */
997 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
999 // struct cmd_strat_conf_result *res = parsed_result;
1000 strat_conf.dump_enabled = 1;
1001 strat_conf_dump(__FUNCTION__);
1004 prog_char str_strat_conf_arg0[] = "strat_conf";
1005 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
1006 prog_char str_strat_conf_arg1[] = "show#base";
1007 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
1009 prog_char help_strat_conf[] = "configure strat options";
1010 parse_pgm_inst_t cmd_strat_conf = {
1011 .f = cmd_strat_conf_parsed, /* function to call */
1012 .data = NULL, /* 2nd arg of func */
1013 .help_str = help_strat_conf,
1014 .tokens = { /* token list, NULL terminated */
1015 (prog_void *)&cmd_strat_conf_arg0,
1016 (prog_void *)&cmd_strat_conf_arg1,
1021 /**********************************************************/
1022 /* strat configuration */
1024 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
1025 struct cmd_strat_conf2_result {
1026 fixed_string_t arg0;
1027 fixed_string_t arg1;
1028 fixed_string_t arg2;
1031 /* function called when cmd_strat_conf2 is parsed successfully */
1032 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
1034 struct cmd_strat_conf2_result *res = parsed_result;
1035 uint8_t on, bit = 0;
1037 if (!strcmp_P(res->arg2, PSTR("on")))
1043 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
1044 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
1045 else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
1046 bit = STRAT_CONF_BYPASS_STATIC2;
1047 else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
1048 bit = STRAT_CONF_TAKE_ONE_LINTEL;
1049 else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
1050 bit = STRAT_CONF_TAKE_ONE_LINTEL;
1051 else if (!strcmp_P(res->arg1, PSTR("store_static2")))
1052 bit = STRAT_CONF_STORE_STATIC2;
1053 else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
1054 bit = STRAT_CONF_BIG_3_TEMPLE;
1055 else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
1056 bit = STRAT_CONF_EARLY_SCAN;
1057 else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
1058 bit = STRAT_CONF_PUSH_OPP_COLS;
1062 strat_conf.flags |= bit;
1064 strat_conf.flags &= (~bit);
1066 strat_conf.dump_enabled = 1;
1067 strat_conf_dump(__FUNCTION__);
1070 prog_char str_strat_conf2_arg0[] = "strat_conf";
1071 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1072 prog_char str_strat_conf2_arg1[] = "faux";
1073 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1074 prog_char str_strat_conf2_arg2[] = "on#off";
1075 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1078 prog_char help_strat_conf2[] = "configure strat options";
1079 parse_pgm_inst_t cmd_strat_conf2 = {
1080 .f = cmd_strat_conf2_parsed, /* function to call */
1081 .data = NULL, /* 2nd arg of func */
1082 .help_str = help_strat_conf2,
1083 .tokens = { /* token list, NULL terminated */
1084 (prog_void *)&cmd_strat_conf2_arg0,
1085 (prog_void *)&cmd_strat_conf2_arg1,
1086 (prog_void *)&cmd_strat_conf2_arg2,
1091 /**********************************************************/
1092 /* strat configuration */
1094 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1095 struct cmd_strat_conf3_result {
1096 fixed_string_t arg0;
1097 fixed_string_t arg1;
1101 /* function called when cmd_strat_conf3 is parsed successfully */
1102 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1105 struct cmd_strat_conf3_result *res = parsed_result;
1107 if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1110 strat_conf.scan_opp_min_time = res->arg2;
1112 else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1115 strat_conf.delay_between_opp_scan = res->arg2;
1117 else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1120 strat_conf.scan_our_min_time = res->arg2;
1122 else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1125 strat_conf.delay_between_our_scan = res->arg2;
1127 else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1128 strat_conf.wait_opponent = res->arg2;
1130 else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1131 strat_conf.lintel_min_time = res->arg2;
1134 strat_conf.dump_enabled = 1;
1135 strat_conf_dump(__FUNCTION__);
1138 prog_char str_strat_conf3_arg0[] = "strat_conf";
1139 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1140 prog_char str_strat_conf3_arg1[] = "faux2";
1141 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1142 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1144 prog_char help_strat_conf3[] = "configure strat options";
1145 parse_pgm_inst_t cmd_strat_conf3 = {
1146 .f = cmd_strat_conf3_parsed, /* function to call */
1147 .data = NULL, /* 2nd arg of func */
1148 .help_str = help_strat_conf3,
1149 .tokens = { /* token list, NULL terminated */
1150 (prog_void *)&cmd_strat_conf3_arg0,
1151 (prog_void *)&cmd_strat_conf3_arg1,
1152 (prog_void *)&cmd_strat_conf3_arg2,
1158 /**********************************************************/
1161 //////////////////////
1165 #define TEST_SPEED 400
1168 static void line2line(double line1x1, double line1y1,
1169 double line1x2, double line1y2,
1170 double line2x1, double line2y1,
1171 double line2x2, double line2y2,
1172 double radius, double dist)
1175 double speed_d, speed_a;
1176 double distance, angle;
1177 double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
1178 double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
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 = line2_angle - line1_angle;
1195 distance = angle * radius;
1197 distance = -distance;
1198 angle = simple_modulo_2pi(angle);
1200 printf_P(PSTR("(%d,%d,%d) "),
1201 position_get_x_s16(&mainboard.pos),
1202 position_get_y_s16(&mainboard.pos),
1203 position_get_a_deg_s16(&mainboard.pos));
1204 printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
1207 /* take some margin on dist to avoid deceleration */
1208 trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
1210 /* circle exit condition */
1211 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1212 TRAJ_FLAGS_NO_NEAR);
1214 strat_set_speed(500, 500);
1215 printf_P(PSTR("(%d,%d,%d) "),
1216 position_get_x_s16(&mainboard.pos),
1217 position_get_y_s16(&mainboard.pos),
1218 position_get_a_deg_s16(&mainboard.pos));
1219 printf_P(PSTR("line\r\n"));
1220 trajectory_line_abs(&mainboard.traj,
1222 line2x2, line2y2, 150.);
1225 static void halfturn(double line1x1, double line1y1,
1226 double line1x2, double line1y2,
1227 double line2x1, double line2y1,
1228 double line2x2, double line2y2,
1229 double radius, double dist, double dir)
1232 double speed_d, speed_a;
1233 double distance, angle;
1235 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1237 strat_set_speed(TEST_SPEED, TEST_SPEED);
1238 quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1240 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1241 &speed_d, &speed_a);
1242 trajectory_line_abs(&mainboard.traj,
1244 line1x2, line1y2, 150.);
1245 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1246 dist, TRAJ_FLAGS_NO_NEAR);
1248 strat_set_speed(speed_d, speed_a);
1249 angle = dir * M_PI/2.;
1250 distance = angle * radius;
1252 distance = -distance;
1253 angle = simple_modulo_2pi(angle);
1256 /* take some margin on dist to avoid deceleration */
1257 DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
1259 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1261 /* circle exit condition */
1262 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1263 TRAJ_FLAGS_NO_NEAR);
1265 DEBUG(E_USER_STRAT, "miniline");
1266 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
1267 dist, TRAJ_FLAGS_NO_NEAR);
1268 DEBUG(E_USER_STRAT, "circle2");
1269 /* take some margin on dist to avoid deceleration */
1270 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1272 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1273 TRAJ_FLAGS_NO_NEAR);
1275 strat_set_speed(500, 500);
1276 DEBUG(E_USER_STRAT, "line");
1277 trajectory_line_abs(&mainboard.traj,
1279 line2x2, line2y2, 150.);
1283 /* function called when cmd_test is parsed successfully */
1284 static void subtraj_test(void)
1287 strat_reset_pos(400, 400, 90);
1288 mainboard.angle.on = 1;
1289 mainboard.distance.on = 1;
1291 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1292 while (!cmdline_keypressed()) {
1295 #define DIST_HARD_TURN 260
1296 #define RADIUS_HARD_TURN 100
1297 #define DIST_EASY_TURN 190
1298 #define RADIUS_EASY_TURN 190
1299 #define DIST_HALF_TURN 225
1300 #define RADIUS_HALF_TURN 130
1303 line2line(375, 597, 375, 1847,
1304 375, 1847, 1050, 1472,
1305 RADIUS_HARD_TURN, DIST_HARD_TURN);
1307 /* easy left and easy right !*/
1308 line2line(825, 1596, 1050, 1472,
1309 1050, 1472, 1500, 1722,
1310 RADIUS_EASY_TURN, DIST_EASY_TURN);
1311 line2line(1050, 1472, 1500, 1722,
1312 1500, 1722, 2175, 1347,
1313 RADIUS_EASY_TURN, DIST_EASY_TURN);
1314 line2line(1500, 1722, 2175, 1347,
1315 2175, 1347, 2175, 847,
1316 RADIUS_EASY_TURN, DIST_EASY_TURN);
1319 halfturn(2175, 1347, 2175, 722,
1320 2625, 722, 2625, 1597,
1321 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1322 halfturn(2625, 847, 2625, 1722,
1323 2175, 1722, 2175, 1097,
1324 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1327 line2line(2175, 1597, 2175, 1097,
1328 2175, 1097, 1500, 722,
1329 RADIUS_EASY_TURN, DIST_EASY_TURN);
1330 line2line(2175, 1097, 1500, 722,
1331 1500, 722, 1050, 972,
1332 RADIUS_EASY_TURN, DIST_EASY_TURN);
1333 line2line(1500, 722, 1050, 972,
1334 1050, 972, 375, 597,
1335 RADIUS_EASY_TURN, DIST_EASY_TURN);
1338 line2line(1050, 972, 375, 597,
1339 375, 597, 375, 1097,
1340 RADIUS_HARD_TURN, DIST_HARD_TURN);
1345 line2line(375, 597, 375, 1097,
1346 375, 1097, 1050, 1472,
1347 RADIUS_EASY_TURN, DIST_EASY_TURN);
1350 line2line(375, 1097, 1050, 1472,
1351 1050, 1472, 375, 1847,
1352 RADIUS_HARD_TURN, DIST_HARD_TURN);
1355 line2line(1050, 1472, 375, 1847,
1356 375, 1847, 375, 1347,
1357 RADIUS_HARD_TURN, DIST_HARD_TURN);
1360 line2line(375, 1847, 375, 1347,
1361 375, 1347, 1050, 972,
1362 RADIUS_EASY_TURN, DIST_EASY_TURN);
1365 line2line(375, 1347, 1050, 972,
1366 1050, 972, 375, 597,
1367 RADIUS_HARD_TURN, DIST_HARD_TURN);
1370 line2line(1050, 972, 375, 597,
1371 375, 597, 375, 1847,
1372 RADIUS_HARD_TURN, DIST_HARD_TURN);
1375 trajectory_hardstop(&mainboard.traj);
1379 /* this structure is filled when cmd_subtraj is parsed successfully */
1380 struct cmd_subtraj_result {
1381 fixed_string_t arg0;
1382 fixed_string_t arg1;
1389 /* function called when cmd_subtraj is parsed successfully */
1390 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1392 struct cmd_subtraj_result *res = parsed_result;
1394 if (!strcmp_P(res->arg1, PSTR("test")))
1397 trajectory_hardstop(&mainboard.traj);
1400 prog_char str_subtraj_arg0[] = "subtraj";
1401 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1402 prog_char str_subtraj_arg1[] = "faux";
1403 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1404 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1405 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1406 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1407 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1409 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1410 parse_pgm_inst_t cmd_subtraj = {
1411 .f = cmd_subtraj_parsed, /* function to call */
1412 .data = NULL, /* 2nd arg of func */
1413 .help_str = help_subtraj,
1414 .tokens = { /* token list, NULL terminated */
1415 (prog_void *)&cmd_subtraj_arg0,
1416 (prog_void *)&cmd_subtraj_arg1,
1417 (prog_void *)&cmd_subtraj_arg2,
1418 (prog_void *)&cmd_subtraj_arg3,
1419 (prog_void *)&cmd_subtraj_arg4,
1420 (prog_void *)&cmd_subtraj_arg5,