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 "strat_db.h"
  63 #include "../common/i2c_commands.h"
  64 #include "i2c_protocol.h"
  65
  66 /**********************************************************/
  67 /* Traj_Speeds for trajectory_manager */
  68
  69 /* this structure is filled when cmd_traj_speed is parsed successfully */
  70 struct cmd_traj_speed_result {
  71         fixed_string_t arg0;
  72         fixed_string_t arg1;
  73         float s;
  74 };
  75
  76 /* function called when cmd_traj_speed is parsed successfully */
  77 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
  78 {
  79         struct cmd_traj_speed_result * res = parsed_result;
  80
  81         if (!strcmp_P(res->arg1, PSTR("angle"))) {
  82                 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
  83         }
  84         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
  85                 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
  86         }
  87         /* else it is a "show" */
  88
  89         printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
  90                  mainboard.traj.a_speed,
  91                  mainboard.traj.d_speed);
  92 }
  93
  94 prog_char str_traj_speed_arg0[] = "traj_speed";
  95 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
  96 prog_char str_traj_speed_arg1[] = "angle#distance";
  97 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
  98 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT);
  99
 100 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
 101 parse_pgm_inst_t cmd_traj_speed = {
 102         .f = cmd_traj_speed_parsed,  /* function to call */
 103         .data = NULL,      /* 2nd arg of func */
 104         .help_str = help_traj_speed,
 105         .tokens = {        /* token list, NULL terminated */
 106                 (prog_void *)&cmd_traj_speed_arg0,
 107                 (prog_void *)&cmd_traj_speed_arg1,
 108                 (prog_void *)&cmd_traj_speed_s,
 109                 NULL,
 110         },
 111 };
 112
 113 /* show */
 114
 115 prog_char str_traj_speed_show_arg[] = "show";
 116 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
 117
 118 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
 119 parse_pgm_inst_t cmd_traj_speed_show = {
 120         .f = cmd_traj_speed_parsed,  /* function to call */
 121         .data = NULL,      /* 2nd arg of func */
 122         .help_str = help_traj_speed_show,
 123         .tokens = {        /* token list, NULL terminated */
 124                 (prog_void *)&cmd_traj_speed_arg0,
 125                 (prog_void *)&cmd_traj_speed_show_arg,
 126                 NULL,
 127         },
 128 };
 129
 130 /**********************************************************/
 131 /* Traj_Accs for trajectory_manager */
 132
 133 /* this structure is filled when cmd_traj_acc is parsed successfully */
 134 struct cmd_traj_acc_result {
 135         fixed_string_t arg0;
 136         fixed_string_t arg1;
 137         float s;
 138 };
 139
 140 /* function called when cmd_traj_acc is parsed successfully */
 141 static void cmd_traj_acc_parsed(void *parsed_result, void *data)
 142 {
 143         struct cmd_traj_acc_result * res = parsed_result;
 144
 145         if (!strcmp_P(res->arg1, PSTR("angle"))) {
 146                 trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s);
 147         }
 148         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
 149                 trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc);
 150         }
 151         /* else it is a "show" */
 152
 153         printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
 154                  mainboard.traj.a_acc,
 155                  mainboard.traj.d_acc);
 156 }
 157
 158 prog_char str_traj_acc_arg0[] = "traj_acc";
 159 parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0);
 160 prog_char str_traj_acc_arg1[] = "angle#distance";
 161 parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1);
 162 parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT);
 163
 164 prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager";
 165 parse_pgm_inst_t cmd_traj_acc = {
 166         .f = cmd_traj_acc_parsed,  /* function to call */
 167         .data = NULL,      /* 2nd arg of func */
 168         .help_str = help_traj_acc,
 169         .tokens = {        /* token list, NULL terminated */
 170                 (prog_void *)&cmd_traj_acc_arg0,
 171                 (prog_void *)&cmd_traj_acc_arg1,
 172                 (prog_void *)&cmd_traj_acc_s,
 173                 NULL,
 174         },
 175 };
 176
 177 /* show */
 178
 179 prog_char str_traj_acc_show_arg[] = "show";
 180 parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg);
 181
 182 prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager";
 183 parse_pgm_inst_t cmd_traj_acc_show = {
 184         .f = cmd_traj_acc_parsed,  /* function to call */
 185         .data = NULL,      /* 2nd arg of func */
 186         .help_str = help_traj_acc_show,
 187         .tokens = {        /* token list, NULL terminated */
 188                 (prog_void *)&cmd_traj_acc_arg0,
 189                 (prog_void *)&cmd_traj_acc_show_arg,
 190                 NULL,
 191         },
 192 };
 193
 194 /**********************************************************/
 195 /* circle coef configuration */
 196
 197 /* this structure is filled when cmd_circle_coef is parsed successfully */
 198 struct cmd_circle_coef_result {
 199         fixed_string_t arg0;
 200         fixed_string_t arg1;
 201         float circle_coef;
 202 };
 203
 204
 205 /* function called when cmd_circle_coef is parsed successfully */
 206 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
 207 {
 208         struct cmd_circle_coef_result *res = parsed_result;
 209
 210         if (!strcmp_P(res->arg1, PSTR("set"))) {
 211                 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
 212         }
 213
 214         printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
 215 }
 216
 217 prog_char str_circle_coef_arg0[] = "circle_coef";
 218 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
 219 prog_char str_circle_coef_arg1[] = "set";
 220 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
 221 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
 222
 223 prog_char help_circle_coef[] = "Set circle coef";
 224 parse_pgm_inst_t cmd_circle_coef = {
 225         .f = cmd_circle_coef_parsed,  /* function to call */
 226         .data = NULL,      /* 2nd arg of func */
 227         .help_str = help_circle_coef,
 228         .tokens = {        /* token list, NULL terminated */
 229                 (prog_void *)&cmd_circle_coef_arg0,
 230                 (prog_void *)&cmd_circle_coef_arg1,
 231                 (prog_void *)&cmd_circle_coef_val,
 232                 NULL,
 233         },
 234 };
 235
 236 /* show */
 237
 238 prog_char str_circle_coef_show_arg[] = "show";
 239 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
 240
 241 prog_char help_circle_coef_show[] = "Show circle coef";
 242 parse_pgm_inst_t cmd_circle_coef_show = {
 243         .f = cmd_circle_coef_parsed,  /* function to call */
 244         .data = NULL,      /* 2nd arg of func */
 245         .help_str = help_circle_coef_show,
 246         .tokens = {        /* token list, NULL terminated */
 247                 (prog_void *)&cmd_circle_coef_arg0,
 248                 (prog_void *)&cmd_circle_coef_show_arg,
 249                 NULL,
 250         },
 251 };
 252
 253 /**********************************************************/
 254 /* trajectory window configuration */
 255
 256 /* this structure is filled when cmd_trajectory is parsed successfully */
 257 struct cmd_trajectory_result {
 258         fixed_string_t arg0;
 259         fixed_string_t arg1;
 260         float d_win;
 261         float a_win;
 262         float a_start;
 263 };
 264
 265
 266 /* function called when cmd_trajectory is parsed successfully */
 267 static void cmd_trajectory_parsed(void * parsed_result, void * data)
 268 {
 269         struct cmd_trajectory_result * res = parsed_result;
 270
 271         if (!strcmp_P(res->arg1, PSTR("set"))) {
 272                 trajectory_set_windows(&mainboard.traj, res->d_win,
 273                                        res->a_win, res->a_start);
 274         }
 275
 276         printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
 277                  DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
 278 }
 279
 280 prog_char str_trajectory_arg0[] = "trajectory";
 281 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
 282 prog_char str_trajectory_arg1[] = "set";
 283 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
 284 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
 285 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
 286 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
 287
 288 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
 289 parse_pgm_inst_t cmd_trajectory = {
 290         .f = cmd_trajectory_parsed,  /* function to call */
 291         .data = NULL,      /* 2nd arg of func */
 292         .help_str = help_trajectory,
 293         .tokens = {        /* token list, NULL terminated */
 294                 (prog_void *)&cmd_trajectory_arg0,
 295                 (prog_void *)&cmd_trajectory_arg1,
 296                 (prog_void *)&cmd_trajectory_d,
 297                 (prog_void *)&cmd_trajectory_a,
 298                 (prog_void *)&cmd_trajectory_as,
 299                 NULL,
 300         },
 301 };
 302
 303 /* show */
 304
 305 prog_char str_trajectory_show_arg[] = "show";
 306 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
 307
 308 prog_char help_trajectory_show[] = "Show trajectory window configuration";
 309 parse_pgm_inst_t cmd_trajectory_show = {
 310         .f = cmd_trajectory_parsed,  /* function to call */
 311         .data = NULL,      /* 2nd arg of func */
 312         .help_str = help_trajectory_show,
 313         .tokens = {        /* token list, NULL terminated */
 314                 (prog_void *)&cmd_trajectory_arg0,
 315                 (prog_void *)&cmd_trajectory_show_arg,
 316                 NULL,
 317         },
 318 };
 319
 320 /**********************************************************/
 321 /* rs_gains configuration */
 322
 323 /* this structure is filled when cmd_rs_gains is parsed successfully */
 324 struct cmd_rs_gains_result {
 325         fixed_string_t arg0;
 326         fixed_string_t arg1;
 327         float left;
 328         float right;
 329 };
 330
 331 /* function called when cmd_rs_gains is parsed successfully */
 332 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
 333 {
 334 #ifdef HOST_VERSION
 335         printf("not implemented\n");
 336 #else
 337         struct cmd_rs_gains_result * res = parsed_result;
 338
 339         if (!strcmp_P(res->arg1, PSTR("set"))) {
 340                 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
 341                                         LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
 342                 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
 343                                          RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
 344         }
 345         printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
 346                  mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
 347 #endif
 348 }
 349
 350 prog_char str_rs_gains_arg0[] = "rs_gains";
 351 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
 352 prog_char str_rs_gains_arg1[] = "set";
 353 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
 354 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
 355 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
 356
 357 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
 358 parse_pgm_inst_t cmd_rs_gains = {
 359         .f = cmd_rs_gains_parsed,  /* function to call */
 360         .data = NULL,      /* 2nd arg of func */
 361         .help_str = help_rs_gains,
 362         .tokens = {        /* token list, NULL terminated */
 363                 (prog_void *)&cmd_rs_gains_arg0,
 364                 (prog_void *)&cmd_rs_gains_arg1,
 365                 (prog_void *)&cmd_rs_gains_l,
 366                 (prog_void *)&cmd_rs_gains_r,
 367                 NULL,
 368         },
 369 };
 370
 371 /* show */
 372
 373 prog_char str_rs_gains_show_arg[] = "show";
 374 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
 375
 376 prog_char help_rs_gains_show[] = "Show rs_gains";
 377 parse_pgm_inst_t cmd_rs_gains_show = {
 378         .f = cmd_rs_gains_parsed,  /* function to call */
 379         .data = NULL,      /* 2nd arg of func */
 380         .help_str = help_rs_gains_show,
 381         .tokens = {        /* token list, NULL terminated */
 382                 (prog_void *)&cmd_rs_gains_arg0,
 383                 (prog_void *)&cmd_rs_gains_show_arg,
 384                 NULL,
 385         },
 386 };
 387
 388 /**********************************************************/
 389 /* track configuration */
 390
 391 /* this structure is filled when cmd_track is parsed successfully */
 392 struct cmd_track_result {
 393         fixed_string_t arg0;
 394         fixed_string_t arg1;
 395         float val;
 396 };
 397
 398 /* function called when cmd_track is parsed successfully */
 399 static void cmd_track_parsed(void * parsed_result, void * data)
 400 {
 401         struct cmd_track_result * res = parsed_result;
 402
 403         if (!strcmp_P(res->arg1, PSTR("set"))) {
 404                 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
 405         }
 406         printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
 407 }
 408
 409 prog_char str_track_arg0[] = "track";
 410 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
 411 prog_char str_track_arg1[] = "set";
 412 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
 413 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
 414
 415 prog_char help_track[] = "Set track in mm";
 416 parse_pgm_inst_t cmd_track = {
 417         .f = cmd_track_parsed,  /* function to call */
 418         .data = NULL,      /* 2nd arg of func */
 419         .help_str = help_track,
 420         .tokens = {        /* token list, NULL terminated */
 421                 (prog_void *)&cmd_track_arg0,
 422                 (prog_void *)&cmd_track_arg1,
 423                 (prog_void *)&cmd_track_val,
 424                 NULL,
 425         },
 426 };
 427
 428 /* show */
 429
 430 prog_char str_track_show_arg[] = "show";
 431 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
 432
 433 prog_char help_track_show[] = "Show track";
 434 parse_pgm_inst_t cmd_track_show = {
 435         .f = cmd_track_parsed,  /* function to call */
 436         .data = NULL,      /* 2nd arg of func */
 437         .help_str = help_track_show,
 438         .tokens = {        /* token list, NULL terminated */
 439                 (prog_void *)&cmd_track_arg0,
 440                 (prog_void *)&cmd_track_show_arg,
 441                 NULL,
 442         },
 443 };
 444
 445
 446
 447 /**********************************************************/
 448 /* Pt_Lists for testing traj */
 449
 450 #define PT_LIST_SIZE 10
 451 static struct xy_point pt_list[PT_LIST_SIZE];
 452 static uint16_t pt_list_len = 0;
 453
 454 /* this structure is filled when cmd_pt_list is parsed successfully */
 455 struct cmd_pt_list_result {
 456         fixed_string_t arg0;
 457         fixed_string_t arg1;
 458         uint16_t arg2;
 459         int16_t arg3;
 460         int16_t arg4;
 461 };
 462
 463 /* function called when cmd_pt_list is parsed successfully */
 464 static void cmd_pt_list_parsed(void * parsed_result, void * data)
 465 {
 466         struct cmd_pt_list_result * res = parsed_result;
 467         uint8_t i, why=0;
 468
 469         if (!strcmp_P(res->arg1, PSTR("append"))) {
 470                 res->arg2 = pt_list_len;
 471         }
 472         if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
 473                 printf_P(PSTR("removed\r\n"));
 474                 return;
 475         }
 476
 477         if (!strcmp_P(res->arg1, PSTR("insert")) ||
 478             !strcmp_P(res->arg1, PSTR("append"))) {
 479                 if (res->arg2 > pt_list_len) {
 480                         printf_P(PSTR("Index too large\r\n"));
 481                         return;
 482                 }
 483                 if (pt_list_len >= PT_LIST_SIZE) {
 484                         printf_P(PSTR("List is too large\r\n"));
 485                         return;
 486                 }
 487                 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
 488                        PT_LIST_SIZE-1-res->arg2);
 489                 pt_list[res->arg2].x = res->arg3;
 490                 pt_list[res->arg2].y = res->arg4;
 491                 pt_list_len++;
 492         }
 493         else if (!strcmp_P(res->arg1, PSTR("del"))) {
 494                 if (pt_list_len <= 0) {
 495                         printf_P(PSTR("Error: list empty\r\n"));
 496                         return;
 497                 }
 498                 if (res->arg2 > pt_list_len) {
 499                         printf_P(PSTR("Index too large\r\n"));
 500                         return;
 501                 }
 502                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
 503                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
 504                 pt_list_len--;
 505         }
 506         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
 507                 pt_list_len = 0;
 508         }
 509
 510         /* else it is a "show" or a "start" */
 511         if (pt_list_len == 0) {
 512                 printf_P(PSTR("List empty\r\n"));
 513                 return;
 514         }
 515  restart:
 516         for (i=0 ; i<pt_list_len ; i++) {
 517                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
 518                 if (!strcmp_P(res->arg1, PSTR("start"))) {
 519                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
 520                         why = wait_traj_end(0xFF); /* all */
 521                 }
 522                 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
 523                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
 524                         why = wait_traj_end(0xFF); /* all */
 525                 }
 526 #if 0
 527                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
 528                         while (1) {
 529                                 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
 530                                 printf("next point\r\n");
 531                                 if (why != END_OBSTACLE)
 532                                         break;
 533                         }
 534                 }
 535 #endif
 536                 if (why & (~(END_TRAJ | END_NEAR)))
 537                         trajectory_stop(&mainboard.traj);
 538                 if (why & END_INTR)
 539                         break;
 540         }
 541         if (why & END_INTR)
 542                 return;
 543         if (!strcmp_P(res->arg1, PSTR("loop_start")))
 544                 goto restart;
 545 }
 546
 547 prog_char str_pt_list_arg0[] = "pt_list";
 548 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
 549 prog_char str_pt_list_arg1[] = "insert";
 550 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
 551 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
 552 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
 553 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
 554
 555 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
 556 parse_pgm_inst_t cmd_pt_list = {
 557         .f = cmd_pt_list_parsed,  /* function to call */
 558         .data = NULL,      /* 2nd arg of func */
 559         .help_str = help_pt_list,
 560         .tokens = {        /* token list, NULL terminated */
 561                 (prog_void *)&cmd_pt_list_arg0,
 562                 (prog_void *)&cmd_pt_list_arg1,
 563                 (prog_void *)&cmd_pt_list_arg2,
 564                 (prog_void *)&cmd_pt_list_arg3,
 565                 (prog_void *)&cmd_pt_list_arg4,
 566                 NULL,
 567         },
 568 };
 569
 570 /* append */
 571
 572 prog_char str_pt_list_arg1_append[] = "append";
 573 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
 574
 575 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
 576 parse_pgm_inst_t cmd_pt_list_append = {
 577         .f = cmd_pt_list_parsed,  /* function to call */
 578         .data = NULL,      /* 2nd arg of func */
 579         .help_str = help_pt_list_append,
 580         .tokens = {        /* token list, NULL terminated */
 581                 (prog_void *)&cmd_pt_list_arg0,
 582                 (prog_void *)&cmd_pt_list_arg1_append,
 583                 (prog_void *)&cmd_pt_list_arg3,
 584                 (prog_void *)&cmd_pt_list_arg4,
 585                 NULL,
 586         },
 587 };
 588
 589 /* del */
 590
 591 prog_char str_pt_list_del_arg[] = "del";
 592 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
 593
 594 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
 595 parse_pgm_inst_t cmd_pt_list_del = {
 596         .f = cmd_pt_list_parsed,  /* function to call */
 597         .data = NULL,      /* 2nd arg of func */
 598         .help_str = help_pt_list_del,
 599         .tokens = {        /* token list, NULL terminated */
 600                 (prog_void *)&cmd_pt_list_arg0,
 601                 (prog_void *)&cmd_pt_list_del_arg,
 602                 (prog_void *)&cmd_pt_list_arg2,
 603                 NULL,
 604         },
 605 };
 606 /* show */
 607
 608 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
 609 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
 610
 611 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
 612 parse_pgm_inst_t cmd_pt_list_show = {
 613         .f = cmd_pt_list_parsed,  /* function to call */
 614         .data = NULL,      /* 2nd arg of func */
 615         .help_str = help_pt_list_show,
 616         .tokens = {        /* token list, NULL terminated */
 617                 (prog_void *)&cmd_pt_list_arg0,
 618                 (prog_void *)&cmd_pt_list_show_arg,
 619                 NULL,
 620         },
 621 };
 622
 623
 624
 625 /**********************************************************/
 626 /* Goto function */
 627
 628 /* this structure is filled when cmd_goto is parsed successfully */
 629 struct cmd_goto_result {
 630         fixed_string_t arg0;
 631         fixed_string_t arg1;
 632         int32_t arg2;
 633         int32_t arg3;
 634         int32_t arg4;
 635 };
 636
 637 /* function called when cmd_goto is parsed successfully */
 638 static void cmd_goto_parsed(void * parsed_result, void * data)
 639 {
 640         struct cmd_goto_result * res = parsed_result;
 641         uint8_t err;
 642         microseconds t1, t2;
 643
 644         interrupt_traj_reset();
 645         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
 646                 trajectory_a_rel(&mainboard.traj, res->arg2);
 647         }
 648         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
 649                 trajectory_d_rel(&mainboard.traj, res->arg2);
 650         }
 651         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
 652                 trajectory_a_abs(&mainboard.traj, res->arg2);
 653         }
 654         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
 655                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
 656         }
 657         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
 658                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
 659         }
 660         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
 661                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
 662         }
 663         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
 664                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 665         }
 666         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
 667 #if 0
 668                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
 669                 if (err != END_TRAJ && err != END_NEAR)
 670                         strat_hardstop();
 671 #else
 672                 printf_P(PSTR("not implemented\r\n"));
 673                 return;
 674 #endif
 675         }
 676         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
 677 #if 0
 678                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
 679                 if (err != END_TRAJ && err != END_NEAR)
 680                         strat_hardstop();
 681 #else
 682                 printf_P(PSTR("not implemented\r\n"));
 683                 return;
 684 #endif
 685         }
 686         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
 687                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 688         }
 689         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
 690                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 691         }
 692         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
 693                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
 694         }
 695         t1 = time_get_us2();
 696         while ((err = test_traj_end(0xFF)) == 0) {
 697                 t2 = time_get_us2();
 698                 if (t2 - t1 > 200000) {
 699                         dump_cs_debug("angle", &mainboard.angle.cs);
 700                         dump_cs_debug("distance", &mainboard.distance.cs);
 701                         t1 = t2;
 702                 }
 703         }
 704         if (err != END_TRAJ && err != END_NEAR)
 705                 strat_hardstop();
 706         printf_P(PSTR("returned %s\r\n"), get_err(err));
 707 }
 708
 709 prog_char str_goto_arg0[] = "goto";
 710 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
 711 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
 712 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
 713 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
 714
 715 /* 1 params */
 716 prog_char help_goto1[] = "Change orientation of the mainboard";
 717 parse_pgm_inst_t cmd_goto1 = {
 718         .f = cmd_goto_parsed,  /* function to call */
 719         .data = NULL,      /* 2nd arg of func */
 720         .help_str = help_goto1,
 721         .tokens = {        /* token list, NULL terminated */
 722                 (prog_void *)&cmd_goto_arg0,
 723                 (prog_void *)&cmd_goto_arg1_a,
 724                 (prog_void *)&cmd_goto_arg2,
 725                 NULL,
 726         },
 727 };
 728
 729 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";
 730 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
 731 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
 732
 733 /* 2 params */
 734 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
 735 parse_pgm_inst_t cmd_goto2 = {
 736         .f = cmd_goto_parsed,  /* function to call */
 737         .data = NULL,      /* 2nd arg of func */
 738         .help_str = help_goto2,
 739         .tokens = {        /* token list, NULL terminated */
 740                 (prog_void *)&cmd_goto_arg0,
 741                 (prog_void *)&cmd_goto_arg1_b,
 742                 (prog_void *)&cmd_goto_arg2,
 743                 (prog_void *)&cmd_goto_arg3,
 744                 NULL,
 745         },
 746 };
 747
 748 /**********************************************************/
 749 /* Position tests */
 750
 751 /* this structure is filled when cmd_position is parsed successfully */
 752 struct cmd_position_result {
 753         fixed_string_t arg0;
 754         fixed_string_t arg1;
 755         int32_t arg2;
 756         int32_t arg3;
 757         int32_t arg4;
 758 };
 759
 760 #define AUTOPOS_SPEED_FAST 200
 761 static void auto_position(void)
 762 {
 763         uint8_t err;
 764         uint16_t old_spdd, old_spda;
 765
 766         interrupt_traj_reset();
 767         strat_get_speed(&old_spdd, &old_spda);
 768         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
 769
 770         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
 771         if (err == END_INTR)
 772                 goto intr;
 773         strat_reset_pos(ROBOT_WIDTH/2 + 100,
 774                         COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
 775                         COLOR_A(-90));
 776         strat_hardstop();
 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         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
 789         if (err == END_INTR)
 790                 goto intr;
 791         strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
 792                         DO_NOT_SET_POS,
 793                         180);
 794         strat_hardstop();
 795
 796         trajectory_d_rel(&mainboard.traj, -170);
 797         err = wait_traj_end(END_INTR|END_TRAJ);
 798         if (err == END_INTR)
 799                 goto intr;
 800         wait_ms(100);
 801
 802         trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
 803         err = wait_traj_end(END_INTR|END_TRAJ);
 804         if (err == END_INTR)
 805                 goto intr;
 806         wait_ms(100);
 807
 808         strat_set_speed(old_spdd, old_spda);
 809         return;
 810
 811 intr:
 812         strat_hardstop();
 813         strat_set_speed(old_spdd, old_spda);
 814 }
 815
 816 /* function called when cmd_position is parsed successfully */
 817 static void cmd_position_parsed(void * parsed_result, void * data)
 818 {
 819         struct cmd_position_result * res = parsed_result;
 820
 821         /* display raw position values */
 822         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 823                 position_set(&mainboard.pos, 0, 0, 0);
 824         }
 825         else if (!strcmp_P(res->arg1, PSTR("set"))) {
 826                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
 827         }
 828         else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
 829                 mainboard.our_color = I2C_COLOR_BLUE;
 830 #ifndef HOST_VERSION
 831                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
 832                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
 833 #endif
 834                 auto_position();
 835         }
 836         else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
 837                 mainboard.our_color = I2C_COLOR_YELLOW;
 838 #ifndef HOST_VERSION
 839                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
 840                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
 841 #endif
 842                 auto_position();
 843         }
 844
 845         /* else it's just a "show" */
 846         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
 847                  position_get_x_double(&mainboard.pos),
 848                  position_get_y_double(&mainboard.pos),
 849                  DEG(position_get_a_rad_double(&mainboard.pos)));
 850 }
 851
 852 prog_char str_position_arg0[] = "position";
 853 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
 854 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
 855 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
 856
 857 prog_char help_position[] = "Show/reset (x,y,a) position";
 858 parse_pgm_inst_t cmd_position = {
 859         .f = cmd_position_parsed,  /* function to call */
 860         .data = NULL,      /* 2nd arg of func */
 861         .help_str = help_position,
 862         .tokens = {        /* token list, NULL terminated */
 863                 (prog_void *)&cmd_position_arg0,
 864                 (prog_void *)&cmd_position_arg1,
 865                 NULL,
 866         },
 867 };
 868
 869
 870 prog_char str_position_arg1_set[] = "set";
 871 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
 872 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
 873 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
 874 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
 875
 876 prog_char help_position_set[] = "Set (x,y,a) position";
 877 parse_pgm_inst_t cmd_position_set = {
 878         .f = cmd_position_parsed,  /* function to call */
 879         .data = NULL,      /* 2nd arg of func */
 880         .help_str = help_position_set,
 881         .tokens = {        /* token list, NULL terminated */
 882                 (prog_void *)&cmd_position_arg0,
 883                 (prog_void *)&cmd_position_arg1_set,
 884                 (prog_void *)&cmd_position_arg2,
 885                 (prog_void *)&cmd_position_arg3,
 886                 (prog_void *)&cmd_position_arg4,
 887                 NULL,
 888         },
 889 };
 890
 891
 892 /**********************************************************/
 893 /* strat configuration */
 894
 895 /* this structure is filled when cmd_strat_db is parsed successfully */
 896 struct cmd_strat_db_result {
 897         fixed_string_t arg0;
 898         fixed_string_t arg1;
 899 };
 900
 901 /* function called when cmd_strat_db is parsed successfully */
 902 static void cmd_strat_db_parsed(void *parsed_result, void *data)
 903 {
 904         struct cmd_strat_db_result *res = parsed_result;
 905
 906         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 907                 strat_db_init();
 908         }
 909         strat_db.dump_enabled = 1;
 910         strat_db_dump(__FUNCTION__);
 911 }
 912
 913 prog_char str_strat_db_arg0[] = "strat_db";
 914 parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0);
 915 prog_char str_strat_db_arg1[] = "show#reset";
 916 parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1);
 917
 918 prog_char help_strat_db[] = "reset/show strat_db";
 919 parse_pgm_inst_t cmd_strat_db = {
 920         .f = cmd_strat_db_parsed,  /* function to call */
 921         .data = NULL,      /* 2nd arg of func */
 922         .help_str = help_strat_db,
 923         .tokens = {        /* token list, NULL terminated */
 924                 (prog_void *)&cmd_strat_db_arg0,
 925                 (prog_void *)&cmd_strat_db_arg1,
 926                 NULL,
 927         },
 928 };
 929
 930 /**********************************************************/
 931 /* strat configuration */
 932
 933 /* this structure is filled when cmd_strat_conf is parsed successfully */
 934 struct cmd_strat_conf_result {
 935         fixed_string_t arg0;
 936         fixed_string_t arg1;
 937 };
 938
 939 /* function called when cmd_strat_conf is parsed successfully */
 940 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
 941 {
 942         //      struct cmd_strat_conf_result *res = parsed_result;
 943         strat_conf.dump_enabled = 1;
 944         strat_conf_dump(__FUNCTION__);
 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_conf.flags |= bit;
1006         else
1007                 strat_conf.flags &= (~bit);
1008
1009         strat_conf.dump_enabled = 1;
1010         strat_conf_dump(__FUNCTION__);
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_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_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_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_conf.delay_between_our_scan = res->arg2;
1069         }
1070         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1071                 strat_conf.wait_opponent = res->arg2;
1072         }
1073         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1074                 strat_conf.lintel_min_time = res->arg2;
1075         }
1076 #endif
1077         strat_conf.dump_enabled = 1;
1078         strat_conf_dump(__FUNCTION__);
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