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