projects/microb2010/mainboard/commands_traj.c

   1 /*
   2  *  Copyright Droids Corporation (2009)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25
  26 #include <aversive/pgmspace.h>
  27 #include <aversive/wait.h>
  28 #include <aversive/error.h>
  29
  30 #include <ax12.h>
  31 #include <uart.h>
  32 #include <pwm_ng.h>
  33 #include <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 };
 493
 494 /* function called when cmd_goto is parsed successfully */
 495 static void cmd_goto_parsed(void * parsed_result, void * data)
 496 {
 497         struct cmd_goto_result * res = parsed_result;
 498         uint8_t err;
 499         microseconds t1, t2;
 500
 501         interrupt_traj_reset();
 502         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
 503                 trajectory_a_rel(&mainboard.traj, res->arg2);
 504         }
 505         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
 506                 trajectory_d_rel(&mainboard.traj, res->arg2);
 507         }
 508         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
 509                 trajectory_a_abs(&mainboard.traj, res->arg2);
 510         }
 511         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
 512                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
 513         }
 514         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
 515                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
 516         }
 517         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
 518                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
 519         }
 520         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
 521                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 522         }
 523         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
 524                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
 525                 if (err != END_TRAJ && err != END_NEAR)
 526                         strat_hardstop();
 527         }
 528         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
 529                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
 530                 if (err != END_TRAJ && err != END_NEAR)
 531                         strat_hardstop();
 532         }
 533         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
 534                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 535         }
 536         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
 537                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 538         }
 539         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
 540                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
 541         }
 542         t1 = time_get_us2();
 543         while ((err = test_traj_end(0xFF)) == 0) {
 544                 t2 = time_get_us2();
 545                 if (t2 - t1 > 200000) {
 546                         dump_cs_debug("angle", &mainboard.angle.cs);
 547                         dump_cs_debug("distance", &mainboard.distance.cs);
 548                         t1 = t2;
 549                 }
 550         }
 551         if (err != END_TRAJ && err != END_NEAR)
 552                 strat_hardstop();
 553         printf_P(PSTR("returned %s\r\n"), get_err(err));
 554 }
 555
 556 prog_char str_goto_arg0[] = "goto";
 557 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
 558 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
 559 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
 560 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
 561
 562 /* 1 params */
 563 prog_char help_goto1[] = "Change orientation of the mainboard";
 564 parse_pgm_inst_t cmd_goto1 = {
 565         .f = cmd_goto_parsed,  /* function to call */
 566         .data = NULL,      /* 2nd arg of func */
 567         .help_str = help_goto1,
 568         .tokens = {        /* token list, NULL terminated */
 569                 (prog_void *)&cmd_goto_arg0,
 570                 (prog_void *)&cmd_goto_arg1_a,
 571                 (prog_void *)&cmd_goto_arg2,
 572                 NULL,
 573         },
 574 };
 575
 576 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";
 577 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
 578 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
 579
 580 /* 2 params */
 581 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
 582 parse_pgm_inst_t cmd_goto2 = {
 583         .f = cmd_goto_parsed,  /* function to call */
 584         .data = NULL,      /* 2nd arg of func */
 585         .help_str = help_goto2,
 586         .tokens = {        /* token list, NULL terminated */
 587                 (prog_void *)&cmd_goto_arg0,
 588                 (prog_void *)&cmd_goto_arg1_b,
 589                 (prog_void *)&cmd_goto_arg2,
 590                 (prog_void *)&cmd_goto_arg3,
 591                 NULL,
 592         },
 593 };
 594
 595 /**********************************************************/
 596 /* Position tests */
 597
 598 /* this structure is filled when cmd_position is parsed successfully */
 599 struct cmd_position_result {
 600         fixed_string_t arg0;
 601         fixed_string_t arg1;
 602         int32_t arg2;
 603         int32_t arg3;
 604         int32_t arg4;
 605 };
 606
 607 #define AUTOPOS_SPEED_FAST 200
 608 static void auto_position(void)
 609 {
 610         uint8_t err;
 611         uint16_t old_spdd, old_spda;
 612
 613         interrupt_traj_reset();
 614         strat_get_speed(&old_spdd, &old_spda);
 615         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
 616
 617         trajectory_d_rel(&mainboard.traj, -300);
 618         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
 619         if (err == END_INTR)
 620                 goto intr;
 621         wait_ms(100);
 622         strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
 623
 624         trajectory_d_rel(&mainboard.traj, 120);
 625         err = wait_traj_end(END_INTR|END_TRAJ);
 626         if (err == END_INTR)
 627                 goto intr;
 628
 629         trajectory_a_rel(&mainboard.traj, COLOR_A(90));
 630         err = wait_traj_end(END_INTR|END_TRAJ);
 631         if (err == END_INTR)
 632                 goto intr;
 633
 634         trajectory_d_rel(&mainboard.traj, -300);
 635         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
 636         if (err == END_INTR)
 637                 goto intr;
 638         wait_ms(100);
 639         strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2),
 640                         COLOR_A(90));
 641
 642         trajectory_d_rel(&mainboard.traj, 120);
 643         err = wait_traj_end(END_INTR|END_TRAJ);
 644         if (err == END_INTR)
 645                 goto intr;
 646         wait_ms(100);
 647
 648         trajectory_a_rel(&mainboard.traj, COLOR_A(-40));
 649         err = wait_traj_end(END_INTR|END_TRAJ);
 650         if (err == END_INTR)
 651                 goto intr;
 652         wait_ms(100);
 653
 654         strat_set_speed(old_spdd, old_spda);
 655         return;
 656
 657 intr:
 658         strat_hardstop();
 659         strat_set_speed(old_spdd, old_spda);
 660 }
 661
 662 /* function called when cmd_position is parsed successfully */
 663 static void cmd_position_parsed(void * parsed_result, void * data)
 664 {
 665         struct cmd_position_result * res = parsed_result;
 666
 667         /* display raw position values */
 668         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 669                 position_set(&mainboard.pos, 0, 0, 0);
 670         }
 671         else if (!strcmp_P(res->arg1, PSTR("set"))) {
 672                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
 673         }
 674         else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) {
 675                 mainboard.our_color = I2C_COLOR_GREEN;
 676                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN);
 677                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN);
 678                 auto_position();
 679         }
 680         else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
 681                 mainboard.our_color = I2C_COLOR_RED;
 682                 i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED);
 683                 i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED);
 684                 auto_position();
 685         }
 686
 687         /* else it's just a "show" */
 688         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
 689                  position_get_x_double(&mainboard.pos),
 690                  position_get_y_double(&mainboard.pos),
 691                  DEG(position_get_a_rad_double(&mainboard.pos)));
 692 }
 693
 694 prog_char str_position_arg0[] = "position";
 695 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
 696 prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red";
 697 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
 698
 699 prog_char help_position[] = "Show/reset (x,y,a) position";
 700 parse_pgm_inst_t cmd_position = {
 701         .f = cmd_position_parsed,  /* function to call */
 702         .data = NULL,      /* 2nd arg of func */
 703         .help_str = help_position,
 704         .tokens = {        /* token list, NULL terminated */
 705                 (prog_void *)&cmd_position_arg0,
 706                 (prog_void *)&cmd_position_arg1,
 707                 NULL,
 708         },
 709 };
 710
 711
 712 prog_char str_position_arg1_set[] = "set";
 713 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
 714 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
 715 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
 716 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
 717
 718 prog_char help_position_set[] = "Set (x,y,a) position";
 719 parse_pgm_inst_t cmd_position_set = {
 720         .f = cmd_position_parsed,  /* function to call */
 721         .data = NULL,      /* 2nd arg of func */
 722         .help_str = help_position_set,
 723         .tokens = {        /* token list, NULL terminated */
 724                 (prog_void *)&cmd_position_arg0,
 725                 (prog_void *)&cmd_position_arg1_set,
 726                 (prog_void *)&cmd_position_arg2,
 727                 (prog_void *)&cmd_position_arg3,
 728                 (prog_void *)&cmd_position_arg4,
 729                 NULL,
 730         },
 731 };
 732
 733
 734 /**********************************************************/
 735 /* strat configuration */
 736
 737 /* this structure is filled when cmd_strat_infos is parsed successfully */
 738 struct cmd_strat_infos_result {
 739         fixed_string_t arg0;
 740         fixed_string_t arg1;
 741 };
 742
 743 /* function called when cmd_strat_infos is parsed successfully */
 744 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
 745 {
 746         struct cmd_strat_infos_result *res = parsed_result;
 747
 748         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 749                 strat_reset_infos();
 750         }
 751         strat_infos.dump_enabled = 1;
 752         strat_dump_infos(__FUNCTION__);
 753 }
 754
 755 prog_char str_strat_infos_arg0[] = "strat_infos";
 756 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
 757 prog_char str_strat_infos_arg1[] = "show#reset";
 758 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
 759
 760 prog_char help_strat_infos[] = "reset/show strat_infos";
 761 parse_pgm_inst_t cmd_strat_infos = {
 762         .f = cmd_strat_infos_parsed,  /* function to call */
 763         .data = NULL,      /* 2nd arg of func */
 764         .help_str = help_strat_infos,
 765         .tokens = {        /* token list, NULL terminated */
 766                 (prog_void *)&cmd_strat_infos_arg0,
 767                 (prog_void *)&cmd_strat_infos_arg1,
 768                 NULL,
 769         },
 770 };
 771
 772 /**********************************************************/
 773 /* strat configuration */
 774
 775 /* this structure is filled when cmd_strat_conf is parsed successfully */
 776 struct cmd_strat_conf_result {
 777         fixed_string_t arg0;
 778         fixed_string_t arg1;
 779 };
 780
 781 /* function called when cmd_strat_conf is parsed successfully */
 782 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
 783 {
 784         struct cmd_strat_conf_result *res = parsed_result;
 785
 786         if (!strcmp_P(res->arg1, PSTR("base"))) {
 787                 strat_infos.conf.flags = 0;
 788                 strat_infos.conf.scan_our_min_time = 90;
 789                 strat_infos.conf.delay_between_our_scan = 90;
 790                 strat_infos.conf.scan_opp_min_time = 90;
 791                 strat_infos.conf.delay_between_opp_scan = 90;
 792         }
 793         else if (!strcmp_P(res->arg1, PSTR("big3"))) {
 794                 strat_infos.conf.flags =
 795                         STRAT_CONF_STORE_STATIC2 |
 796                         STRAT_CONF_BIG_3_TEMPLE;
 797                 strat_infos.conf.scan_our_min_time = 90;
 798                 strat_infos.conf.delay_between_our_scan = 90;
 799                 strat_infos.conf.scan_opp_min_time = 90;
 800                 strat_infos.conf.delay_between_opp_scan = 90;
 801         }
 802         else if (!strcmp_P(res->arg1, PSTR("base_check"))) {
 803                 strat_infos.conf.flags = 0;
 804                 strat_infos.conf.scan_our_min_time = 35;
 805                 strat_infos.conf.delay_between_our_scan = 90;
 806                 strat_infos.conf.scan_opp_min_time = 90;
 807                 strat_infos.conf.delay_between_opp_scan = 90;
 808         }
 809         else if (!strcmp_P(res->arg1, PSTR("big3_check"))) {
 810                 strat_infos.conf.flags =
 811                         STRAT_CONF_STORE_STATIC2 |
 812                         STRAT_CONF_BIG_3_TEMPLE;
 813                 strat_infos.conf.scan_our_min_time = 35;
 814                 strat_infos.conf.delay_between_our_scan = 90;
 815                 strat_infos.conf.scan_opp_min_time = 90;
 816                 strat_infos.conf.delay_between_opp_scan = 90;
 817         }
 818         else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) {
 819                 strat_infos.conf.flags =
 820                         STRAT_CONF_TAKE_ONE_LINTEL |
 821                         STRAT_CONF_STORE_STATIC2 |
 822                         STRAT_CONF_EARLY_SCAN |
 823                         STRAT_CONF_PUSH_OPP_COLS;
 824                 strat_infos.conf.scan_our_min_time = 50;
 825                 strat_infos.conf.delay_between_our_scan = 90;
 826                 strat_infos.conf.scan_opp_min_time = 15;
 827                 strat_infos.conf.delay_between_opp_scan = 90;
 828                 strat_infos.conf.wait_opponent = 5;
 829         }
 830         else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) {
 831                 strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL;
 832                 strat_infos.conf.scan_our_min_time = 90;
 833                 strat_infos.conf.delay_between_our_scan = 90;
 834                 strat_infos.conf.scan_opp_min_time = 30;
 835                 strat_infos.conf.delay_between_opp_scan = 90;
 836         }
 837         else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) {
 838                 strat_infos.conf.flags =
 839                         STRAT_CONF_ONLY_ONE_ON_DISC;
 840                 strat_infos.conf.scan_our_min_time = 90;
 841                 strat_infos.conf.delay_between_our_scan = 90;
 842                 strat_infos.conf.scan_opp_min_time = 90;
 843                 strat_infos.conf.delay_between_opp_scan = 90;
 844         }
 845         strat_infos.dump_enabled = 1;
 846         strat_dump_conf();
 847 }
 848
 849 prog_char str_strat_conf_arg0[] = "strat_conf";
 850 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
 851 prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc";
 852 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
 853
 854 prog_char help_strat_conf[] = "configure strat options";
 855 parse_pgm_inst_t cmd_strat_conf = {
 856         .f = cmd_strat_conf_parsed,  /* function to call */
 857         .data = NULL,      /* 2nd arg of func */
 858         .help_str = help_strat_conf,
 859         .tokens = {        /* token list, NULL terminated */
 860                 (prog_void *)&cmd_strat_conf_arg0,
 861                 (prog_void *)&cmd_strat_conf_arg1,
 862                 NULL,
 863         },
 864 };
 865
 866 /**********************************************************/
 867 /* strat configuration */
 868
 869 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
 870 struct cmd_strat_conf2_result {
 871         fixed_string_t arg0;
 872         fixed_string_t arg1;
 873         fixed_string_t arg2;
 874 };
 875
 876 /* function called when cmd_strat_conf2 is parsed successfully */
 877 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
 878 {
 879         struct cmd_strat_conf2_result *res = parsed_result;
 880         uint8_t on, bit = 0;
 881
 882         if (!strcmp_P(res->arg2, PSTR("on")))
 883                 on = 1;
 884         else
 885                 on = 0;
 886
 887         if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
 888                 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
 889         else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
 890                 bit = STRAT_CONF_BYPASS_STATIC2;
 891         else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
 892                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
 893         else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
 894                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
 895         else if (!strcmp_P(res->arg1, PSTR("store_static2")))
 896                 bit = STRAT_CONF_STORE_STATIC2;
 897         else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
 898                 bit = STRAT_CONF_BIG_3_TEMPLE;
 899         else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
 900                 bit = STRAT_CONF_EARLY_SCAN;
 901         else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
 902                 bit = STRAT_CONF_PUSH_OPP_COLS;
 903
 904         if (on)
 905                 strat_infos.conf.flags |= bit;
 906         else
 907                 strat_infos.conf.flags &= (~bit);
 908
 909         strat_infos.dump_enabled = 1;
 910         strat_dump_conf();
 911 }
 912
 913 prog_char str_strat_conf2_arg0[] = "strat_conf";
 914 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
 915 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";
 916 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
 917 prog_char str_strat_conf2_arg2[] = "on#off";
 918 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
 919
 920
 921 prog_char help_strat_conf2[] = "configure strat options";
 922 parse_pgm_inst_t cmd_strat_conf2 = {
 923         .f = cmd_strat_conf2_parsed,  /* function to call */
 924         .data = NULL,      /* 2nd arg of func */
 925         .help_str = help_strat_conf2,
 926         .tokens = {        /* token list, NULL terminated */
 927                 (prog_void *)&cmd_strat_conf2_arg0,
 928                 (prog_void *)&cmd_strat_conf2_arg1,
 929                 (prog_void *)&cmd_strat_conf2_arg2,
 930                 NULL,
 931         },
 932 };
 933
 934 /**********************************************************/
 935 /* strat configuration */
 936
 937 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
 938 struct cmd_strat_conf3_result {
 939         fixed_string_t arg0;
 940         fixed_string_t arg1;
 941         uint8_t arg2;
 942 };
 943
 944 /* function called when cmd_strat_conf3 is parsed successfully */
 945 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
 946 {
 947         struct cmd_strat_conf3_result *res = parsed_result;
 948
 949         if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
 950                 if (res->arg2 > 90)
 951                         res->arg2 = 90;
 952                 strat_infos.conf.scan_opp_min_time = res->arg2;
 953         }
 954         else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
 955                 if (res->arg2 > 90)
 956                         res->arg2 = 90;
 957                 strat_infos.conf.delay_between_opp_scan = res->arg2;
 958         }
 959         else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
 960                 if (res->arg2 > 90)
 961                         res->arg2 = 90;
 962                 strat_infos.conf.scan_our_min_time = res->arg2;
 963         }
 964         else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
 965                 if (res->arg2 > 90)
 966                         res->arg2 = 90;
 967                 strat_infos.conf.delay_between_our_scan = res->arg2;
 968         }
 969         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
 970                 strat_infos.conf.wait_opponent = res->arg2;
 971         }
 972         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
 973                 strat_infos.conf.lintel_min_time = res->arg2;
 974         }
 975         strat_infos.dump_enabled = 1;
 976         strat_dump_conf();
 977 }
 978
 979 prog_char str_strat_conf3_arg0[] = "strat_conf";
 980 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
 981 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";
 982 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
 983 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
 984
 985 prog_char help_strat_conf3[] = "configure strat options";
 986 parse_pgm_inst_t cmd_strat_conf3 = {
 987         .f = cmd_strat_conf3_parsed,  /* function to call */
 988         .data = NULL,      /* 2nd arg of func */
 989         .help_str = help_strat_conf3,
 990         .tokens = {        /* token list, NULL terminated */
 991                 (prog_void *)&cmd_strat_conf3_arg0,
 992                 (prog_void *)&cmd_strat_conf3_arg1,
 993                 (prog_void *)&cmd_strat_conf3_arg2,
 994                 NULL,
 995         },
 996 };
 997
 998 /**********************************************************/
 999 /* strat configuration */
1000
1001 /* this structure is filled when cmd_strat_conf4 is parsed successfully */
1002 struct cmd_strat_conf4_result {
1003         fixed_string_t arg0;
1004         fixed_string_t arg1;
1005         int16_t arg2;
1006 };
1007
1008 /* function called when cmd_strat_conf4 is parsed successfully */
1009 static void cmd_strat_conf4_parsed(void *parsed_result, void *data)
1010 {
1011         struct cmd_strat_conf4_result *res = parsed_result;
1012
1013         if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) {
1014                 strat_infos.conf.scan_opp_angle = res->arg2;
1015         }
1016         strat_infos.dump_enabled = 1;
1017         strat_dump_conf();
1018 }
1019
1020 prog_char str_strat_conf4_arg0[] = "strat_conf";
1021 parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0);
1022 prog_char str_strat_conf4_arg1[] = "scan_opponent_angle";
1023 parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1);
1024 parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16);
1025
1026 prog_char help_strat_conf4[] = "configure strat options";
1027 parse_pgm_inst_t cmd_strat_conf4 = {
1028         .f = cmd_strat_conf4_parsed,  /* function to call */
1029         .data = NULL,      /* 2nd arg of func */
1030         .help_str = help_strat_conf4,
1031         .tokens = {        /* token list, NULL terminated */
1032                 (prog_void *)&cmd_strat_conf4_arg0,
1033                 (prog_void *)&cmd_strat_conf4_arg1,
1034                 (prog_void *)&cmd_strat_conf4_arg2,
1035                 NULL,
1036         },
1037 };
1038
1039
1040 /**********************************************************/
1041 /* Subtraj */
1042
1043 /* this structure is filled when cmd_subtraj is parsed successfully */
1044 struct cmd_subtraj_result {
1045         fixed_string_t arg0;
1046         fixed_string_t arg1;
1047         int32_t arg2;
1048         int32_t arg3;
1049         int32_t arg4;
1050         int32_t arg5;
1051 };
1052
1053 /* function called when cmd_subtraj is parsed successfully */
1054 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1055 {
1056         struct cmd_subtraj_result *res = parsed_result;
1057         uint8_t err = 0;
1058         struct column_dispenser *disp;
1059
1060         if (strcmp_P(res->arg1, PSTR("static")) == 0) {
1061                 err = strat_static_columns(res->arg2);
1062         }
1063         else if (strcmp_P(res->arg1, PSTR("static2")) == 0) {
1064                 strat_infos.s_cols.configuration = res->arg2;
1065                 switch (res->arg2) {
1066                 case 1:
1067                         position_set(&mainboard.pos, 1398,
1068                                      COLOR_Y(1297), COLOR_A(-66));
1069                         break;
1070                 case 2:
1071                         position_set(&mainboard.pos, 1232,
1072                                      COLOR_Y(1051), COLOR_A(4));
1073                         break;
1074                 case 3:
1075                         position_set(&mainboard.pos, 1232,
1076                                      COLOR_Y(1043), COLOR_A(5));
1077                         break;
1078                 case 4:
1079                         position_set(&mainboard.pos, 1346,
1080                                      COLOR_Y(852), COLOR_A(57));
1081                         break;
1082                 default:
1083                         return;
1084                 }
1085                 if (res->arg2 == 1 && res->arg3 == 1) {
1086                         strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE;
1087                 }
1088                 if (res->arg2 == 1 && res->arg3 == 2) {
1089                         strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE;
1090                 }
1091                 err = strat_static_columns_pass2();
1092         }
1093         else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) {
1094                 err = strat_goto_lintel_disp(&strat_infos.l1);
1095         }
1096         else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) {
1097                 err = strat_goto_lintel_disp(&strat_infos.l2);
1098         }
1099         else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) {
1100                 disp = &strat_infos.c1;
1101                 err = strat_goto_col_disp(&disp);
1102         }
1103         else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) {
1104                 disp = &strat_infos.c2;
1105                 err = strat_goto_col_disp(&disp);
1106         }
1107         else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) {
1108                 disp = &strat_infos.c3;
1109                 err = strat_goto_col_disp(&disp);
1110         }
1111         else if (strcmp_P(res->arg1, PSTR("disc")) == 0) {
1112                 if (res->arg2 == 0) {
1113                         printf_P(PSTR("bad level\r\n"));
1114                         return;
1115                 }
1116                 err = strat_goto_disc(res->arg2);
1117         }
1118
1119         printf_P(PSTR("substrat returned %s\r\n"), get_err(err));
1120         trajectory_hardstop(&mainboard.traj);
1121 }
1122
1123 prog_char str_subtraj_arg0[] = "subtraj";
1124 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1125 prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2";
1126 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1127 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1128 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1129 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1130 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1131
1132 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1133 parse_pgm_inst_t cmd_subtraj = {
1134         .f = cmd_subtraj_parsed,  /* function to call */
1135         .data = NULL,      /* 2nd arg of func */
1136         .help_str = help_subtraj,
1137         .tokens = {        /* token list, NULL terminated */
1138                 (prog_void *)&cmd_subtraj_arg0,
1139                 (prog_void *)&cmd_subtraj_arg1,
1140                 (prog_void *)&cmd_subtraj_arg2,
1141                 (prog_void *)&cmd_subtraj_arg3,
1142                 (prog_void *)&cmd_subtraj_arg4,
1143                 (prog_void *)&cmd_subtraj_arg5,
1144                 NULL,
1145         },
1146 };