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, -180);
 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, -170);
 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(-110));
 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 (!strcmp_P(res->arg1, PSTR("our_orange")))
1046                 bit = STRAT_CONF_OUR_ORANGE;
1047
1048         if (on)
1049                 strat_conf.flags |= bit;
1050         else
1051                 strat_conf.flags &= (~bit);
1052
1053         strat_conf.dump_enabled = 1;
1054         strat_conf_dump(__FUNCTION__);
1055 }
1056
1057 prog_char str_strat_conf2_arg0[] = "strat_conf";
1058 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1059 prog_char str_strat_conf2_arg1[] = "our_orange";
1060 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1061 prog_char str_strat_conf2_arg2[] = "on#off";
1062 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1063
1064
1065 prog_char help_strat_conf2[] = "configure strat options";
1066 parse_pgm_inst_t cmd_strat_conf2 = {
1067         .f = cmd_strat_conf2_parsed,  /* function to call */
1068         .data = NULL,      /* 2nd arg of func */
1069         .help_str = help_strat_conf2,
1070         .tokens = {        /* token list, NULL terminated */
1071                 (prog_void *)&cmd_strat_conf2_arg0,
1072                 (prog_void *)&cmd_strat_conf2_arg1,
1073                 (prog_void *)&cmd_strat_conf2_arg2,
1074                 NULL,
1075         },
1076 };
1077
1078 /**********************************************************/
1079 /* strat configuration */
1080
1081 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1082 struct cmd_strat_conf3_result {
1083         fixed_string_t arg0;
1084         fixed_string_t arg1;
1085         uint8_t arg2;
1086 };
1087
1088 /* function called when cmd_strat_conf3 is parsed successfully */
1089 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1090 {
1091         struct cmd_strat_conf3_result *res = parsed_result;
1092
1093         if (!strcmp_P(res->arg1, PSTR("orphan_tomato"))) {
1094                 if (res->arg2 > 90)
1095                         res->arg2 = 90;
1096                 strat_conf.orphan_tomato = res->arg2;
1097         }
1098         else if (!strcmp_P(res->arg1, PSTR("opp_orange"))) {
1099                 if (res->arg2 > 90)
1100                         res->arg2 = 90;
1101                 strat_conf.opp_orange = res->arg2;
1102         }
1103         strat_conf.dump_enabled = 1;
1104         strat_conf_dump(__FUNCTION__);
1105 }
1106
1107 prog_char str_strat_conf3_arg0[] = "strat_conf";
1108 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1109 prog_char str_strat_conf3_arg1[] = "orphan_tomato#opp_orange";
1110 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1111 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
1112
1113 prog_char help_strat_conf3[] = "configure strat options";
1114 parse_pgm_inst_t cmd_strat_conf3 = {
1115         .f = cmd_strat_conf3_parsed,  /* function to call */
1116         .data = NULL,      /* 2nd arg of func */
1117         .help_str = help_strat_conf3,
1118         .tokens = {        /* token list, NULL terminated */
1119                 (prog_void *)&cmd_strat_conf3_arg0,
1120                 (prog_void *)&cmd_strat_conf3_arg1,
1121                 (prog_void *)&cmd_strat_conf3_arg2,
1122                 NULL,
1123         },
1124 };
1125
1126
1127 /**********************************************************/
1128 /* Subtraj */
1129
1130
1131 /* function called when cmd_test is parsed successfully */
1132 static void subtraj_test(void)
1133 {
1134 }
1135
1136
1137 /* this structure is filled when cmd_subtraj is parsed successfully */
1138 struct cmd_subtraj_result {
1139         fixed_string_t arg0;
1140         fixed_string_t arg1;
1141         int32_t arg2;
1142         int32_t arg3;
1143         int32_t arg4;
1144         int32_t arg5;
1145 };
1146
1147 /* function called when cmd_subtraj is parsed successfully */
1148 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1149 {
1150         struct cmd_subtraj_result *res = parsed_result;
1151
1152         if (!strcmp_P(res->arg1, PSTR("test")))
1153                 subtraj_test();
1154         else if (!strcmp_P(res->arg1, PSTR("orange")))
1155                 subtraj_test();
1156         else if (!strcmp_P(res->arg1, PSTR("tomato"))) {
1157                 position_set(&mainboard.pos, 2625,
1158                              COLOR_Y(1847), COLOR_A(0.00));
1159                 get_orphan_tomatoes();
1160         }
1161
1162         trajectory_hardstop(&mainboard.traj);
1163 }
1164
1165 prog_char str_subtraj_arg0[] = "subtraj";
1166 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1167 prog_char str_subtraj_arg1[] = "test#orange#tomato";
1168 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1169 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1170 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1171 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1172 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1173
1174 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1175 parse_pgm_inst_t cmd_subtraj = {
1176         .f = cmd_subtraj_parsed,  /* function to call */
1177         .data = NULL,      /* 2nd arg of func */
1178         .help_str = help_subtraj,
1179         .tokens = {        /* token list, NULL terminated */
1180                 (prog_void *)&cmd_subtraj_arg0,
1181                 (prog_void *)&cmd_subtraj_arg1,
1182                 (prog_void *)&cmd_subtraj_arg2,
1183                 (prog_void *)&cmd_subtraj_arg3,
1184                 (prog_void *)&cmd_subtraj_arg4,
1185                 (prog_void *)&cmd_subtraj_arg5,
1186                 NULL,
1187         },
1188 };
1189