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         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
 770         if (err == END_INTR)
 771                 goto intr;
 772         strat_reset_pos(ROBOT_WIDTH/2 + 100,
 773                         COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
 774                         COLOR_A(-90));
 775         strat_hardstop();
 776
 777         trajectory_d_rel(&mainboard.traj, -180);
 778         err = wait_traj_end(END_INTR|END_TRAJ);
 779         if (err == END_INTR)
 780                 goto intr;
 781
 782         trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
 783         err = wait_traj_end(END_INTR|END_TRAJ);
 784         if (err == END_INTR)
 785                 goto intr;
 786
 787         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
 788         if (err == END_INTR)
 789                 goto intr;
 790         strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
 791                         DO_NOT_SET_POS,
 792                         180);
 793         strat_hardstop();
 794
 795         trajectory_d_rel(&mainboard.traj, -170);
 796         err = wait_traj_end(END_INTR|END_TRAJ);
 797         if (err == END_INTR)
 798                 goto intr;
 799         wait_ms(100);
 800
 801         trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
 802         err = wait_traj_end(END_INTR|END_TRAJ);
 803         if (err == END_INTR)
 804                 goto intr;
 805         wait_ms(100);
 806
 807         strat_set_speed(old_spdd, old_spda);
 808         return;
 809
 810 intr:
 811         strat_hardstop();
 812         strat_set_speed(old_spdd, old_spda);
 813 }
 814
 815 /* function called when cmd_position is parsed successfully */
 816 static void cmd_position_parsed(void * parsed_result, void * data)
 817 {
 818         struct cmd_position_result * res = parsed_result;
 819
 820         /* display raw position values */
 821         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 822                 position_set(&mainboard.pos, 0, 0, 0);
 823         }
 824         else if (!strcmp_P(res->arg1, PSTR("set"))) {
 825                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
 826         }
 827         else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
 828                 mainboard.our_color = I2C_COLOR_BLUE;
 829 #ifndef HOST_VERSION
 830                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
 831                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
 832 #endif
 833                 auto_position();
 834         }
 835         else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
 836                 mainboard.our_color = I2C_COLOR_YELLOW;
 837 #ifndef HOST_VERSION
 838                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
 839                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
 840 #endif
 841                 auto_position();
 842         }
 843
 844         /* else it's just a "show" */
 845         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
 846                  position_get_x_double(&mainboard.pos),
 847                  position_get_y_double(&mainboard.pos),
 848                  DEG(position_get_a_rad_double(&mainboard.pos)));
 849 }
 850
 851 prog_char str_position_arg0[] = "position";
 852 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
 853 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
 854 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
 855
 856 prog_char help_position[] = "Show/reset (x,y,a) position";
 857 parse_pgm_inst_t cmd_position = {
 858         .f = cmd_position_parsed,  /* function to call */
 859         .data = NULL,      /* 2nd arg of func */
 860         .help_str = help_position,
 861         .tokens = {        /* token list, NULL terminated */
 862                 (prog_void *)&cmd_position_arg0,
 863                 (prog_void *)&cmd_position_arg1,
 864                 NULL,
 865         },
 866 };
 867
 868
 869 prog_char str_position_arg1_set[] = "set";
 870 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
 871 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
 872 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
 873 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
 874
 875 prog_char help_position_set[] = "Set (x,y,a) position";
 876 parse_pgm_inst_t cmd_position_set = {
 877         .f = cmd_position_parsed,  /* function to call */
 878         .data = NULL,      /* 2nd arg of func */
 879         .help_str = help_position_set,
 880         .tokens = {        /* token list, NULL terminated */
 881                 (prog_void *)&cmd_position_arg0,
 882                 (prog_void *)&cmd_position_arg1_set,
 883                 (prog_void *)&cmd_position_arg2,
 884                 (prog_void *)&cmd_position_arg3,
 885                 (prog_void *)&cmd_position_arg4,
 886                 NULL,
 887         },
 888 };
 889
 890
 891 /**********************************************************/
 892 /* strat configuration */
 893
 894 /* this structure is filled when cmd_strat_infos is parsed successfully */
 895 struct cmd_strat_infos_result {
 896         fixed_string_t arg0;
 897         fixed_string_t arg1;
 898 };
 899
 900 /* function called when cmd_strat_infos is parsed successfully */
 901 static void cmd_strat_infos_parsed(void *parsed_result, void *data)
 902 {
 903         struct cmd_strat_infos_result *res = parsed_result;
 904
 905         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 906                 strat_reset_infos();
 907         }
 908         strat_infos.dump_enabled = 1;
 909         strat_dump_infos(__FUNCTION__);
 910 }
 911
 912 prog_char str_strat_infos_arg0[] = "strat_infos";
 913 parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
 914 prog_char str_strat_infos_arg1[] = "show#reset";
 915 parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
 916
 917 prog_char help_strat_infos[] = "reset/show strat_infos";
 918 parse_pgm_inst_t cmd_strat_infos = {
 919         .f = cmd_strat_infos_parsed,  /* function to call */
 920         .data = NULL,      /* 2nd arg of func */
 921         .help_str = help_strat_infos,
 922         .tokens = {        /* token list, NULL terminated */
 923                 (prog_void *)&cmd_strat_infos_arg0,
 924                 (prog_void *)&cmd_strat_infos_arg1,
 925                 NULL,
 926         },
 927 };
 928
 929 /**********************************************************/
 930 /* strat configuration */
 931
 932 /* this structure is filled when cmd_strat_conf is parsed successfully */
 933 struct cmd_strat_conf_result {
 934         fixed_string_t arg0;
 935         fixed_string_t arg1;
 936 };
 937
 938 /* function called when cmd_strat_conf is parsed successfully */
 939 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
 940 {
 941         //      struct cmd_strat_conf_result *res = parsed_result;
 942
 943         strat_infos.dump_enabled = 1;
 944         strat_dump_conf();
 945 }
 946
 947 prog_char str_strat_conf_arg0[] = "strat_conf";
 948 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
 949 prog_char str_strat_conf_arg1[] = "show#base";
 950 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
 951
 952 prog_char help_strat_conf[] = "configure strat options";
 953 parse_pgm_inst_t cmd_strat_conf = {
 954         .f = cmd_strat_conf_parsed,  /* function to call */
 955         .data = NULL,      /* 2nd arg of func */
 956         .help_str = help_strat_conf,
 957         .tokens = {        /* token list, NULL terminated */
 958                 (prog_void *)&cmd_strat_conf_arg0,
 959                 (prog_void *)&cmd_strat_conf_arg1,
 960                 NULL,
 961         },
 962 };
 963
 964 /**********************************************************/
 965 /* strat configuration */
 966
 967 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
 968 struct cmd_strat_conf2_result {
 969         fixed_string_t arg0;
 970         fixed_string_t arg1;
 971         fixed_string_t arg2;
 972 };
 973
 974 /* function called when cmd_strat_conf2 is parsed successfully */
 975 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
 976 {
 977         struct cmd_strat_conf2_result *res = parsed_result;
 978         uint8_t on, bit = 0;
 979
 980         if (!strcmp_P(res->arg2, PSTR("on")))
 981                 on = 1;
 982         else
 983                 on = 0;
 984
 985 #if 0
 986         if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
 987                 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
 988         else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
 989                 bit = STRAT_CONF_BYPASS_STATIC2;
 990         else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
 991                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
 992         else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
 993                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
 994         else if (!strcmp_P(res->arg1, PSTR("store_static2")))
 995                 bit = STRAT_CONF_STORE_STATIC2;
 996         else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
 997                 bit = STRAT_CONF_BIG_3_TEMPLE;
 998         else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
 999                 bit = STRAT_CONF_EARLY_SCAN;
1000         else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
1001                 bit = STRAT_CONF_PUSH_OPP_COLS;
1002 #endif
1003
1004         if (on)
1005                 strat_infos.conf.flags |= bit;
1006         else
1007                 strat_infos.conf.flags &= (~bit);
1008
1009         strat_infos.dump_enabled = 1;
1010         strat_dump_conf();
1011 }
1012
1013 prog_char str_strat_conf2_arg0[] = "strat_conf";
1014 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1015 prog_char str_strat_conf2_arg1[] = "faux";
1016 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1017 prog_char str_strat_conf2_arg2[] = "on#off";
1018 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1019
1020
1021 prog_char help_strat_conf2[] = "configure strat options";
1022 parse_pgm_inst_t cmd_strat_conf2 = {
1023         .f = cmd_strat_conf2_parsed,  /* function to call */
1024         .data = NULL,      /* 2nd arg of func */
1025         .help_str = help_strat_conf2,
1026         .tokens = {        /* token list, NULL terminated */
1027                 (prog_void *)&cmd_strat_conf2_arg0,
1028                 (prog_void *)&cmd_strat_conf2_arg1,
1029                 (prog_void *)&cmd_strat_conf2_arg2,
1030                 NULL,
1031         },
1032 };
1033
1034 /**********************************************************/
1035 /* strat configuration */
1036
1037 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1038 struct cmd_strat_conf3_result {
1039         fixed_string_t arg0;
1040         fixed_string_t arg1;
1041         uint8_t arg2;
1042 };
1043
1044 /* function called when cmd_strat_conf3 is parsed successfully */
1045 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1046 {
1047 #if 0
1048         struct cmd_strat_conf3_result *res = parsed_result;
1049
1050         if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1051                 if (res->arg2 > 90)
1052                         res->arg2 = 90;
1053                 strat_infos.conf.scan_opp_min_time = res->arg2;
1054         }
1055         else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1056                 if (res->arg2 > 90)
1057                         res->arg2 = 90;
1058                 strat_infos.conf.delay_between_opp_scan = res->arg2;
1059         }
1060         else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1061                 if (res->arg2 > 90)
1062                         res->arg2 = 90;
1063                 strat_infos.conf.scan_our_min_time = res->arg2;
1064         }
1065         else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1066                 if (res->arg2 > 90)
1067                         res->arg2 = 90;
1068                 strat_infos.conf.delay_between_our_scan = res->arg2;
1069         }
1070         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1071                 strat_infos.conf.wait_opponent = res->arg2;
1072         }
1073         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1074                 strat_infos.conf.lintel_min_time = res->arg2;
1075         }
1076 #endif
1077         strat_infos.dump_enabled = 1;
1078         strat_dump_conf();
1079 }
1080
1081 prog_char str_strat_conf3_arg0[] = "strat_conf";
1082 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1083 prog_char str_strat_conf3_arg1[] = "faux2";
1084 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1085 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1086
1087 prog_char help_strat_conf3[] = "configure strat options";
1088 parse_pgm_inst_t cmd_strat_conf3 = {
1089         .f = cmd_strat_conf3_parsed,  /* function to call */
1090         .data = NULL,      /* 2nd arg of func */
1091         .help_str = help_strat_conf3,
1092         .tokens = {        /* token list, NULL terminated */
1093                 (prog_void *)&cmd_strat_conf3_arg0,
1094                 (prog_void *)&cmd_strat_conf3_arg1,
1095                 (prog_void *)&cmd_strat_conf3_arg2,
1096                 NULL,
1097         },
1098 };
1099
1100
1101 /**********************************************************/
1102 /* Subtraj */
1103
1104 //////////////////////
1105
1106 // 500 -- 5
1107 // 400 -- 3
1108 #define TEST_SPEED 400
1109 #define TEST_ACC 3
1110
1111 static void line2line(double line1x1, double line1y1,
1112                       double line1x2, double line1y2,
1113                       double line2x1, double line2y1,
1114                       double line2x2, double line2y2,
1115                       double radius, double dist)
1116 {
1117         uint8_t err;
1118         double speed_d, speed_a;
1119         double distance, angle;
1120         double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
1121         double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
1122
1123         printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1124
1125         strat_set_speed(TEST_SPEED, TEST_SPEED);
1126         quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1127
1128         circle_get_da_speed_from_radius(&mainboard.traj, radius,
1129                                         &speed_d, &speed_a);
1130         trajectory_line_abs(&mainboard.traj,
1131                             line1x1, line1y1,
1132                             line1x2, line1y2, 150.);
1133         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1134                                     dist, TRAJ_FLAGS_NO_NEAR);
1135         /* circle */
1136         strat_set_speed(speed_d, speed_a);
1137         angle = line2_angle - line1_angle;
1138         distance = angle * radius;
1139         if (distance < 0)
1140                 distance = -distance;
1141         angle = simple_modulo_2pi(angle);
1142         angle = DEG(angle);
1143         printf_P(PSTR("(%d,%d,%d) "),
1144                  position_get_x_s16(&mainboard.pos),
1145                  position_get_y_s16(&mainboard.pos),
1146                  position_get_a_deg_s16(&mainboard.pos));
1147         printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
1148                  distance, angle);
1149
1150         /* take some margin on dist to avoid deceleration */
1151         trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
1152
1153         /* circle exit condition */
1154         err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1155                                     TRAJ_FLAGS_NO_NEAR);
1156
1157         strat_set_speed(500, 500);
1158         printf_P(PSTR("(%d,%d,%d) "),
1159                  position_get_x_s16(&mainboard.pos),
1160                  position_get_y_s16(&mainboard.pos),
1161                  position_get_a_deg_s16(&mainboard.pos));
1162         printf_P(PSTR("line\r\n"));
1163         trajectory_line_abs(&mainboard.traj,
1164                             line2x1, line2y1,
1165                             line2x2, line2y2, 150.);
1166 }
1167
1168 static void halfturn(double line1x1, double line1y1,
1169                      double line1x2, double line1y2,
1170                      double line2x1, double line2y1,
1171                      double line2x2, double line2y2,
1172                      double radius, double dist, double dir)
1173 {
1174         uint8_t err;
1175         double speed_d, speed_a;
1176         double distance, angle;
1177
1178         printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1179
1180         strat_set_speed(TEST_SPEED, TEST_SPEED);
1181         quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1182
1183         circle_get_da_speed_from_radius(&mainboard.traj, radius,
1184                                         &speed_d, &speed_a);
1185         trajectory_line_abs(&mainboard.traj,
1186                             line1x1, line1y1,
1187                             line1x2, line1y2, 150.);
1188         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1189                                     dist, TRAJ_FLAGS_NO_NEAR);
1190         /* circle */
1191         strat_set_speed(speed_d, speed_a);
1192         angle = dir * M_PI/2.;
1193         distance = angle * radius;
1194         if (distance < 0)
1195                 distance = -distance;
1196         angle = simple_modulo_2pi(angle);
1197         angle = DEG(angle);
1198
1199         /* take some margin on dist to avoid deceleration */
1200         DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
1201               distance, angle);
1202         trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1203
1204         /* circle exit condition */
1205         err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1206                                     TRAJ_FLAGS_NO_NEAR);
1207
1208         DEBUG(E_USER_STRAT, "miniline");
1209         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
1210                                     dist, TRAJ_FLAGS_NO_NEAR);
1211         DEBUG(E_USER_STRAT, "circle2");
1212         /* take some margin on dist to avoid deceleration */
1213         trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1214
1215         err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1216                                     TRAJ_FLAGS_NO_NEAR);
1217
1218         strat_set_speed(500, 500);
1219         DEBUG(E_USER_STRAT, "line");
1220         trajectory_line_abs(&mainboard.traj,
1221                             line2x1, line2y1,
1222                             line2x2, line2y2, 150.);
1223 }
1224
1225
1226 /* function called when cmd_test is parsed successfully */
1227 static void subtraj_test(void)
1228 {
1229 #ifdef HOST_VERSION
1230         strat_reset_pos(400, 400, 90);
1231         mainboard.angle.on = 1;
1232         mainboard.distance.on = 1;
1233 #endif
1234         printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1235         while (!cmdline_keypressed()) {
1236                 /****** PASS1 */
1237
1238 #define DIST_HARD_TURN   260
1239 #define RADIUS_HARD_TURN 100
1240 #define DIST_EASY_TURN   190
1241 #define RADIUS_EASY_TURN 190
1242 #define DIST_HALF_TURN   225
1243 #define RADIUS_HALF_TURN 130
1244
1245                 /* hard turn */
1246                 line2line(375, 597, 375, 1847,
1247                           375, 1847, 1050, 1472,
1248                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1249
1250                 /* easy left and easy right !*/
1251                 line2line(825, 1596, 1050, 1472,
1252                           1050, 1472, 1500, 1722,
1253                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1254                 line2line(1050, 1472, 1500, 1722,
1255                           1500, 1722, 2175, 1347,
1256                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1257                 line2line(1500, 1722, 2175, 1347,
1258                           2175, 1347, 2175, 847,
1259                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1260
1261                 /* half turns */
1262                 halfturn(2175, 1347, 2175, 722,
1263                          2625, 722, 2625, 1597,
1264                          RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1265                 halfturn(2625, 847, 2625, 1722,
1266                           2175, 1722, 2175, 1097,
1267                           RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1268
1269                 /* easy turns */
1270                 line2line(2175, 1597, 2175, 1097,
1271                           2175, 1097, 1500, 722,
1272                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1273                 line2line(2175, 1097, 1500, 722,
1274                           1500, 722, 1050, 972,
1275                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1276                 line2line(1500, 722, 1050, 972,
1277                           1050, 972, 375, 597,
1278                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1279
1280                 /* hard turn */
1281                 line2line(1050, 972, 375, 597,
1282                           375, 597, 375, 1097,
1283                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1284
1285                 /****** PASS2 */
1286
1287                 /* easy turn */
1288                 line2line(375, 597, 375, 1097,
1289                           375, 1097, 1050, 1472,
1290                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1291
1292                 /* hard turn */
1293                 line2line(375, 1097, 1050, 1472,
1294                           1050, 1472, 375, 1847,
1295                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1296
1297                 /* hard turn */
1298                 line2line(1050, 1472, 375, 1847,
1299                           375, 1847, 375, 1347,
1300                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1301
1302                 /* easy turn */
1303                 line2line(375, 1847, 375, 1347,
1304                           375, 1347, 1050, 972,
1305                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1306
1307                 /* hard turn */
1308                 line2line(375, 1347, 1050, 972,
1309                           1050, 972, 375, 597,
1310                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1311
1312                 /* hard turn */
1313                 line2line(1050, 972, 375, 597,
1314                           375, 597, 375, 1847,
1315                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1316
1317         }
1318         trajectory_hardstop(&mainboard.traj);
1319 }
1320
1321
1322 /* this structure is filled when cmd_subtraj is parsed successfully */
1323 struct cmd_subtraj_result {
1324         fixed_string_t arg0;
1325         fixed_string_t arg1;
1326         int32_t arg2;
1327         int32_t arg3;
1328         int32_t arg4;
1329         int32_t arg5;
1330 };
1331
1332 /* function called when cmd_subtraj is parsed successfully */
1333 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1334 {
1335         struct cmd_subtraj_result *res = parsed_result;
1336
1337         if (!strcmp_P(res->arg1, PSTR("test")))
1338                 subtraj_test();
1339
1340         trajectory_hardstop(&mainboard.traj);
1341 }
1342
1343 prog_char str_subtraj_arg0[] = "subtraj";
1344 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1345 prog_char str_subtraj_arg1[] = "faux";
1346 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1347 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1348 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1349 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1350 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1351
1352 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1353 parse_pgm_inst_t cmd_subtraj = {
1354         .f = cmd_subtraj_parsed,  /* function to call */
1355         .data = NULL,      /* 2nd arg of func */
1356         .help_str = help_subtraj,
1357         .tokens = {        /* token list, NULL terminated */
1358                 (prog_void *)&cmd_subtraj_arg0,
1359                 (prog_void *)&cmd_subtraj_arg1,
1360                 (prog_void *)&cmd_subtraj_arg2,
1361                 (prog_void *)&cmd_subtraj_arg3,
1362                 (prog_void *)&cmd_subtraj_arg4,
1363                 (prog_void *)&cmd_subtraj_arg5,
1364                 NULL,
1365         },
1366 };
1367