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