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