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