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 /* centrifugal configuration */
 447
 448 /* this structure is filled when cmd_centrifugal is parsed successfully */
 449 struct cmd_centrifugal_result {
 450         fixed_string_t arg0;
 451         fixed_string_t arg1;
 452         float val;
 453 };
 454
 455 /* function called when cmd_centrifugal is parsed successfully */
 456 static void cmd_centrifugal_parsed(void * parsed_result, void * data)
 457 {
 458         struct cmd_centrifugal_result * res = parsed_result;
 459
 460         if (!strcmp_P(res->arg1, PSTR("set"))) {
 461                 position_set_centrifugal_coef(&mainboard.pos, res->val);
 462         }
 463         printf_P(PSTR("centrifugal set %f\r\n"), mainboard.pos.centrifugal_coef);
 464 }
 465
 466 prog_char str_centrifugal_arg0[] = "centrifugal";
 467 parse_pgm_token_string_t cmd_centrifugal_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg0, str_centrifugal_arg0);
 468 prog_char str_centrifugal_arg1[] = "set";
 469 parse_pgm_token_string_t cmd_centrifugal_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_arg1);
 470 parse_pgm_token_num_t cmd_centrifugal_val = TOKEN_NUM_INITIALIZER(struct cmd_centrifugal_result, val, FLOAT);
 471
 472 prog_char help_centrifugal[] = "Set centrifugal coef";
 473 parse_pgm_inst_t cmd_centrifugal = {
 474         .f = cmd_centrifugal_parsed,  /* function to call */
 475         .data = NULL,      /* 2nd arg of func */
 476         .help_str = help_centrifugal,
 477         .tokens = {        /* token list, NULL terminated */
 478                 (prog_void *)&cmd_centrifugal_arg0,
 479                 (prog_void *)&cmd_centrifugal_arg1,
 480                 (prog_void *)&cmd_centrifugal_val,
 481                 NULL,
 482         },
 483 };
 484
 485 /* show */
 486
 487 prog_char str_centrifugal_show_arg[] = "show";
 488 parse_pgm_token_string_t cmd_centrifugal_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_show_arg);
 489
 490 prog_char help_centrifugal_show[] = "Show centrifugal";
 491 parse_pgm_inst_t cmd_centrifugal_show = {
 492         .f = cmd_centrifugal_parsed,  /* function to call */
 493         .data = NULL,      /* 2nd arg of func */
 494         .help_str = help_centrifugal_show,
 495         .tokens = {        /* token list, NULL terminated */
 496                 (prog_void *)&cmd_centrifugal_arg0,
 497                 (prog_void *)&cmd_centrifugal_show_arg,
 498                 NULL,
 499         },
 500 };
 501
 502
 503
 504 /**********************************************************/
 505 /* Pt_Lists for testing traj */
 506
 507 #define PT_LIST_SIZE 10
 508 static struct xy_point pt_list[PT_LIST_SIZE];
 509 static uint16_t pt_list_len = 0;
 510
 511 /* this structure is filled when cmd_pt_list is parsed successfully */
 512 struct cmd_pt_list_result {
 513         fixed_string_t arg0;
 514         fixed_string_t arg1;
 515         uint16_t arg2;
 516         int16_t arg3;
 517         int16_t arg4;
 518 };
 519
 520 /* function called when cmd_pt_list is parsed successfully */
 521 static void cmd_pt_list_parsed(void * parsed_result, void * data)
 522 {
 523         struct cmd_pt_list_result * res = parsed_result;
 524         uint8_t i, why=0;
 525
 526         if (!strcmp_P(res->arg1, PSTR("append"))) {
 527                 res->arg2 = pt_list_len;
 528         }
 529         if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
 530                 printf_P(PSTR("removed\r\n"));
 531                 return;
 532         }
 533
 534         if (!strcmp_P(res->arg1, PSTR("insert")) ||
 535             !strcmp_P(res->arg1, PSTR("append"))) {
 536                 if (res->arg2 > pt_list_len) {
 537                         printf_P(PSTR("Index too large\r\n"));
 538                         return;
 539                 }
 540                 if (pt_list_len >= PT_LIST_SIZE) {
 541                         printf_P(PSTR("List is too large\r\n"));
 542                         return;
 543                 }
 544                 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
 545                        PT_LIST_SIZE-1-res->arg2);
 546                 pt_list[res->arg2].x = res->arg3;
 547                 pt_list[res->arg2].y = res->arg4;
 548                 pt_list_len++;
 549         }
 550         else if (!strcmp_P(res->arg1, PSTR("del"))) {
 551                 if (pt_list_len <= 0) {
 552                         printf_P(PSTR("Error: list empty\r\n"));
 553                         return;
 554                 }
 555                 if (res->arg2 > pt_list_len) {
 556                         printf_P(PSTR("Index too large\r\n"));
 557                         return;
 558                 }
 559                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
 560                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
 561                 pt_list_len--;
 562         }
 563         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
 564                 pt_list_len = 0;
 565         }
 566
 567         /* else it is a "show" or a "start" */
 568         if (pt_list_len == 0) {
 569                 printf_P(PSTR("List empty\r\n"));
 570                 return;
 571         }
 572  restart:
 573         for (i=0 ; i<pt_list_len ; i++) {
 574                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
 575                 if (!strcmp_P(res->arg1, PSTR("start"))) {
 576                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
 577                         why = wait_traj_end(0xFF); /* all */
 578                 }
 579                 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
 580                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
 581                         why = wait_traj_end(0xFF); /* all */
 582                 }
 583 #if 0
 584                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
 585                         while (1) {
 586                                 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
 587                                 printf("next point\r\n");
 588                                 if (why != END_OBSTACLE)
 589                                         break;
 590                         }
 591                 }
 592 #endif
 593                 if (why & (~(END_TRAJ | END_NEAR)))
 594                         trajectory_stop(&mainboard.traj);
 595                 if (why & END_INTR)
 596                         break;
 597         }
 598         if (why & END_INTR)
 599                 return;
 600         if (!strcmp_P(res->arg1, PSTR("loop_start")))
 601                 goto restart;
 602 }
 603
 604 prog_char str_pt_list_arg0[] = "pt_list";
 605 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
 606 prog_char str_pt_list_arg1[] = "insert";
 607 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
 608 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
 609 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
 610 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
 611
 612 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
 613 parse_pgm_inst_t cmd_pt_list = {
 614         .f = cmd_pt_list_parsed,  /* function to call */
 615         .data = NULL,      /* 2nd arg of func */
 616         .help_str = help_pt_list,
 617         .tokens = {        /* token list, NULL terminated */
 618                 (prog_void *)&cmd_pt_list_arg0,
 619                 (prog_void *)&cmd_pt_list_arg1,
 620                 (prog_void *)&cmd_pt_list_arg2,
 621                 (prog_void *)&cmd_pt_list_arg3,
 622                 (prog_void *)&cmd_pt_list_arg4,
 623                 NULL,
 624         },
 625 };
 626
 627 /* append */
 628
 629 prog_char str_pt_list_arg1_append[] = "append";
 630 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
 631
 632 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
 633 parse_pgm_inst_t cmd_pt_list_append = {
 634         .f = cmd_pt_list_parsed,  /* function to call */
 635         .data = NULL,      /* 2nd arg of func */
 636         .help_str = help_pt_list_append,
 637         .tokens = {        /* token list, NULL terminated */
 638                 (prog_void *)&cmd_pt_list_arg0,
 639                 (prog_void *)&cmd_pt_list_arg1_append,
 640                 (prog_void *)&cmd_pt_list_arg3,
 641                 (prog_void *)&cmd_pt_list_arg4,
 642                 NULL,
 643         },
 644 };
 645
 646 /* del */
 647
 648 prog_char str_pt_list_del_arg[] = "del";
 649 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
 650
 651 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
 652 parse_pgm_inst_t cmd_pt_list_del = {
 653         .f = cmd_pt_list_parsed,  /* function to call */
 654         .data = NULL,      /* 2nd arg of func */
 655         .help_str = help_pt_list_del,
 656         .tokens = {        /* token list, NULL terminated */
 657                 (prog_void *)&cmd_pt_list_arg0,
 658                 (prog_void *)&cmd_pt_list_del_arg,
 659                 (prog_void *)&cmd_pt_list_arg2,
 660                 NULL,
 661         },
 662 };
 663 /* show */
 664
 665 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
 666 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
 667
 668 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
 669 parse_pgm_inst_t cmd_pt_list_show = {
 670         .f = cmd_pt_list_parsed,  /* function to call */
 671         .data = NULL,      /* 2nd arg of func */
 672         .help_str = help_pt_list_show,
 673         .tokens = {        /* token list, NULL terminated */
 674                 (prog_void *)&cmd_pt_list_arg0,
 675                 (prog_void *)&cmd_pt_list_show_arg,
 676                 NULL,
 677         },
 678 };
 679
 680
 681
 682 /**********************************************************/
 683 /* Goto function */
 684
 685 /* this structure is filled when cmd_goto is parsed successfully */
 686 struct cmd_goto_result {
 687         fixed_string_t arg0;
 688         fixed_string_t arg1;
 689         int32_t arg2;
 690         int32_t arg3;
 691         int32_t arg4;
 692 };
 693
 694 /* function called when cmd_goto is parsed successfully */
 695 static void cmd_goto_parsed(void * parsed_result, void * data)
 696 {
 697         struct cmd_goto_result * res = parsed_result;
 698         uint8_t err;
 699         microseconds t1, t2;
 700
 701         interrupt_traj_reset();
 702         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
 703                 trajectory_a_rel(&mainboard.traj, res->arg2);
 704         }
 705         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
 706                 trajectory_d_rel(&mainboard.traj, res->arg2);
 707         }
 708         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
 709                 trajectory_a_abs(&mainboard.traj, res->arg2);
 710         }
 711         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
 712                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
 713         }
 714         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
 715                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
 716         }
 717         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
 718                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
 719         }
 720         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
 721                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 722         }
 723         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
 724 #if 0
 725                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
 726                 if (err != END_TRAJ && err != END_NEAR)
 727                         strat_hardstop();
 728 #else
 729                 printf_P(PSTR("not implemented\r\n"));
 730                 return;
 731 #endif
 732         }
 733         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
 734 #if 0
 735                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
 736                 if (err != END_TRAJ && err != END_NEAR)
 737                         strat_hardstop();
 738 #else
 739                 printf_P(PSTR("not implemented\r\n"));
 740                 return;
 741 #endif
 742         }
 743         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
 744                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 745         }
 746         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
 747                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 748         }
 749         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
 750                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
 751         }
 752         t1 = time_get_us2();
 753         while ((err = test_traj_end(0xFF)) == 0) {
 754                 t2 = time_get_us2();
 755                 if (t2 - t1 > 200000) {
 756                         dump_cs_debug("angle", &mainboard.angle.cs);
 757                         dump_cs_debug("distance", &mainboard.distance.cs);
 758                         t1 = t2;
 759                 }
 760         }
 761         if (err != END_TRAJ && err != END_NEAR)
 762                 strat_hardstop();
 763         printf_P(PSTR("returned %s\r\n"), get_err(err));
 764 }
 765
 766 prog_char str_goto_arg0[] = "goto";
 767 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
 768 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
 769 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
 770 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
 771
 772 /* 1 params */
 773 prog_char help_goto1[] = "Change orientation of the mainboard";
 774 parse_pgm_inst_t cmd_goto1 = {
 775         .f = cmd_goto_parsed,  /* function to call */
 776         .data = NULL,      /* 2nd arg of func */
 777         .help_str = help_goto1,
 778         .tokens = {        /* token list, NULL terminated */
 779                 (prog_void *)&cmd_goto_arg0,
 780                 (prog_void *)&cmd_goto_arg1_a,
 781                 (prog_void *)&cmd_goto_arg2,
 782                 NULL,
 783         },
 784 };
 785
 786 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";
 787 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
 788 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
 789
 790 /* 2 params */
 791 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
 792 parse_pgm_inst_t cmd_goto2 = {
 793         .f = cmd_goto_parsed,  /* function to call */
 794         .data = NULL,      /* 2nd arg of func */
 795         .help_str = help_goto2,
 796         .tokens = {        /* token list, NULL terminated */
 797                 (prog_void *)&cmd_goto_arg0,
 798                 (prog_void *)&cmd_goto_arg1_b,
 799                 (prog_void *)&cmd_goto_arg2,
 800                 (prog_void *)&cmd_goto_arg3,
 801                 NULL,
 802         },
 803 };
 804
 805 /**********************************************************/
 806 /* Position tests */
 807
 808 /* this structure is filled when cmd_position is parsed successfully */
 809 struct cmd_position_result {
 810         fixed_string_t arg0;
 811         fixed_string_t arg1;
 812         int32_t arg2;
 813         int32_t arg3;
 814         int32_t arg4;
 815 };
 816
 817 #define AUTOPOS_SPEED_FAST 200
 818 static void auto_position(void)
 819 {
 820         uint8_t err;
 821         uint16_t old_spdd, old_spda;
 822
 823         interrupt_traj_reset();
 824         strat_get_speed(&old_spdd, &old_spda);
 825         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
 826
 827         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
 828         if (err == END_INTR)
 829                 goto intr;
 830         strat_reset_pos(ROBOT_WIDTH/2 + 100,
 831                         COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
 832                         COLOR_A(-90));
 833         strat_hardstop();
 834
 835         trajectory_d_rel(&mainboard.traj, -180);
 836         err = wait_traj_end(END_INTR|END_TRAJ);
 837         if (err == END_INTR)
 838                 goto intr;
 839
 840         trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
 841         err = wait_traj_end(END_INTR|END_TRAJ);
 842         if (err == END_INTR)
 843                 goto intr;
 844
 845         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
 846         if (err == END_INTR)
 847                 goto intr;
 848         strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
 849                         DO_NOT_SET_POS,
 850                         180);
 851         strat_hardstop();
 852
 853         trajectory_d_rel(&mainboard.traj, -170);
 854         err = wait_traj_end(END_INTR|END_TRAJ);
 855         if (err == END_INTR)
 856                 goto intr;
 857         wait_ms(100);
 858
 859         trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
 860         err = wait_traj_end(END_INTR|END_TRAJ);
 861         if (err == END_INTR)
 862                 goto intr;
 863         wait_ms(100);
 864
 865         strat_set_speed(old_spdd, old_spda);
 866         return;
 867
 868 intr:
 869         strat_hardstop();
 870         strat_set_speed(old_spdd, old_spda);
 871 }
 872
 873 /* function called when cmd_position is parsed successfully */
 874 static void cmd_position_parsed(void * parsed_result, void * data)
 875 {
 876         struct cmd_position_result * res = parsed_result;
 877
 878         /* display raw position values */
 879         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 880                 position_set(&mainboard.pos, 0, 0, 0);
 881         }
 882         else if (!strcmp_P(res->arg1, PSTR("set"))) {
 883                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
 884         }
 885         else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
 886                 mainboard.our_color = I2C_COLOR_BLUE;
 887 #ifndef HOST_VERSION
 888                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
 889                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
 890 #endif
 891                 auto_position();
 892         }
 893         else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
 894                 mainboard.our_color = I2C_COLOR_YELLOW;
 895 #ifndef HOST_VERSION
 896                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
 897                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
 898 #endif
 899                 auto_position();
 900         }
 901
 902         /* else it's just a "show" */
 903         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
 904                  position_get_x_double(&mainboard.pos),
 905                  position_get_y_double(&mainboard.pos),
 906                  DEG(position_get_a_rad_double(&mainboard.pos)));
 907 }
 908
 909 prog_char str_position_arg0[] = "position";
 910 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
 911 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
 912 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
 913
 914 prog_char help_position[] = "Show/reset (x,y,a) position";
 915 parse_pgm_inst_t cmd_position = {
 916         .f = cmd_position_parsed,  /* function to call */
 917         .data = NULL,      /* 2nd arg of func */
 918         .help_str = help_position,
 919         .tokens = {        /* token list, NULL terminated */
 920                 (prog_void *)&cmd_position_arg0,
 921                 (prog_void *)&cmd_position_arg1,
 922                 NULL,
 923         },
 924 };
 925
 926
 927 prog_char str_position_arg1_set[] = "set";
 928 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
 929 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
 930 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
 931 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
 932
 933 prog_char help_position_set[] = "Set (x,y,a) position";
 934 parse_pgm_inst_t cmd_position_set = {
 935         .f = cmd_position_parsed,  /* function to call */
 936         .data = NULL,      /* 2nd arg of func */
 937         .help_str = help_position_set,
 938         .tokens = {        /* token list, NULL terminated */
 939                 (prog_void *)&cmd_position_arg0,
 940                 (prog_void *)&cmd_position_arg1_set,
 941                 (prog_void *)&cmd_position_arg2,
 942                 (prog_void *)&cmd_position_arg3,
 943                 (prog_void *)&cmd_position_arg4,
 944                 NULL,
 945         },
 946 };
 947
 948
 949 /**********************************************************/
 950 /* strat configuration */
 951
 952 /* this structure is filled when cmd_strat_db is parsed successfully */
 953 struct cmd_strat_db_result {
 954         fixed_string_t arg0;
 955         fixed_string_t arg1;
 956 };
 957
 958 /* function called when cmd_strat_db is parsed successfully */
 959 static void cmd_strat_db_parsed(void *parsed_result, void *data)
 960 {
 961         struct cmd_strat_db_result *res = parsed_result;
 962
 963         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 964                 strat_db_init();
 965         }
 966         strat_db.dump_enabled = 1;
 967         strat_db_dump(__FUNCTION__);
 968 }
 969
 970 prog_char str_strat_db_arg0[] = "strat_db";
 971 parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0);
 972 prog_char str_strat_db_arg1[] = "show#reset";
 973 parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1);
 974
 975 prog_char help_strat_db[] = "reset/show strat_db";
 976 parse_pgm_inst_t cmd_strat_db = {
 977         .f = cmd_strat_db_parsed,  /* function to call */
 978         .data = NULL,      /* 2nd arg of func */
 979         .help_str = help_strat_db,
 980         .tokens = {        /* token list, NULL terminated */
 981                 (prog_void *)&cmd_strat_db_arg0,
 982                 (prog_void *)&cmd_strat_db_arg1,
 983                 NULL,
 984         },
 985 };
 986
 987 /**********************************************************/
 988 /* strat configuration */
 989
 990 /* this structure is filled when cmd_strat_conf is parsed successfully */
 991 struct cmd_strat_conf_result {
 992         fixed_string_t arg0;
 993         fixed_string_t arg1;
 994 };
 995
 996 /* function called when cmd_strat_conf is parsed successfully */
 997 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
 998 {
 999         //      struct cmd_strat_conf_result *res = parsed_result;
1000         strat_conf.dump_enabled = 1;
1001         strat_conf_dump(__FUNCTION__);
1002 }
1003
1004 prog_char str_strat_conf_arg0[] = "strat_conf";
1005 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
1006 prog_char str_strat_conf_arg1[] = "show#base";
1007 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
1008
1009 prog_char help_strat_conf[] = "configure strat options";
1010 parse_pgm_inst_t cmd_strat_conf = {
1011         .f = cmd_strat_conf_parsed,  /* function to call */
1012         .data = NULL,      /* 2nd arg of func */
1013         .help_str = help_strat_conf,
1014         .tokens = {        /* token list, NULL terminated */
1015                 (prog_void *)&cmd_strat_conf_arg0,
1016                 (prog_void *)&cmd_strat_conf_arg1,
1017                 NULL,
1018         },
1019 };
1020
1021 /**********************************************************/
1022 /* strat configuration */
1023
1024 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
1025 struct cmd_strat_conf2_result {
1026         fixed_string_t arg0;
1027         fixed_string_t arg1;
1028         fixed_string_t arg2;
1029 };
1030
1031 /* function called when cmd_strat_conf2 is parsed successfully */
1032 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
1033 {
1034         struct cmd_strat_conf2_result *res = parsed_result;
1035         uint8_t on, bit = 0;
1036
1037         if (!strcmp_P(res->arg2, PSTR("on")))
1038                 on = 1;
1039         else
1040                 on = 0;
1041
1042 #if 0
1043         if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
1044                 bit = STRAT_CONF_ONLY_ONE_ON_DISC;
1045         else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
1046                 bit = STRAT_CONF_BYPASS_STATIC2;
1047         else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
1048                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
1049         else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
1050                 bit = STRAT_CONF_TAKE_ONE_LINTEL;
1051         else if (!strcmp_P(res->arg1, PSTR("store_static2")))
1052                 bit = STRAT_CONF_STORE_STATIC2;
1053         else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
1054                 bit = STRAT_CONF_BIG_3_TEMPLE;
1055         else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
1056                 bit = STRAT_CONF_EARLY_SCAN;
1057         else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
1058                 bit = STRAT_CONF_PUSH_OPP_COLS;
1059 #endif
1060
1061         if (on)
1062                 strat_conf.flags |= bit;
1063         else
1064                 strat_conf.flags &= (~bit);
1065
1066         strat_conf.dump_enabled = 1;
1067         strat_conf_dump(__FUNCTION__);
1068 }
1069
1070 prog_char str_strat_conf2_arg0[] = "strat_conf";
1071 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1072 prog_char str_strat_conf2_arg1[] = "faux";
1073 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1074 prog_char str_strat_conf2_arg2[] = "on#off";
1075 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1076
1077
1078 prog_char help_strat_conf2[] = "configure strat options";
1079 parse_pgm_inst_t cmd_strat_conf2 = {
1080         .f = cmd_strat_conf2_parsed,  /* function to call */
1081         .data = NULL,      /* 2nd arg of func */
1082         .help_str = help_strat_conf2,
1083         .tokens = {        /* token list, NULL terminated */
1084                 (prog_void *)&cmd_strat_conf2_arg0,
1085                 (prog_void *)&cmd_strat_conf2_arg1,
1086                 (prog_void *)&cmd_strat_conf2_arg2,
1087                 NULL,
1088         },
1089 };
1090
1091 /**********************************************************/
1092 /* strat configuration */
1093
1094 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1095 struct cmd_strat_conf3_result {
1096         fixed_string_t arg0;
1097         fixed_string_t arg1;
1098         uint8_t arg2;
1099 };
1100
1101 /* function called when cmd_strat_conf3 is parsed successfully */
1102 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1103 {
1104 #if 0
1105         struct cmd_strat_conf3_result *res = parsed_result;
1106
1107         if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
1108                 if (res->arg2 > 90)
1109                         res->arg2 = 90;
1110                 strat_conf.scan_opp_min_time = res->arg2;
1111         }
1112         else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
1113                 if (res->arg2 > 90)
1114                         res->arg2 = 90;
1115                 strat_conf.delay_between_opp_scan = res->arg2;
1116         }
1117         else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
1118                 if (res->arg2 > 90)
1119                         res->arg2 = 90;
1120                 strat_conf.scan_our_min_time = res->arg2;
1121         }
1122         else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
1123                 if (res->arg2 > 90)
1124                         res->arg2 = 90;
1125                 strat_conf.delay_between_our_scan = res->arg2;
1126         }
1127         else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
1128                 strat_conf.wait_opponent = res->arg2;
1129         }
1130         else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
1131                 strat_conf.lintel_min_time = res->arg2;
1132         }
1133 #endif
1134         strat_conf.dump_enabled = 1;
1135         strat_conf_dump(__FUNCTION__);
1136 }
1137
1138 prog_char str_strat_conf3_arg0[] = "strat_conf";
1139 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1140 prog_char str_strat_conf3_arg1[] = "faux2";
1141 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1142 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1143
1144 prog_char help_strat_conf3[] = "configure strat options";
1145 parse_pgm_inst_t cmd_strat_conf3 = {
1146         .f = cmd_strat_conf3_parsed,  /* function to call */
1147         .data = NULL,      /* 2nd arg of func */
1148         .help_str = help_strat_conf3,
1149         .tokens = {        /* token list, NULL terminated */
1150                 (prog_void *)&cmd_strat_conf3_arg0,
1151                 (prog_void *)&cmd_strat_conf3_arg1,
1152                 (prog_void *)&cmd_strat_conf3_arg2,
1153                 NULL,
1154         },
1155 };
1156
1157
1158 /**********************************************************/
1159 /* Subtraj */
1160
1161 //////////////////////
1162
1163 // 500 -- 5
1164 // 400 -- 3
1165 #define TEST_SPEED 400
1166 #define TEST_ACC 3
1167
1168 static void line2line(double line1x1, double line1y1,
1169                       double line1x2, double line1y2,
1170                       double line2x1, double line2y1,
1171                       double line2x2, double line2y2,
1172                       double radius, double dist)
1173 {
1174         uint8_t err;
1175         double speed_d, speed_a;
1176         double distance, angle;
1177         double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
1178         double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
1179
1180         printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1181
1182         strat_set_speed(TEST_SPEED, TEST_SPEED);
1183         quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1184
1185         circle_get_da_speed_from_radius(&mainboard.traj, radius,
1186                                         &speed_d, &speed_a);
1187         trajectory_line_abs(&mainboard.traj,
1188                             line1x1, line1y1,
1189                             line1x2, line1y2, 150.);
1190         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1191                                     dist, TRAJ_FLAGS_NO_NEAR);
1192         /* circle */
1193         strat_set_speed(speed_d, speed_a);
1194         angle = line2_angle - line1_angle;
1195         distance = angle * radius;
1196         if (distance < 0)
1197                 distance = -distance;
1198         angle = simple_modulo_2pi(angle);
1199         angle = DEG(angle);
1200         printf_P(PSTR("(%d,%d,%d) "),
1201                  position_get_x_s16(&mainboard.pos),
1202                  position_get_y_s16(&mainboard.pos),
1203                  position_get_a_deg_s16(&mainboard.pos));
1204         printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
1205                  distance, angle);
1206
1207         /* take some margin on dist to avoid deceleration */
1208         trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
1209
1210         /* circle exit condition */
1211         err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1212                                     TRAJ_FLAGS_NO_NEAR);
1213
1214         strat_set_speed(500, 500);
1215         printf_P(PSTR("(%d,%d,%d) "),
1216                  position_get_x_s16(&mainboard.pos),
1217                  position_get_y_s16(&mainboard.pos),
1218                  position_get_a_deg_s16(&mainboard.pos));
1219         printf_P(PSTR("line\r\n"));
1220         trajectory_line_abs(&mainboard.traj,
1221                             line2x1, line2y1,
1222                             line2x2, line2y2, 150.);
1223 }
1224
1225 static void halfturn(double line1x1, double line1y1,
1226                      double line1x2, double line1y2,
1227                      double line2x1, double line2y1,
1228                      double line2x2, double line2y2,
1229                      double radius, double dist, double dir)
1230 {
1231         uint8_t err;
1232         double speed_d, speed_a;
1233         double distance, angle;
1234
1235         printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1236
1237         strat_set_speed(TEST_SPEED, TEST_SPEED);
1238         quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
1239
1240         circle_get_da_speed_from_radius(&mainboard.traj, radius,
1241                                         &speed_d, &speed_a);
1242         trajectory_line_abs(&mainboard.traj,
1243                             line1x1, line1y1,
1244                             line1x2, line1y2, 150.);
1245         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
1246                                     dist, TRAJ_FLAGS_NO_NEAR);
1247         /* circle */
1248         strat_set_speed(speed_d, speed_a);
1249         angle = dir * M_PI/2.;
1250         distance = angle * radius;
1251         if (distance < 0)
1252                 distance = -distance;
1253         angle = simple_modulo_2pi(angle);
1254         angle = DEG(angle);
1255
1256         /* take some margin on dist to avoid deceleration */
1257         DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
1258               distance, angle);
1259         trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1260
1261         /* circle exit condition */
1262         err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1263                                     TRAJ_FLAGS_NO_NEAR);
1264
1265         DEBUG(E_USER_STRAT, "miniline");
1266         err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
1267                                     dist, TRAJ_FLAGS_NO_NEAR);
1268         DEBUG(E_USER_STRAT, "circle2");
1269         /* take some margin on dist to avoid deceleration */
1270         trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
1271
1272         err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
1273                                     TRAJ_FLAGS_NO_NEAR);
1274
1275         strat_set_speed(500, 500);
1276         DEBUG(E_USER_STRAT, "line");
1277         trajectory_line_abs(&mainboard.traj,
1278                             line2x1, line2y1,
1279                             line2x2, line2y2, 150.);
1280 }
1281
1282
1283 /* function called when cmd_test is parsed successfully */
1284 static void subtraj_test(void)
1285 {
1286 #ifdef HOST_VERSION
1287         strat_reset_pos(400, 400, 90);
1288         mainboard.angle.on = 1;
1289         mainboard.distance.on = 1;
1290 #endif
1291         printf_P(PSTR("%s()\r\n"), __FUNCTION__);
1292         while (!cmdline_keypressed()) {
1293                 /****** PASS1 */
1294
1295 #define DIST_HARD_TURN   260
1296 #define RADIUS_HARD_TURN 100
1297 #define DIST_EASY_TURN   190
1298 #define RADIUS_EASY_TURN 190
1299 #define DIST_HALF_TURN   225
1300 #define RADIUS_HALF_TURN 130
1301
1302                 /* hard turn */
1303                 line2line(375, 597, 375, 1847,
1304                           375, 1847, 1050, 1472,
1305                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1306
1307                 /* easy left and easy right !*/
1308                 line2line(825, 1596, 1050, 1472,
1309                           1050, 1472, 1500, 1722,
1310                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1311                 line2line(1050, 1472, 1500, 1722,
1312                           1500, 1722, 2175, 1347,
1313                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1314                 line2line(1500, 1722, 2175, 1347,
1315                           2175, 1347, 2175, 847,
1316                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1317
1318                 /* half turns */
1319                 halfturn(2175, 1347, 2175, 722,
1320                          2625, 722, 2625, 1597,
1321                          RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1322                 halfturn(2625, 847, 2625, 1722,
1323                           2175, 1722, 2175, 1097,
1324                           RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
1325
1326                 /* easy turns */
1327                 line2line(2175, 1597, 2175, 1097,
1328                           2175, 1097, 1500, 722,
1329                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1330                 line2line(2175, 1097, 1500, 722,
1331                           1500, 722, 1050, 972,
1332                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1333                 line2line(1500, 722, 1050, 972,
1334                           1050, 972, 375, 597,
1335                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1336
1337                 /* hard turn */
1338                 line2line(1050, 972, 375, 597,
1339                           375, 597, 375, 1097,
1340                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1341
1342                 /****** PASS2 */
1343
1344                 /* easy turn */
1345                 line2line(375, 597, 375, 1097,
1346                           375, 1097, 1050, 1472,
1347                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1348
1349                 /* hard turn */
1350                 line2line(375, 1097, 1050, 1472,
1351                           1050, 1472, 375, 1847,
1352                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1353
1354                 /* hard turn */
1355                 line2line(1050, 1472, 375, 1847,
1356                           375, 1847, 375, 1347,
1357                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1358
1359                 /* easy turn */
1360                 line2line(375, 1847, 375, 1347,
1361                           375, 1347, 1050, 972,
1362                           RADIUS_EASY_TURN, DIST_EASY_TURN);
1363
1364                 /* hard turn */
1365                 line2line(375, 1347, 1050, 972,
1366                           1050, 972, 375, 597,
1367                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1368
1369                 /* hard turn */
1370                 line2line(1050, 972, 375, 597,
1371                           375, 597, 375, 1847,
1372                           RADIUS_HARD_TURN, DIST_HARD_TURN);
1373
1374         }
1375         trajectory_hardstop(&mainboard.traj);
1376 }
1377
1378
1379 /* this structure is filled when cmd_subtraj is parsed successfully */
1380 struct cmd_subtraj_result {
1381         fixed_string_t arg0;
1382         fixed_string_t arg1;
1383         int32_t arg2;
1384         int32_t arg3;
1385         int32_t arg4;
1386         int32_t arg5;
1387 };
1388
1389 /* function called when cmd_subtraj is parsed successfully */
1390 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1391 {
1392         struct cmd_subtraj_result *res = parsed_result;
1393
1394         if (!strcmp_P(res->arg1, PSTR("test")))
1395                 subtraj_test();
1396
1397         trajectory_hardstop(&mainboard.traj);
1398 }
1399
1400 prog_char str_subtraj_arg0[] = "subtraj";
1401 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1402 prog_char str_subtraj_arg1[] = "faux";
1403 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1404 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1405 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1406 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1407 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1408
1409 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1410 parse_pgm_inst_t cmd_subtraj = {
1411         .f = cmd_subtraj_parsed,  /* function to call */
1412         .data = NULL,      /* 2nd arg of func */
1413         .help_str = help_subtraj,
1414         .tokens = {        /* token list, NULL terminated */
1415                 (prog_void *)&cmd_subtraj_arg0,
1416                 (prog_void *)&cmd_subtraj_arg1,
1417                 (prog_void *)&cmd_subtraj_arg2,
1418                 (prog_void *)&cmd_subtraj_arg3,
1419                 (prog_void *)&cmd_subtraj_arg4,
1420                 (prog_void *)&cmd_subtraj_arg5,
1421                 NULL,
1422         },
1423 };
1424