projects/microb2009/mainboard/commands_traj.c

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