projects/microb2010/mainboard/commands_traj.c

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