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);
828 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
831 strat_reset_pos(ROBOT_WIDTH/2 + 100,
832 COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
836 trajectory_d_rel(&mainboard.traj, -180);
837 err = wait_traj_end(END_INTR|END_TRAJ);
842 trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
843 err = wait_traj_end(END_INTR|END_TRAJ);
848 err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
851 strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
856 trajectory_d_rel(&mainboard.traj, -170);
857 err = wait_traj_end(END_INTR|END_TRAJ);
862 trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
863 err = wait_traj_end(END_INTR|END_TRAJ);
868 strat_set_speed(old_spdd, old_spda);
873 strat_set_speed(old_spdd, old_spda);
876 /* function called when cmd_position is parsed successfully */
877 static void cmd_position_parsed(void * parsed_result, void * data)
879 struct cmd_position_result * res = parsed_result;
881 /* display raw position values */
882 if (!strcmp_P(res->arg1, PSTR("reset"))) {
883 position_set(&mainboard.pos, 0, 0, 0);
885 else if (!strcmp_P(res->arg1, PSTR("set"))) {
886 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
888 else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
889 mainboard.our_color = I2C_COLOR_BLUE;
891 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
892 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
896 else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
897 mainboard.our_color = I2C_COLOR_YELLOW;
899 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
900 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
905 /* else it's just a "show" */
906 printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
907 position_get_x_double(&mainboard.pos),
908 position_get_y_double(&mainboard.pos),
909 DEG(position_get_a_rad_double(&mainboard.pos)));
912 prog_char str_position_arg0[] = "position";
913 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
914 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
915 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
917 prog_char help_position[] = "Show/reset (x,y,a) position";
918 parse_pgm_inst_t cmd_position = {
919 .f = cmd_position_parsed, /* function to call */
920 .data = NULL, /* 2nd arg of func */
921 .help_str = help_position,
922 .tokens = { /* token list, NULL terminated */
923 (prog_void *)&cmd_position_arg0,
924 (prog_void *)&cmd_position_arg1,
930 prog_char str_position_arg1_set[] = "set";
931 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
932 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
933 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
934 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
936 prog_char help_position_set[] = "Set (x,y,a) position";
937 parse_pgm_inst_t cmd_position_set = {
938 .f = cmd_position_parsed, /* function to call */
939 .data = NULL, /* 2nd arg of func */
940 .help_str = help_position_set,
941 .tokens = { /* token list, NULL terminated */
942 (prog_void *)&cmd_position_arg0,
943 (prog_void *)&cmd_position_arg1_set,
944 (prog_void *)&cmd_position_arg2,
945 (prog_void *)&cmd_position_arg3,
946 (prog_void *)&cmd_position_arg4,
952 /**********************************************************/
953 /* strat configuration */
955 /* this structure is filled when cmd_strat_db is parsed successfully */
956 struct cmd_strat_db_result {
961 /* function called when cmd_strat_db is parsed successfully */
962 static void cmd_strat_db_parsed(void *parsed_result, void *data)
964 struct cmd_strat_db_result *res = parsed_result;
966 if (!strcmp_P(res->arg1, PSTR("reset"))) {
969 strat_db.dump_enabled = 1;
970 strat_db_dump(__FUNCTION__);
973 prog_char str_strat_db_arg0[] = "strat_db";
974 parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0);
975 prog_char str_strat_db_arg1[] = "show#reset";
976 parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1);
978 prog_char help_strat_db[] = "reset/show strat_db";
979 parse_pgm_inst_t cmd_strat_db = {
980 .f = cmd_strat_db_parsed, /* function to call */
981 .data = NULL, /* 2nd arg of func */
982 .help_str = help_strat_db,
983 .tokens = { /* token list, NULL terminated */
984 (prog_void *)&cmd_strat_db_arg0,
985 (prog_void *)&cmd_strat_db_arg1,
990 /**********************************************************/
991 /* strat configuration */
993 /* this structure is filled when cmd_strat_conf is parsed successfully */
994 struct cmd_strat_conf_result {
999 /* function called when cmd_strat_conf is parsed successfully */
1000 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
1002 // struct cmd_strat_conf_result *res = parsed_result;
1003 strat_conf.dump_enabled = 1;
1004 strat_conf_dump(__FUNCTION__);
1007 prog_char str_strat_conf_arg0[] = "strat_conf";
1008 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
1009 prog_char str_strat_conf_arg1[] = "show#base";
1010 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
1012 prog_char help_strat_conf[] = "configure strat options";
1013 parse_pgm_inst_t cmd_strat_conf = {
1014 .f = cmd_strat_conf_parsed, /* function to call */
1015 .data = NULL, /* 2nd arg of func */
1016 .help_str = help_strat_conf,
1017 .tokens = { /* token list, NULL terminated */
1018 (prog_void *)&cmd_strat_conf_arg0,
1019 (prog_void *)&cmd_strat_conf_arg1,
1024 /**********************************************************/
1025 /* strat configuration */
1027 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
1028 struct cmd_strat_conf2_result {
1029 fixed_string_t arg0;
1030 fixed_string_t arg1;
1031 fixed_string_t arg2;
1034 /* function called when cmd_strat_conf2 is parsed successfully */
1035 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
1037 struct cmd_strat_conf2_result *res = parsed_result;
1038 uint8_t on, bit = 0;
1040 if (!strcmp_P(res->arg2, PSTR("on")))
1046 if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
1047 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
1048 else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
1049 bit = STRAT_CONF_BYPASS_STATIC2;
1050 else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
1051 bit = STRAT_CONF_TAKE_ONE_LINTEL;
1052 else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
1053 bit = STRAT_CONF_TAKE_ONE_LINTEL;
1054 else if (!strcmp_P(res->arg1, PSTR("store_static2")))
1055 bit = STRAT_CONF_STORE_STATIC2;
1056 else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
1057 bit = STRAT_CONF_BIG_3_TEMPLE;
1058 else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
1059 bit = STRAT_CONF_EARLY_SCAN;
1060 else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
1061 bit = STRAT_CONF_PUSH_OPP_COLS;
1065 strat_conf.flags |= bit;
1067 strat_conf.flags &= (~bit);
1069 strat_conf.dump_enabled = 1;
1070 strat_conf_dump(__FUNCTION__);
1073 prog_char str_strat_conf2_arg0[] = "strat_conf";
1074 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1075 prog_char str_strat_conf2_arg1[] = "faux";
1076 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1077 prog_char str_strat_conf2_arg2[] = "on#off";
1078 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1081 prog_char help_strat_conf2[] = "configure strat options";
1082 parse_pgm_inst_t cmd_strat_conf2 = {
1083 .f = cmd_strat_conf2_parsed, /* function to call */
1084 .data = NULL, /* 2nd arg of func */
1085 .help_str = help_strat_conf2,
1086 .tokens = { /* token list, NULL terminated */
1087 (prog_void *)&cmd_strat_conf2_arg0,
1088 (prog_void *)&cmd_strat_conf2_arg1,
1089 (prog_void *)&cmd_strat_conf2_arg2,
1094 /**********************************************************/
1095 /* strat configuration */
1097 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1098 struct cmd_strat_conf3_result {
1099 fixed_string_t arg0;
1100 fixed_string_t arg1;
1104 /* function called when cmd_strat_conf3 is parsed successfully */
1105 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1108 struct cmd_strat_conf3_result *res = parsed_result;
1110 if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1113 strat_conf.scan_opp_min_time = res->arg2;
1115 else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1118 strat_conf.delay_between_opp_scan = res->arg2;
1120 else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1123 strat_conf.scan_our_min_time = res->arg2;
1125 else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1128 strat_conf.delay_between_our_scan = res->arg2;
1130 else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1131 strat_conf.wait_opponent = res->arg2;
1133 else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1134 strat_conf.lintel_min_time = res->arg2;
1137 strat_conf.dump_enabled = 1;
1138 strat_conf_dump(__FUNCTION__);
1141 prog_char str_strat_conf3_arg0[] = "strat_conf";
1142 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1143 prog_char str_strat_conf3_arg1[] = "faux2";
1144 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1145 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1147 prog_char help_strat_conf3[] = "configure strat options";
1148 parse_pgm_inst_t cmd_strat_conf3 = {
1149 .f = cmd_strat_conf3_parsed, /* function to call */
1150 .data = NULL, /* 2nd arg of func */
1151 .help_str = help_strat_conf3,
1152 .tokens = { /* token list, NULL terminated */
1153 (prog_void *)&cmd_strat_conf3_arg0,
1154 (prog_void *)&cmd_strat_conf3_arg1,
1155 (prog_void *)&cmd_strat_conf3_arg2,
1161 /**********************************************************/
1164 //////////////////////
1168 #define TEST_SPEED 400
1171 static void line2line(double line1x1, double line1y1,
1172 double line1x2, double line1y2,
1173 double line2x1, double line2y1,
1174 double line2x2, double line2y2,
1175 double radius, double dist)
1178 double speed_d, speed_a;
1179 double distance, angle;
1180 double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
1181 double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
1183 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1185 strat_set_speed(TEST_SPEED, TEST_SPEED);
1186 quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1188 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1189 &speed_d, &speed_a);
1190 trajectory_line_abs(&mainboard.traj,
1192 line1x2, line1y2, 150.);
1193 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1194 dist, TRAJ_FLAGS_NO_NEAR);
1196 strat_set_speed(speed_d, speed_a);
1197 angle = line2_angle - line1_angle;
1198 distance = angle * radius;
1200 distance = -distance;
1201 angle = simple_modulo_2pi(angle);
1203 printf_P(PSTR("(%d,%d,%d) "),
1204 position_get_x_s16(&mainboard.pos),
1205 position_get_y_s16(&mainboard.pos),
1206 position_get_a_deg_s16(&mainboard.pos));
1207 printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
1210 /* take some margin on dist to avoid deceleration */
1211 trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
1213 /* circle exit condition */
1214 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1215 TRAJ_FLAGS_NO_NEAR);
1217 strat_set_speed(500, 500);
1218 printf_P(PSTR("(%d,%d,%d) "),
1219 position_get_x_s16(&mainboard.pos),
1220 position_get_y_s16(&mainboard.pos),
1221 position_get_a_deg_s16(&mainboard.pos));
1222 printf_P(PSTR("line\r\n"));
1223 trajectory_line_abs(&mainboard.traj,
1225 line2x2, line2y2, 150.);
1228 static void halfturn(double line1x1, double line1y1,
1229 double line1x2, double line1y2,
1230 double line2x1, double line2y1,
1231 double line2x2, double line2y2,
1232 double radius, double dist, double dir)
1235 double speed_d, speed_a;
1236 double distance, angle;
1238 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1240 strat_set_speed(TEST_SPEED, TEST_SPEED);
1241 quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1243 circle_get_da_speed_from_radius(&mainboard.traj, radius,
1244 &speed_d, &speed_a);
1245 trajectory_line_abs(&mainboard.traj,
1247 line1x2, line1y2, 150.);
1248 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1249 dist, TRAJ_FLAGS_NO_NEAR);
1251 strat_set_speed(speed_d, speed_a);
1252 angle = dir * M_PI/2.;
1253 distance = angle * radius;
1255 distance = -distance;
1256 angle = simple_modulo_2pi(angle);
1259 /* take some margin on dist to avoid deceleration */
1260 DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
1262 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1264 /* circle exit condition */
1265 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1266 TRAJ_FLAGS_NO_NEAR);
1268 DEBUG(E_USER_STRAT, "miniline");
1269 err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
1270 dist, TRAJ_FLAGS_NO_NEAR);
1271 DEBUG(E_USER_STRAT, "circle2");
1272 /* take some margin on dist to avoid deceleration */
1273 trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1275 err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1276 TRAJ_FLAGS_NO_NEAR);
1278 strat_set_speed(500, 500);
1279 DEBUG(E_USER_STRAT, "line");
1280 trajectory_line_abs(&mainboard.traj,
1282 line2x2, line2y2, 150.);
1286 /* function called when cmd_test is parsed successfully */
1287 static void subtraj_test(void)
1290 strat_reset_pos(400, 400, 90);
1291 mainboard.angle.on = 1;
1292 mainboard.distance.on = 1;
1294 printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1295 while (!cmdline_keypressed()) {
1298 #define DIST_HARD_TURN 260
1299 #define RADIUS_HARD_TURN 100
1300 #define DIST_EASY_TURN 190
1301 #define RADIUS_EASY_TURN 190
1302 #define DIST_HALF_TURN 225
1303 #define RADIUS_HALF_TURN 130
1306 line2line(375, 597, 375, 1847,
1307 375, 1847, 1050, 1472,
1308 RADIUS_HARD_TURN, DIST_HARD_TURN);
1310 /* easy left and easy right !*/
1311 line2line(825, 1596, 1050, 1472,
1312 1050, 1472, 1500, 1722,
1313 RADIUS_EASY_TURN, DIST_EASY_TURN);
1314 line2line(1050, 1472, 1500, 1722,
1315 1500, 1722, 2175, 1347,
1316 RADIUS_EASY_TURN, DIST_EASY_TURN);
1317 line2line(1500, 1722, 2175, 1347,
1318 2175, 1347, 2175, 847,
1319 RADIUS_EASY_TURN, DIST_EASY_TURN);
1322 halfturn(2175, 1347, 2175, 722,
1323 2625, 722, 2625, 1597,
1324 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1325 halfturn(2625, 847, 2625, 1722,
1326 2175, 1722, 2175, 1097,
1327 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1330 line2line(2175, 1597, 2175, 1097,
1331 2175, 1097, 1500, 722,
1332 RADIUS_EASY_TURN, DIST_EASY_TURN);
1333 line2line(2175, 1097, 1500, 722,
1334 1500, 722, 1050, 972,
1335 RADIUS_EASY_TURN, DIST_EASY_TURN);
1336 line2line(1500, 722, 1050, 972,
1337 1050, 972, 375, 597,
1338 RADIUS_EASY_TURN, DIST_EASY_TURN);
1341 line2line(1050, 972, 375, 597,
1342 375, 597, 375, 1097,
1343 RADIUS_HARD_TURN, DIST_HARD_TURN);
1348 line2line(375, 597, 375, 1097,
1349 375, 1097, 1050, 1472,
1350 RADIUS_EASY_TURN, DIST_EASY_TURN);
1353 line2line(375, 1097, 1050, 1472,
1354 1050, 1472, 375, 1847,
1355 RADIUS_HARD_TURN, DIST_HARD_TURN);
1358 line2line(1050, 1472, 375, 1847,
1359 375, 1847, 375, 1347,
1360 RADIUS_HARD_TURN, DIST_HARD_TURN);
1363 line2line(375, 1847, 375, 1347,
1364 375, 1347, 1050, 972,
1365 RADIUS_EASY_TURN, DIST_EASY_TURN);
1368 line2line(375, 1347, 1050, 972,
1369 1050, 972, 375, 597,
1370 RADIUS_HARD_TURN, DIST_HARD_TURN);
1373 line2line(1050, 972, 375, 597,
1374 375, 597, 375, 1847,
1375 RADIUS_HARD_TURN, DIST_HARD_TURN);
1378 trajectory_hardstop(&mainboard.traj);
1382 /* this structure is filled when cmd_subtraj is parsed successfully */
1383 struct cmd_subtraj_result {
1384 fixed_string_t arg0;
1385 fixed_string_t arg1;
1392 /* function called when cmd_subtraj is parsed successfully */
1393 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1395 struct cmd_subtraj_result *res = parsed_result;
1397 if (!strcmp_P(res->arg1, PSTR("test")))
1400 trajectory_hardstop(&mainboard.traj);
1403 prog_char str_subtraj_arg0[] = "subtraj";
1404 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1405 prog_char str_subtraj_arg1[] = "faux";
1406 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1407 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1408 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1409 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1410 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1412 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1413 parse_pgm_inst_t cmd_subtraj = {
1414 .f = cmd_subtraj_parsed, /* function to call */
1415 .data = NULL, /* 2nd arg of func */
1416 .help_str = help_subtraj,
1417 .tokens = { /* token list, NULL terminated */
1418 (prog_void *)&cmd_subtraj_arg0,
1419 (prog_void *)&cmd_subtraj_arg1,
1420 (prog_void *)&cmd_subtraj_arg2,
1421 (prog_void *)&cmd_subtraj_arg3,
1422 (prog_void *)&cmd_subtraj_arg4,
1423 (prog_void *)&cmd_subtraj_arg5,