projects/microb2010/mainboard/commands_traj.c

   1 /*
   2  *  Copyright Droids Corporation (2009)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25
  26 #include <aversive/pgmspace.h>
  27 #include <aversive/wait.h>
  28 #include <aversive/error.h>
  29
  30 #include <ax12.h>
  31 #include <uart.h>
  32 #include <pwm_ng.h>
  33 #include <clock_time.h>
  34 #include <spi.h>
  35 #include <encoders_spi.h>
  36
  37 #include <pid.h>
  38 #include <quadramp.h>
  39 #include <control_system_manager.h>
  40 #include <trajectory_manager.h>
  41 #include <vect_base.h>
  42 #include <lines.h>
  43 #include <polygon.h>
  44 #include <obstacle_avoidance.h>
  45 #include <blocking_detection_manager.h>
  46 #include <robot_system.h>
  47 #include <position_manager.h>
  48
  49 #include <rdline.h>
  50 #include <parse.h>
  51 #include <parse_string.h>
  52 #include <parse_num.h>
  53
  54 #include "main.h"
  55 #include "cs.h"
  56 #include "cmdline.h"
  57 #include "strat_utils.h"
  58 #include "strat_base.h"
  59 #include "strat.h"
  60 #include "../common/i2c_commands.h"
  61 #include "i2c_protocol.h"
  62
  63 /**********************************************************/
  64 /* Traj_Speeds for trajectory_manager */
  65
  66 /* this structure is filled when cmd_traj_speed is parsed successfully */
  67 struct cmd_traj_speed_result {
  68         fixed_string_t arg0;
  69         fixed_string_t arg1;
  70         uint16_t s;
  71 };
  72
  73 /* function called when cmd_traj_speed is parsed successfully */
  74 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
  75 {
  76         struct cmd_traj_speed_result * res = parsed_result;
  77
  78         if (!strcmp_P(res->arg1, PSTR("angle"))) {
  79                 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
  80         }
  81         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
  82                 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
  83         }
  84         /* else it is a "show" */
  85
  86         printf_P(PSTR("angle %u, distance %u\r\n"),
  87                  mainboard.traj.a_speed,
  88                  mainboard.traj.d_speed);
  89 }
  90
  91 prog_char str_traj_speed_arg0[] = "traj_speed";
  92 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
  93 prog_char str_traj_speed_arg1[] = "angle#distance";
  94 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
  95 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
  96
  97 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
  98 parse_pgm_inst_t cmd_traj_speed = {
  99         .f = cmd_traj_speed_parsed,  /* function to call */
 100         .data = NULL,      /* 2nd arg of func */
 101         .help_str = help_traj_speed,
 102         .tokens = {        /* token list, NULL terminated */
 103                 (prog_void *)&cmd_traj_speed_arg0,
 104                 (prog_void *)&cmd_traj_speed_arg1,
 105                 (prog_void *)&cmd_traj_speed_s,
 106                 NULL,
 107         },
 108 };
 109
 110 /* show */
 111
 112 prog_char str_traj_speed_show_arg[] = "show";
 113 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
 114
 115 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
 116 parse_pgm_inst_t cmd_traj_speed_show = {
 117         .f = cmd_traj_speed_parsed,  /* function to call */
 118         .data = NULL,      /* 2nd arg of func */
 119         .help_str = help_traj_speed_show,
 120         .tokens = {        /* token list, NULL terminated */
 121                 (prog_void *)&cmd_traj_speed_arg0,
 122                 (prog_void *)&cmd_traj_speed_show_arg,
 123                 NULL,
 124         },
 125 };
 126
 127 /**********************************************************/
 128 /* circle coef configuration */
 129
 130 /* this structure is filled when cmd_circle_coef is parsed successfully */
 131 struct cmd_circle_coef_result {
 132         fixed_string_t arg0;
 133         fixed_string_t arg1;
 134         float circle_coef;
 135 };
 136
 137
 138 /* function called when cmd_circle_coef is parsed successfully */
 139 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
 140 {
 141         struct cmd_circle_coef_result *res = parsed_result;
 142
 143         if (!strcmp_P(res->arg1, PSTR("set"))) {
 144                 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
 145         }
 146
 147         printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef);
 148 }
 149
 150 prog_char str_circle_coef_arg0[] = "circle_coef";
 151 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
 152 prog_char str_circle_coef_arg1[] = "set";
 153 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
 154 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
 155
 156 prog_char help_circle_coef[] = "Set circle coef";
 157 parse_pgm_inst_t cmd_circle_coef = {
 158         .f = cmd_circle_coef_parsed,  /* function to call */
 159         .data = NULL,      /* 2nd arg of func */
 160         .help_str = help_circle_coef,
 161         .tokens = {        /* token list, NULL terminated */
 162                 (prog_void *)&cmd_circle_coef_arg0,
 163                 (prog_void *)&cmd_circle_coef_arg1,
 164                 (prog_void *)&cmd_circle_coef_val,
 165                 NULL,
 166         },
 167 };
 168
 169 /* show */
 170
 171 prog_char str_circle_coef_show_arg[] = "show";
 172 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
 173
 174 prog_char help_circle_coef_show[] = "Show circle coef";
 175 parse_pgm_inst_t cmd_circle_coef_show = {
 176         .f = cmd_circle_coef_parsed,  /* function to call */
 177         .data = NULL,      /* 2nd arg of func */
 178         .help_str = help_circle_coef_show,
 179         .tokens = {        /* token list, NULL terminated */
 180                 (prog_void *)&cmd_circle_coef_arg0,
 181                 (prog_void *)&cmd_circle_coef_show_arg,
 182                 NULL,
 183         },
 184 };
 185
 186 /**********************************************************/
 187 /* trajectory window configuration */
 188
 189 /* this structure is filled when cmd_trajectory is parsed successfully */
 190 struct cmd_trajectory_result {
 191         fixed_string_t arg0;
 192         fixed_string_t arg1;
 193         float d_win;
 194         float a_win;
 195         float a_start;
 196 };
 197
 198
 199 /* function called when cmd_trajectory is parsed successfully */
 200 static void cmd_trajectory_parsed(void * parsed_result, void * data)
 201 {
 202         struct cmd_trajectory_result * res = parsed_result;
 203
 204         if (!strcmp_P(res->arg1, PSTR("set"))) {
 205                 trajectory_set_windows(&mainboard.traj, res->d_win,
 206                                        res->a_win, res->a_start);
 207         }
 208
 209         printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
 210                  DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
 211 }
 212
 213 prog_char str_trajectory_arg0[] = "trajectory";
 214 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
 215 prog_char str_trajectory_arg1[] = "set";
 216 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
 217 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
 218 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
 219 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
 220
 221 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
 222 parse_pgm_inst_t cmd_trajectory = {
 223         .f = cmd_trajectory_parsed,  /* function to call */
 224         .data = NULL,      /* 2nd arg of func */
 225         .help_str = help_trajectory,
 226         .tokens = {        /* token list, NULL terminated */
 227                 (prog_void *)&cmd_trajectory_arg0,
 228                 (prog_void *)&cmd_trajectory_arg1,
 229                 (prog_void *)&cmd_trajectory_d,
 230                 (prog_void *)&cmd_trajectory_a,
 231                 (prog_void *)&cmd_trajectory_as,
 232                 NULL,
 233         },
 234 };
 235
 236 /* show */
 237
 238 prog_char str_trajectory_show_arg[] = "show";
 239 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
 240
 241 prog_char help_trajectory_show[] = "Show trajectory window configuration";
 242 parse_pgm_inst_t cmd_trajectory_show = {
 243         .f = cmd_trajectory_parsed,  /* function to call */
 244         .data = NULL,      /* 2nd arg of func */
 245         .help_str = help_trajectory_show,
 246         .tokens = {        /* token list, NULL terminated */
 247                 (prog_void *)&cmd_trajectory_arg0,
 248                 (prog_void *)&cmd_trajectory_show_arg,
 249                 NULL,
 250         },
 251 };
 252
 253 /**********************************************************/
 254 /* rs_gains configuration */
 255
 256 /* this structure is filled when cmd_rs_gains is parsed successfully */
 257 struct cmd_rs_gains_result {
 258         fixed_string_t arg0;
 259         fixed_string_t arg1;
 260         float left;
 261         float right;
 262 };
 263
 264 /* function called when cmd_rs_gains is parsed successfully */
 265 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
 266 {
 267         struct cmd_rs_gains_result * res = parsed_result;
 268
 269         if (!strcmp_P(res->arg1, PSTR("set"))) {
 270                 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
 271                                         LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
 272                 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
 273                                          RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
 274         }
 275         printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
 276                  mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
 277 }
 278
 279 prog_char str_rs_gains_arg0[] = "rs_gains";
 280 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
 281 prog_char str_rs_gains_arg1[] = "set";
 282 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
 283 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
 284 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
 285
 286 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
 287 parse_pgm_inst_t cmd_rs_gains = {
 288         .f = cmd_rs_gains_parsed,  /* function to call */
 289         .data = NULL,      /* 2nd arg of func */
 290         .help_str = help_rs_gains,
 291         .tokens = {        /* token list, NULL terminated */
 292                 (prog_void *)&cmd_rs_gains_arg0,
 293                 (prog_void *)&cmd_rs_gains_arg1,
 294                 (prog_void *)&cmd_rs_gains_l,
 295                 (prog_void *)&cmd_rs_gains_r,
 296                 NULL,
 297         },
 298 };
 299
 300 /* show */
 301
 302 prog_char str_rs_gains_show_arg[] = "show";
 303 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
 304
 305 prog_char help_rs_gains_show[] = "Show rs_gains";
 306 parse_pgm_inst_t cmd_rs_gains_show = {
 307         .f = cmd_rs_gains_parsed,  /* function to call */
 308         .data = NULL,      /* 2nd arg of func */
 309         .help_str = help_rs_gains_show,
 310         .tokens = {        /* token list, NULL terminated */
 311                 (prog_void *)&cmd_rs_gains_arg0,
 312                 (prog_void *)&cmd_rs_gains_show_arg,
 313                 NULL,
 314         },
 315 };
 316
 317 /**********************************************************/
 318 /* track configuration */
 319
 320 /* this structure is filled when cmd_track is parsed successfully */
 321 struct cmd_track_result {
 322         fixed_string_t arg0;
 323         fixed_string_t arg1;
 324         float val;
 325 };
 326
 327 /* function called when cmd_track is parsed successfully */
 328 static void cmd_track_parsed(void * parsed_result, void * data)
 329 {
 330         struct cmd_track_result * res = parsed_result;
 331
 332         if (!strcmp_P(res->arg1, PSTR("set"))) {
 333                 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
 334         }
 335         printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
 336 }
 337
 338 prog_char str_track_arg0[] = "track";
 339 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
 340 prog_char str_track_arg1[] = "set";
 341 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
 342 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
 343
 344 prog_char help_track[] = "Set track in mm";
 345 parse_pgm_inst_t cmd_track = {
 346         .f = cmd_track_parsed,  /* function to call */
 347         .data = NULL,      /* 2nd arg of func */
 348         .help_str = help_track,
 349         .tokens = {        /* token list, NULL terminated */
 350                 (prog_void *)&cmd_track_arg0,
 351                 (prog_void *)&cmd_track_arg1,
 352                 (prog_void *)&cmd_track_val,
 353                 NULL,
 354         },
 355 };
 356
 357 /* show */
 358
 359 prog_char str_track_show_arg[] = "show";
 360 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
 361
 362 prog_char help_track_show[] = "Show track";
 363 parse_pgm_inst_t cmd_track_show = {
 364         .f = cmd_track_parsed,  /* function to call */
 365         .data = NULL,      /* 2nd arg of func */
 366         .help_str = help_track_show,
 367         .tokens = {        /* token list, NULL terminated */
 368                 (prog_void *)&cmd_track_arg0,
 369                 (prog_void *)&cmd_track_show_arg,
 370                 NULL,
 371         },
 372 };
 373
 374
 375
 376 /**********************************************************/
 377 /* Pt_Lists for testing traj */
 378
 379 #define PT_LIST_SIZE 10
 380 static struct xy_point pt_list[PT_LIST_SIZE];
 381 static uint16_t pt_list_len = 0;
 382
 383 /* this structure is filled when cmd_pt_list is parsed successfully */
 384 struct cmd_pt_list_result {
 385         fixed_string_t arg0;
 386         fixed_string_t arg1;
 387         uint16_t arg2;
 388         int16_t arg3;
 389         int16_t arg4;
 390 };
 391
 392 /* function called when cmd_pt_list is parsed successfully */
 393 static void cmd_pt_list_parsed(void * parsed_result, void * data)
 394 {
 395         struct cmd_pt_list_result * res = parsed_result;
 396         uint8_t i, why=0;
 397
 398         if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
 399                 printf_P(PSTR("not implemented\r\n"));
 400                 return;
 401         }
 402
 403         if (!strcmp_P(res->arg1, PSTR("append"))) {
 404                 res->arg2 = pt_list_len;
 405         }
 406
 407         if (!strcmp_P(res->arg1, PSTR("insert")) ||
 408             !strcmp_P(res->arg1, PSTR("append"))) {
 409                 if (res->arg2 > pt_list_len) {
 410                         printf_P(PSTR("Index too large\r\n"));
 411                         return;
 412                 }
 413                 if (pt_list_len >= PT_LIST_SIZE) {
 414                         printf_P(PSTR("List is too large\r\n"));
 415                         return;
 416                 }
 417                 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
 418                        PT_LIST_SIZE-1-res->arg2);
 419                 pt_list[res->arg2].x = res->arg3;
 420                 pt_list[res->arg2].y = res->arg4;
 421                 pt_list_len++;
 422         }
 423         else if (!strcmp_P(res->arg1, PSTR("del"))) {
 424                 if (pt_list_len <= 0) {
 425                         printf_P(PSTR("Error: list empty\r\n"));
 426                         return;
 427                 }
 428                 if (res->arg2 > pt_list_len) {
 429                         printf_P(PSTR("Index too large\r\n"));
 430                         return;
 431                 }
 432                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
 433                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
 434                 pt_list_len--;
 435         }
 436         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
 437                 pt_list_len = 0;
 438         }
 439
 440         /* else it is a "show" or a "start" */
 441         if (pt_list_len == 0) {
 442                 printf_P(PSTR("List empty\r\n"));
 443                 return;
 444         }
 445         for (i=0 ; i<pt_list_len ; i++) {
 446                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
 447                 if (!strcmp_P(res->arg1, PSTR("start"))) {
 448                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
 449                         why = wait_traj_end(0xFF); /* all */
 450                 }
 451 #if 0
 452                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
 453                         while (1) {
 454                                 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
 455                                 printf("next point\r\n");
 456                                 if (why != END_OBSTACLE)
 457                                         break;
 458                         }
 459                 }
 460 #endif
 461                 if (why & (~(END_TRAJ | END_NEAR)))
 462                         trajectory_stop(&mainboard.traj);
 463         }
 464 }
 465
 466 prog_char str_pt_list_arg0[] = "pt_list";
 467 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
 468 prog_char str_pt_list_arg1[] = "insert";
 469 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
 470 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
 471 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
 472 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
 473
 474 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
 475 parse_pgm_inst_t cmd_pt_list = {
 476         .f = cmd_pt_list_parsed,  /* function to call */
 477         .data = NULL,      /* 2nd arg of func */
 478         .help_str = help_pt_list,
 479         .tokens = {        /* token list, NULL terminated */
 480                 (prog_void *)&cmd_pt_list_arg0,
 481                 (prog_void *)&cmd_pt_list_arg1,
 482                 (prog_void *)&cmd_pt_list_arg2,
 483                 (prog_void *)&cmd_pt_list_arg3,
 484                 (prog_void *)&cmd_pt_list_arg4,
 485                 NULL,
 486         },
 487 };
 488
 489 /* append */
 490
 491 prog_char str_pt_list_arg1_append[] = "append";
 492 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
 493
 494 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
 495 parse_pgm_inst_t cmd_pt_list_append = {
 496         .f = cmd_pt_list_parsed,  /* function to call */
 497         .data = NULL,      /* 2nd arg of func */
 498         .help_str = help_pt_list_append,
 499         .tokens = {        /* token list, NULL terminated */
 500                 (prog_void *)&cmd_pt_list_arg0,
 501                 (prog_void *)&cmd_pt_list_arg1_append,
 502                 (prog_void *)&cmd_pt_list_arg3,
 503                 (prog_void *)&cmd_pt_list_arg4,
 504                 NULL,
 505         },
 506 };
 507
 508 /* del */
 509
 510 prog_char str_pt_list_del_arg[] = "del";
 511 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
 512
 513 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
 514 parse_pgm_inst_t cmd_pt_list_del = {
 515         .f = cmd_pt_list_parsed,  /* function to call */
 516         .data = NULL,      /* 2nd arg of func */
 517         .help_str = help_pt_list_del,
 518         .tokens = {        /* token list, NULL terminated */
 519                 (prog_void *)&cmd_pt_list_arg0,
 520                 (prog_void *)&cmd_pt_list_del_arg,
 521                 (prog_void *)&cmd_pt_list_arg2,
 522                 NULL,
 523         },
 524 };
 525 /* show */
 526
 527 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
 528 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
 529
 530 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
 531 parse_pgm_inst_t cmd_pt_list_show = {
 532         .f = cmd_pt_list_parsed,  /* function to call */
 533         .data = NULL,      /* 2nd arg of func */
 534         .help_str = help_pt_list_show,
 535         .tokens = {        /* token list, NULL terminated */
 536                 (prog_void *)&cmd_pt_list_arg0,
 537                 (prog_void *)&cmd_pt_list_show_arg,
 538                 NULL,
 539         },
 540 };
 541
 542
 543
 544 /**********************************************************/
 545 /* Goto function */
 546
 547 /* this structure is filled when cmd_goto is parsed successfully */
 548 struct cmd_goto_result {
 549         fixed_string_t arg0;
 550         fixed_string_t arg1;
 551         int32_t arg2;
 552         int32_t arg3;
 553         int32_t arg4;
 554 };
 555
 556 /* function called when cmd_goto is parsed successfully */
 557 static void cmd_goto_parsed(void * parsed_result, void * data)
 558 {
 559         struct cmd_goto_result * res = parsed_result;
 560         uint8_t err;
 561         microseconds t1, t2;
 562
 563         interrupt_traj_reset();
 564         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
 565                 trajectory_a_rel(&mainboard.traj, res->arg2);
 566         }
 567         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
 568                 trajectory_d_rel(&mainboard.traj, res->arg2);
 569         }
 570         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
 571                 trajectory_a_abs(&mainboard.traj, res->arg2);
 572         }
 573         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
 574                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
 575         }
 576         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
 577                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
 578         }
 579         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
 580                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
 581         }
 582         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
 583                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 584         }
 585         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
 586 #if 0
 587                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
 588                 if (err != END_TRAJ && err != END_NEAR)
 589                         strat_hardstop();
 590 #else
 591                 printf_P(PSTR("not implemented\r\n"));
 592                 return;
 593 #endif
 594         }
 595         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
 596 #if 0
 597                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
 598                 if (err != END_TRAJ && err != END_NEAR)
 599                         strat_hardstop();
 600 #else
 601                 printf_P(PSTR("not implemented\r\n"));
 602                 return;
 603 #endif
 604         }
 605         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
 606                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 607         }
 608         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
 609                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 610         }
 611         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
 612                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
 613         }
 614         t1 = time_get_us2();
 615         while ((err = test_traj_end(0xFF)) == 0) {
 616                 t2 = time_get_us2();
 617                 if (t2 - t1 > 200000) {
 618                         dump_cs_debug("angle", &mainboard.angle.cs);
 619                         dump_cs_debug("distance", &mainboard.distance.cs);
 620                         t1 = t2;
 621                 }
 622         }
 623         if (err != END_TRAJ && err != END_NEAR)
 624                 strat_hardstop();
 625         printf_P(PSTR("returned %s\r\n"), get_err(err));
 626 }
 627
 628 prog_char str_goto_arg0[] = "goto";
 629 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
 630 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
 631 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
 632 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
 633
 634 /* 1 params */
 635 prog_char help_goto1[] = "Change orientation of the mainboard";
 636 parse_pgm_inst_t cmd_goto1 = {
 637         .f = cmd_goto_parsed,  /* function to call */
 638         .data = NULL,      /* 2nd arg of func */
 639         .help_str = help_goto1,
 640         .tokens = {        /* token list, NULL terminated */
 641                 (prog_void *)&cmd_goto_arg0,
 642                 (prog_void *)&cmd_goto_arg1_a,
 643                 (prog_void *)&cmd_goto_arg2,
 644                 NULL,
 645         },
 646 };
 647
 648 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";
 649 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
 650 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
 651
 652 /* 2 params */
 653 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
 654 parse_pgm_inst_t cmd_goto2 = {
 655         .f = cmd_goto_parsed,  /* function to call */
 656         .data = NULL,      /* 2nd arg of func */
 657         .help_str = help_goto2,
 658         .tokens = {        /* token list, NULL terminated */
 659                 (prog_void *)&cmd_goto_arg0,
 660                 (prog_void *)&cmd_goto_arg1_b,
 661                 (prog_void *)&cmd_goto_arg2,
 662                 (prog_void *)&cmd_goto_arg3,
 663                 NULL,
 664         },
 665 };
 666
 667 /**********************************************************/
 668 /* Position tests */
 669
 670 /* this structure is filled when cmd_position is parsed successfully */
 671 struct cmd_position_result {
 672         fixed_string_t arg0;
 673         fixed_string_t arg1;
 674         int32_t arg2;
 675         int32_t arg3;
 676         int32_t arg4;
 677 };
 678
 679 #define AUTOPOS_SPEED_FAST 200
 680 static void auto_position(void)
 681 {
 682         uint8_t err;
 683         uint16_t old_spdd, old_spda;
 684
 685         interrupt_traj_reset();
 686         strat_get_speed(&old_spdd, &old_spda);
 687         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
 688
 689         trajectory_d_rel(&mainboard.traj, -300);
 690         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
 691         if (err == END_INTR)
 692                 goto intr;
 693         wait_ms(100);
 694         strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
 695
 696         trajectory_d_rel(&mainboard.traj, 120);
 697         err = wait_traj_end(END_INTR|END_TRAJ);
 698         if (err == END_INTR)
 699                 goto intr;
 700
 701         trajectory_a_rel(&mainboard.traj, COLOR_A(90));
 702         err = wait_traj_end(END_INTR|END_TRAJ);
 703         if (err == END_INTR)
 704                 goto intr;
 705
 706         trajectory_d_rel(&mainboard.traj, -300);
 707         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
 708         if (err == END_INTR)
 709                 goto intr;
 710         wait_ms(100);
 711         strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
 712                         COLOR_A(90));
 713
 714         trajectory_d_rel(&mainboard.traj, 120);
 715         err = wait_traj_end(END_INTR|END_TRAJ);
 716         if (err == END_INTR)
 717                 goto intr;
 718         wait_ms(100);
 719
 720         trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
 721         err = wait_traj_end(END_INTR|END_TRAJ);
 722         if (err == END_INTR)
 723                 goto intr;
 724         wait_ms(100);
 725
 726         strat_set_speed(old_spdd, old_spda);
 727         return;
 728
 729 intr:
 730         strat_hardstop();
 731         strat_set_speed(old_spdd, old_spda);
 732 }
 733
 734 /* function called when cmd_position is parsed successfully */
 735 static void cmd_position_parsed(void * parsed_result, void * data)
 736 {
 737         struct cmd_position_result * res = parsed_result;
 738
 739         /* display raw position values */
 740         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 741                 position_set(&mainboard.pos, 0, 0, 0);
 742         }
 743         else if (!strcmp_P(res->arg1, PSTR("set"))) {
 744                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
 745         }
 746         else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
 747                 mainboard.our_color = I2C_COLOR_BLUE;
 748                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
 749                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
 750                 auto_position();
 751         }
 752         else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
 753                 mainboard.our_color = I2C_COLOR_YELLOW;
 754                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
 755                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
 756                 auto_position();
 757         }
 758
 759         /* else it's just a "show" */
 760         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
 761                  position_get_x_double(&mainboard.pos),
 762                  position_get_y_double(&mainboard.pos),
 763                  DEG(position_get_a_rad_double(&mainboard.pos)));
 764 }
 765
 766 prog_char str_position_arg0[] = "position";
 767 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
 768 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_red";
 769 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
 770
 771 prog_char help_position[] = "Show/reset (x,y,a) position";
 772 parse_pgm_inst_t cmd_position = {
 773         .f = cmd_position_parsed,  /* function to call */
 774         .data = NULL,      /* 2nd arg of func */
 775         .help_str = help_position,
 776         .tokens = {        /* token list, NULL terminated */
 777                 (prog_void *)&cmd_position_arg0,
 778                 (prog_void *)&cmd_position_arg1,
 779                 NULL,
 780         },
 781 };
 782
 783
 784 prog_char str_position_arg1_set[] = "set";
 785 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
 786 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
 787 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
 788 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
 789
 790 prog_char help_position_set[] = "Set (x,y,a) position";
 791 parse_pgm_inst_t cmd_position_set = {
 792         .f = cmd_position_parsed,  /* function to call */
 793         .data = NULL,      /* 2nd arg of func */
 794         .help_str = help_position_set,
 795         .tokens = {        /* token list, NULL terminated */
 796                 (prog_void *)&cmd_position_arg0,
 797                 (prog_void *)&cmd_position_arg1_set,
 798                 (prog_void *)&cmd_position_arg2,
 799                 (prog_void *)&cmd_position_arg3,
 800                 (prog_void *)&cmd_position_arg4,
 801                 NULL,
 802         },
 803 };
 804
 805
 806 /**********************************************************/
 807 /* strat configuration */
 808
 809 /* this structure is filled when cmd_strat_infos is parsed successfully */
 810 struct cmd_strat_infos_result {
 811         fixed_string_t arg0;
 812         fixed_string_t arg1;
 813 };
 814
 815 /* function called when cmd_strat_infos is parsed successfully */
 816 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
 817 {
 818         struct cmd_strat_infos_result *res = parsed_result;
 819
 820         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 821                 strat_reset_infos();
 822         }
 823         strat_infos.dump_enabled = 1;
 824         strat_dump_infos(__FUNCTION__);
 825 }
 826
 827 prog_char str_strat_infos_arg0[] = "strat_infos";
 828 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
 829 prog_char str_strat_infos_arg1[] = "show#reset";
 830 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
 831
 832 prog_char help_strat_infos[] = "reset/show strat_infos";
 833 parse_pgm_inst_t cmd_strat_infos = {
 834         .f = cmd_strat_infos_parsed,  /* function to call */
 835         .data = NULL,      /* 2nd arg of func */
 836         .help_str = help_strat_infos,
 837         .tokens = {        /* token list, NULL terminated */
 838                 (prog_void *)&cmd_strat_infos_arg0,
 839                 (prog_void *)&cmd_strat_infos_arg1,
 840                 NULL,
 841         },
 842 };
 843
 844 /**********************************************************/
 845 /* strat configuration */
 846
 847 /* this structure is filled when cmd_strat_conf is parsed successfully */
 848 struct cmd_strat_conf_result {
 849         fixed_string_t arg0;
 850         fixed_string_t arg1;
 851 };
 852
 853 /* function called when cmd_strat_conf is parsed successfully */
 854 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
 855 {
 856         //      struct cmd_strat_conf_result *res = parsed_result;
 857
 858         strat_infos.dump_enabled = 1;
 859         strat_dump_conf();
 860 }
 861
 862 prog_char str_strat_conf_arg0[] = "strat_conf";
 863 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
 864 prog_char str_strat_conf_arg1[] = "show#base";
 865 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
 866
 867 prog_char help_strat_conf[] = "configure strat options";
 868 parse_pgm_inst_t cmd_strat_conf = {
 869         .f = cmd_strat_conf_parsed,  /* function to call */
 870         .data = NULL,      /* 2nd arg of func */
 871         .help_str = help_strat_conf,
 872         .tokens = {        /* token list, NULL terminated */
 873                 (prog_void *)&cmd_strat_conf_arg0,
 874                 (prog_void *)&cmd_strat_conf_arg1,
 875                 NULL,
 876         },
 877 };
 878
 879 /**********************************************************/
 880 /* strat configuration */
 881
 882 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
 883 struct cmd_strat_conf2_result {
 884         fixed_string_t arg0;
 885         fixed_string_t arg1;
 886         fixed_string_t arg2;
 887 };
 888
 889 /* function called when cmd_strat_conf2 is parsed successfully */
 890 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
 891 {
 892         struct cmd_strat_conf2_result *res = parsed_result;
 893         uint8_t on, bit = 0;
 894
 895         if (!strcmp_P(res->arg2, PSTR("on")))
 896                 on = 1;
 897         else
 898                 on = 0;
 899
 900 #if 0
 901         if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
 902                 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
 903         else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
 904                 bit = STRAT_CONF_BYPASS_STATIC2;
 905         else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
 906                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
 907         else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
 908                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
 909         else if (!strcmp_P(res->arg1, PSTR("store_static2")))
 910                 bit = STRAT_CONF_STORE_STATIC2;
 911         else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
 912                 bit = STRAT_CONF_BIG_3_TEMPLE;
 913         else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
 914                 bit = STRAT_CONF_EARLY_SCAN;
 915         else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
 916                 bit = STRAT_CONF_PUSH_OPP_COLS;
 917 #endif
 918
 919         if (on)
 920                 strat_infos.conf.flags |= bit;
 921         else
 922                 strat_infos.conf.flags &= (~bit);
 923
 924         strat_infos.dump_enabled = 1;
 925         strat_dump_conf();
 926 }
 927
 928 prog_char str_strat_conf2_arg0[] = "strat_conf";
 929 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
 930 prog_char str_strat_conf2_arg1[] = "faux";
 931 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
 932 prog_char str_strat_conf2_arg2[] = "on#off";
 933 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
 934
 935
 936 prog_char help_strat_conf2[] = "configure strat options";
 937 parse_pgm_inst_t cmd_strat_conf2 = {
 938         .f = cmd_strat_conf2_parsed,  /* function to call */
 939         .data = NULL,      /* 2nd arg of func */
 940         .help_str = help_strat_conf2,
 941         .tokens = {        /* token list, NULL terminated */
 942                 (prog_void *)&cmd_strat_conf2_arg0,
 943                 (prog_void *)&cmd_strat_conf2_arg1,
 944                 (prog_void *)&cmd_strat_conf2_arg2,
 945                 NULL,
 946         },
 947 };
 948
 949 /**********************************************************/
 950 /* strat configuration */
 951
 952 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
 953 struct cmd_strat_conf3_result {
 954         fixed_string_t arg0;
 955         fixed_string_t arg1;
 956         uint8_t arg2;
 957 };
 958
 959 /* function called when cmd_strat_conf3 is parsed successfully */
 960 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
 961 {
 962 #if 0
 963         struct cmd_strat_conf3_result *res = parsed_result;
 964
 965         if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
 966                 if (res->arg2 > 90)
 967                         res->arg2 = 90;
 968                 strat_infos.conf.scan_opp_min_time = res->arg2;
 969         }
 970         else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
 971                 if (res->arg2 > 90)
 972                         res->arg2 = 90;
 973                 strat_infos.conf.delay_between_opp_scan = res->arg2;
 974         }
 975         else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
 976                 if (res->arg2 > 90)
 977                         res->arg2 = 90;
 978                 strat_infos.conf.scan_our_min_time = res->arg2;
 979         }
 980         else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
 981                 if (res->arg2 > 90)
 982                         res->arg2 = 90;
 983                 strat_infos.conf.delay_between_our_scan = res->arg2;
 984         }
 985         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
 986                 strat_infos.conf.wait_opponent = res->arg2;
 987         }
 988         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
 989                 strat_infos.conf.lintel_min_time = res->arg2;
 990         }
 991 #endif
 992         strat_infos.dump_enabled = 1;
 993         strat_dump_conf();
 994 }
 995
 996 prog_char str_strat_conf3_arg0[] = "strat_conf";
 997 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
 998 prog_char str_strat_conf3_arg1[] = "faux2";
 999 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1000 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1001
1002 prog_char help_strat_conf3[] = "configure strat options";
1003 parse_pgm_inst_t cmd_strat_conf3 = {
1004         .f = cmd_strat_conf3_parsed,  /* function to call */
1005         .data = NULL,      /* 2nd arg of func */
1006         .help_str = help_strat_conf3,
1007         .tokens = {        /* token list, NULL terminated */
1008                 (prog_void *)&cmd_strat_conf3_arg0,
1009                 (prog_void *)&cmd_strat_conf3_arg1,
1010                 (prog_void *)&cmd_strat_conf3_arg2,
1011                 NULL,
1012         },
1013 };
1014
1015
1016 /**********************************************************/
1017 /* Subtraj */
1018
1019 /* this structure is filled when cmd_subtraj is parsed successfully */
1020 struct cmd_subtraj_result {
1021         fixed_string_t arg0;
1022         fixed_string_t arg1;
1023         int32_t arg2;
1024         int32_t arg3;
1025         int32_t arg4;
1026         int32_t arg5;
1027 };
1028
1029 /* function called when cmd_subtraj is parsed successfully */
1030 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1031 {
1032 /*      struct cmd_subtraj_result *res = parsed_result; */
1033
1034         printf_P(PSTR("TODO\r\n"));
1035         trajectory_hardstop(&mainboard.traj);
1036 }
1037
1038 prog_char str_subtraj_arg0[] = "subtraj";
1039 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1040 prog_char str_subtraj_arg1[] = "faux";
1041 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1042 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1043 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1044 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1045 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1046
1047 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1048 parse_pgm_inst_t cmd_subtraj = {
1049         .f = cmd_subtraj_parsed,  /* function to call */
1050         .data = NULL,      /* 2nd arg of func */
1051         .help_str = help_subtraj,
1052         .tokens = {        /* token list, NULL terminated */
1053                 (prog_void *)&cmd_subtraj_arg0,
1054                 (prog_void *)&cmd_subtraj_arg1,
1055                 (prog_void *)&cmd_subtraj_arg2,
1056                 (prog_void *)&cmd_subtraj_arg3,
1057                 (prog_void *)&cmd_subtraj_arg4,
1058                 (prog_void *)&cmd_subtraj_arg5,
1059                 NULL,
1060         },
1061 };