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