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 #include "beacon.h"
  66
  67 /**********************************************************/
  68 /* Traj_Speeds for trajectory_manager */
  69
  70 /* this structure is filled when cmd_traj_speed is parsed successfully */
  71 struct cmd_traj_speed_result {
  72         fixed_string_t arg0;
  73         fixed_string_t arg1;
  74         float s;
  75 };
  76
  77 /* function called when cmd_traj_speed is parsed successfully */
  78 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
  79 {
  80         struct cmd_traj_speed_result * res = parsed_result;
  81
  82         if (!strcmp_P(res->arg1, PSTR("angle"))) {
  83                 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
  84         }
  85         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
  86                 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
  87         }
  88         /* else it is a "show" */
  89
  90         printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
  91                  mainboard.traj.a_speed,
  92                  mainboard.traj.d_speed);
  93 }
  94
  95 prog_char str_traj_speed_arg0[] = "traj_speed";
  96 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
  97 prog_char str_traj_speed_arg1[] = "angle#distance";
  98 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
  99 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, FLOAT);
 100
 101 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
 102 parse_pgm_inst_t cmd_traj_speed = {
 103         .f = cmd_traj_speed_parsed,  /* function to call */
 104         .data = NULL,      /* 2nd arg of func */
 105         .help_str = help_traj_speed,
 106         .tokens = {        /* token list, NULL terminated */
 107                 (prog_void *)&cmd_traj_speed_arg0,
 108                 (prog_void *)&cmd_traj_speed_arg1,
 109                 (prog_void *)&cmd_traj_speed_s,
 110                 NULL,
 111         },
 112 };
 113
 114 /* show */
 115
 116 prog_char str_traj_speed_show_arg[] = "show";
 117 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
 118
 119 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
 120 parse_pgm_inst_t cmd_traj_speed_show = {
 121         .f = cmd_traj_speed_parsed,  /* function to call */
 122         .data = NULL,      /* 2nd arg of func */
 123         .help_str = help_traj_speed_show,
 124         .tokens = {        /* token list, NULL terminated */
 125                 (prog_void *)&cmd_traj_speed_arg0,
 126                 (prog_void *)&cmd_traj_speed_show_arg,
 127                 NULL,
 128         },
 129 };
 130
 131 /**********************************************************/
 132 /* Traj_Accs for trajectory_manager */
 133
 134 /* this structure is filled when cmd_traj_acc is parsed successfully */
 135 struct cmd_traj_acc_result {
 136         fixed_string_t arg0;
 137         fixed_string_t arg1;
 138         float s;
 139 };
 140
 141 /* function called when cmd_traj_acc is parsed successfully */
 142 static void cmd_traj_acc_parsed(void *parsed_result, void *data)
 143 {
 144         struct cmd_traj_acc_result * res = parsed_result;
 145
 146         if (!strcmp_P(res->arg1, PSTR("angle"))) {
 147                 trajectory_set_acc(&mainboard.traj, mainboard.traj.d_acc, res->s);
 148         }
 149         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
 150                 trajectory_set_acc(&mainboard.traj, res->s, mainboard.traj.a_acc);
 151         }
 152         /* else it is a "show" */
 153
 154         printf_P(PSTR("angle %2.2f, distance %2.2f\r\n"),
 155                  mainboard.traj.a_acc,
 156                  mainboard.traj.d_acc);
 157 }
 158
 159 prog_char str_traj_acc_arg0[] = "traj_acc";
 160 parse_pgm_token_string_t cmd_traj_acc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg0, str_traj_acc_arg0);
 161 prog_char str_traj_acc_arg1[] = "angle#distance";
 162 parse_pgm_token_string_t cmd_traj_acc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_arg1);
 163 parse_pgm_token_num_t cmd_traj_acc_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_acc_result, s, FLOAT);
 164
 165 prog_char help_traj_acc[] = "Set traj_acc values for trajectory manager";
 166 parse_pgm_inst_t cmd_traj_acc = {
 167         .f = cmd_traj_acc_parsed,  /* function to call */
 168         .data = NULL,      /* 2nd arg of func */
 169         .help_str = help_traj_acc,
 170         .tokens = {        /* token list, NULL terminated */
 171                 (prog_void *)&cmd_traj_acc_arg0,
 172                 (prog_void *)&cmd_traj_acc_arg1,
 173                 (prog_void *)&cmd_traj_acc_s,
 174                 NULL,
 175         },
 176 };
 177
 178 /* show */
 179
 180 prog_char str_traj_acc_show_arg[] = "show";
 181 parse_pgm_token_string_t cmd_traj_acc_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_acc_result, arg1, str_traj_acc_show_arg);
 182
 183 prog_char help_traj_acc_show[] = "Show traj_acc values for trajectory manager";
 184 parse_pgm_inst_t cmd_traj_acc_show = {
 185         .f = cmd_traj_acc_parsed,  /* function to call */
 186         .data = NULL,      /* 2nd arg of func */
 187         .help_str = help_traj_acc_show,
 188         .tokens = {        /* token list, NULL terminated */
 189                 (prog_void *)&cmd_traj_acc_arg0,
 190                 (prog_void *)&cmd_traj_acc_show_arg,
 191                 NULL,
 192         },
 193 };
 194
 195 /**********************************************************/
 196 /* circle coef configuration */
 197
 198 /* this structure is filled when cmd_circle_coef is parsed successfully */
 199 struct cmd_circle_coef_result {
 200         fixed_string_t arg0;
 201         fixed_string_t arg1;
 202         float circle_coef;
 203 };
 204
 205
 206 /* function called when cmd_circle_coef is parsed successfully */
 207 static void cmd_circle_coef_parsed(void *parsed_result, void *data)
 208 {
 209         struct cmd_circle_coef_result *res = parsed_result;
 210
 211         if (!strcmp_P(res->arg1, PSTR("set"))) {
 212                 trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
 213         }
 214
 215         printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
 216 }
 217
 218 prog_char str_circle_coef_arg0[] = "circle_coef";
 219 parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
 220 prog_char str_circle_coef_arg1[] = "set";
 221 parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
 222 parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
 223
 224 prog_char help_circle_coef[] = "Set circle coef";
 225 parse_pgm_inst_t cmd_circle_coef = {
 226         .f = cmd_circle_coef_parsed,  /* function to call */
 227         .data = NULL,      /* 2nd arg of func */
 228         .help_str = help_circle_coef,
 229         .tokens = {        /* token list, NULL terminated */
 230                 (prog_void *)&cmd_circle_coef_arg0,
 231                 (prog_void *)&cmd_circle_coef_arg1,
 232                 (prog_void *)&cmd_circle_coef_val,
 233                 NULL,
 234         },
 235 };
 236
 237 /* show */
 238
 239 prog_char str_circle_coef_show_arg[] = "show";
 240 parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
 241
 242 prog_char help_circle_coef_show[] = "Show circle coef";
 243 parse_pgm_inst_t cmd_circle_coef_show = {
 244         .f = cmd_circle_coef_parsed,  /* function to call */
 245         .data = NULL,      /* 2nd arg of func */
 246         .help_str = help_circle_coef_show,
 247         .tokens = {        /* token list, NULL terminated */
 248                 (prog_void *)&cmd_circle_coef_arg0,
 249                 (prog_void *)&cmd_circle_coef_show_arg,
 250                 NULL,
 251         },
 252 };
 253
 254 /**********************************************************/
 255 /* trajectory window configuration */
 256
 257 /* this structure is filled when cmd_trajectory is parsed successfully */
 258 struct cmd_trajectory_result {
 259         fixed_string_t arg0;
 260         fixed_string_t arg1;
 261         float d_win;
 262         float a_win;
 263         float a_start;
 264 };
 265
 266
 267 /* function called when cmd_trajectory is parsed successfully */
 268 static void cmd_trajectory_parsed(void * parsed_result, void * data)
 269 {
 270         struct cmd_trajectory_result * res = parsed_result;
 271
 272         if (!strcmp_P(res->arg1, PSTR("set"))) {
 273                 trajectory_set_windows(&mainboard.traj, res->d_win,
 274                                        res->a_win, res->a_start);
 275         }
 276
 277         printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
 278                  DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
 279 }
 280
 281 prog_char str_trajectory_arg0[] = "trajectory";
 282 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
 283 prog_char str_trajectory_arg1[] = "set";
 284 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
 285 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
 286 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
 287 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
 288
 289 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
 290 parse_pgm_inst_t cmd_trajectory = {
 291         .f = cmd_trajectory_parsed,  /* function to call */
 292         .data = NULL,      /* 2nd arg of func */
 293         .help_str = help_trajectory,
 294         .tokens = {        /* token list, NULL terminated */
 295                 (prog_void *)&cmd_trajectory_arg0,
 296                 (prog_void *)&cmd_trajectory_arg1,
 297                 (prog_void *)&cmd_trajectory_d,
 298                 (prog_void *)&cmd_trajectory_a,
 299                 (prog_void *)&cmd_trajectory_as,
 300                 NULL,
 301         },
 302 };
 303
 304 /* show */
 305
 306 prog_char str_trajectory_show_arg[] = "show";
 307 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
 308
 309 prog_char help_trajectory_show[] = "Show trajectory window configuration";
 310 parse_pgm_inst_t cmd_trajectory_show = {
 311         .f = cmd_trajectory_parsed,  /* function to call */
 312         .data = NULL,      /* 2nd arg of func */
 313         .help_str = help_trajectory_show,
 314         .tokens = {        /* token list, NULL terminated */
 315                 (prog_void *)&cmd_trajectory_arg0,
 316                 (prog_void *)&cmd_trajectory_show_arg,
 317                 NULL,
 318         },
 319 };
 320
 321 /**********************************************************/
 322 /* rs_gains configuration */
 323
 324 /* this structure is filled when cmd_rs_gains is parsed successfully */
 325 struct cmd_rs_gains_result {
 326         fixed_string_t arg0;
 327         fixed_string_t arg1;
 328         float left;
 329         float right;
 330 };
 331
 332 /* function called when cmd_rs_gains is parsed successfully */
 333 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
 334 {
 335 #ifdef HOST_VERSION
 336         printf("not implemented\n");
 337 #else
 338         struct cmd_rs_gains_result * res = parsed_result;
 339
 340         if (!strcmp_P(res->arg1, PSTR("set"))) {
 341                 rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
 342                                         LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
 343                 rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
 344                                          RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
 345         }
 346         printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
 347                  mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
 348 #endif
 349 }
 350
 351 prog_char str_rs_gains_arg0[] = "rs_gains";
 352 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
 353 prog_char str_rs_gains_arg1[] = "set";
 354 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
 355 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
 356 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
 357
 358 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
 359 parse_pgm_inst_t cmd_rs_gains = {
 360         .f = cmd_rs_gains_parsed,  /* function to call */
 361         .data = NULL,      /* 2nd arg of func */
 362         .help_str = help_rs_gains,
 363         .tokens = {        /* token list, NULL terminated */
 364                 (prog_void *)&cmd_rs_gains_arg0,
 365                 (prog_void *)&cmd_rs_gains_arg1,
 366                 (prog_void *)&cmd_rs_gains_l,
 367                 (prog_void *)&cmd_rs_gains_r,
 368                 NULL,
 369         },
 370 };
 371
 372 /* show */
 373
 374 prog_char str_rs_gains_show_arg[] = "show";
 375 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
 376
 377 prog_char help_rs_gains_show[] = "Show rs_gains";
 378 parse_pgm_inst_t cmd_rs_gains_show = {
 379         .f = cmd_rs_gains_parsed,  /* function to call */
 380         .data = NULL,      /* 2nd arg of func */
 381         .help_str = help_rs_gains_show,
 382         .tokens = {        /* token list, NULL terminated */
 383                 (prog_void *)&cmd_rs_gains_arg0,
 384                 (prog_void *)&cmd_rs_gains_show_arg,
 385                 NULL,
 386         },
 387 };
 388
 389 /**********************************************************/
 390 /* track configuration */
 391
 392 /* this structure is filled when cmd_track is parsed successfully */
 393 struct cmd_track_result {
 394         fixed_string_t arg0;
 395         fixed_string_t arg1;
 396         float val;
 397 };
 398
 399 /* function called when cmd_track is parsed successfully */
 400 static void cmd_track_parsed(void * parsed_result, void * data)
 401 {
 402         struct cmd_track_result * res = parsed_result;
 403
 404         if (!strcmp_P(res->arg1, PSTR("set"))) {
 405                 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
 406         }
 407         printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
 408 }
 409
 410 prog_char str_track_arg0[] = "track";
 411 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
 412 prog_char str_track_arg1[] = "set";
 413 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
 414 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
 415
 416 prog_char help_track[] = "Set track in mm";
 417 parse_pgm_inst_t cmd_track = {
 418         .f = cmd_track_parsed,  /* function to call */
 419         .data = NULL,      /* 2nd arg of func */
 420         .help_str = help_track,
 421         .tokens = {        /* token list, NULL terminated */
 422                 (prog_void *)&cmd_track_arg0,
 423                 (prog_void *)&cmd_track_arg1,
 424                 (prog_void *)&cmd_track_val,
 425                 NULL,
 426         },
 427 };
 428
 429 /* show */
 430
 431 prog_char str_track_show_arg[] = "show";
 432 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
 433
 434 prog_char help_track_show[] = "Show track";
 435 parse_pgm_inst_t cmd_track_show = {
 436         .f = cmd_track_parsed,  /* function to call */
 437         .data = NULL,      /* 2nd arg of func */
 438         .help_str = help_track_show,
 439         .tokens = {        /* token list, NULL terminated */
 440                 (prog_void *)&cmd_track_arg0,
 441                 (prog_void *)&cmd_track_show_arg,
 442                 NULL,
 443         },
 444 };
 445
 446 /**********************************************************/
 447 /* centrifugal configuration */
 448
 449 /* this structure is filled when cmd_centrifugal is parsed successfully */
 450 struct cmd_centrifugal_result {
 451         fixed_string_t arg0;
 452         fixed_string_t arg1;
 453         float val;
 454 };
 455
 456 /* function called when cmd_centrifugal is parsed successfully */
 457 static void cmd_centrifugal_parsed(void * parsed_result, void * data)
 458 {
 459         struct cmd_centrifugal_result * res = parsed_result;
 460
 461         if (!strcmp_P(res->arg1, PSTR("set"))) {
 462                 position_set_centrifugal_coef(&mainboard.pos, res->val);
 463         }
 464         printf_P(PSTR("centrifugal set %f\r\n"), mainboard.pos.centrifugal_coef);
 465 }
 466
 467 prog_char str_centrifugal_arg0[] = "centrifugal";
 468 parse_pgm_token_string_t cmd_centrifugal_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg0, str_centrifugal_arg0);
 469 prog_char str_centrifugal_arg1[] = "set";
 470 parse_pgm_token_string_t cmd_centrifugal_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_arg1);
 471 parse_pgm_token_num_t cmd_centrifugal_val = TOKEN_NUM_INITIALIZER(struct cmd_centrifugal_result, val, FLOAT);
 472
 473 prog_char help_centrifugal[] = "Set centrifugal coef";
 474 parse_pgm_inst_t cmd_centrifugal = {
 475         .f = cmd_centrifugal_parsed,  /* function to call */
 476         .data = NULL,      /* 2nd arg of func */
 477         .help_str = help_centrifugal,
 478         .tokens = {        /* token list, NULL terminated */
 479                 (prog_void *)&cmd_centrifugal_arg0,
 480                 (prog_void *)&cmd_centrifugal_arg1,
 481                 (prog_void *)&cmd_centrifugal_val,
 482                 NULL,
 483         },
 484 };
 485
 486 /* show */
 487
 488 prog_char str_centrifugal_show_arg[] = "show";
 489 parse_pgm_token_string_t cmd_centrifugal_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_centrifugal_result, arg1, str_centrifugal_show_arg);
 490
 491 prog_char help_centrifugal_show[] = "Show centrifugal";
 492 parse_pgm_inst_t cmd_centrifugal_show = {
 493         .f = cmd_centrifugal_parsed,  /* function to call */
 494         .data = NULL,      /* 2nd arg of func */
 495         .help_str = help_centrifugal_show,
 496         .tokens = {        /* token list, NULL terminated */
 497                 (prog_void *)&cmd_centrifugal_arg0,
 498                 (prog_void *)&cmd_centrifugal_show_arg,
 499                 NULL,
 500         },
 501 };
 502
 503
 504
 505 /**********************************************************/
 506 /* Pt_Lists for testing traj */
 507
 508 #define PT_LIST_SIZE 10
 509 static struct xy_point pt_list[PT_LIST_SIZE];
 510 static uint16_t pt_list_len = 0;
 511
 512 /* this structure is filled when cmd_pt_list is parsed successfully */
 513 struct cmd_pt_list_result {
 514         fixed_string_t arg0;
 515         fixed_string_t arg1;
 516         uint16_t arg2;
 517         int16_t arg3;
 518         int16_t arg4;
 519 };
 520
 521 /* function called when cmd_pt_list is parsed successfully */
 522 static void cmd_pt_list_parsed(void * parsed_result, void * data)
 523 {
 524         struct cmd_pt_list_result * res = parsed_result;
 525         uint8_t i, why=0;
 526
 527         if (!strcmp_P(res->arg1, PSTR("append"))) {
 528                 res->arg2 = pt_list_len;
 529         }
 530         if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
 531                 printf_P(PSTR("removed\r\n"));
 532                 return;
 533         }
 534
 535         if (!strcmp_P(res->arg1, PSTR("insert")) ||
 536             !strcmp_P(res->arg1, PSTR("append"))) {
 537                 if (res->arg2 > pt_list_len) {
 538                         printf_P(PSTR("Index too large\r\n"));
 539                         return;
 540                 }
 541                 if (pt_list_len >= PT_LIST_SIZE) {
 542                         printf_P(PSTR("List is too large\r\n"));
 543                         return;
 544                 }
 545                 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
 546                        PT_LIST_SIZE-1-res->arg2);
 547                 pt_list[res->arg2].x = res->arg3;
 548                 pt_list[res->arg2].y = res->arg4;
 549                 pt_list_len++;
 550         }
 551         else if (!strcmp_P(res->arg1, PSTR("del"))) {
 552                 if (pt_list_len <= 0) {
 553                         printf_P(PSTR("Error: list empty\r\n"));
 554                         return;
 555                 }
 556                 if (res->arg2 > pt_list_len) {
 557                         printf_P(PSTR("Index too large\r\n"));
 558                         return;
 559                 }
 560                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
 561                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
 562                 pt_list_len--;
 563         }
 564         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
 565                 pt_list_len = 0;
 566         }
 567
 568         /* else it is a "show" or a "start" */
 569         if (pt_list_len == 0) {
 570                 printf_P(PSTR("List empty\r\n"));
 571                 return;
 572         }
 573  restart:
 574         for (i=0 ; i<pt_list_len ; i++) {
 575                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
 576                 if (!strcmp_P(res->arg1, PSTR("start"))) {
 577                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
 578                         why = wait_traj_end(0xFF); /* all */
 579                 }
 580                 else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
 581                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
 582                         why = wait_traj_end(0xFF); /* all */
 583                 }
 584 #if 0
 585                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
 586                         while (1) {
 587                                 why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
 588                                 printf("next point\r\n");
 589                                 if (why != END_OBSTACLE)
 590                                         break;
 591                         }
 592                 }
 593 #endif
 594                 if (why & (~(END_TRAJ | END_NEAR)))
 595                         trajectory_stop(&mainboard.traj);
 596                 if (why & END_INTR)
 597                         break;
 598         }
 599         if (why & END_INTR)
 600                 return;
 601         if (!strcmp_P(res->arg1, PSTR("loop_start")))
 602                 goto restart;
 603 }
 604
 605 prog_char str_pt_list_arg0[] = "pt_list";
 606 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
 607 prog_char str_pt_list_arg1[] = "insert";
 608 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
 609 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
 610 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
 611 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
 612
 613 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
 614 parse_pgm_inst_t cmd_pt_list = {
 615         .f = cmd_pt_list_parsed,  /* function to call */
 616         .data = NULL,      /* 2nd arg of func */
 617         .help_str = help_pt_list,
 618         .tokens = {        /* token list, NULL terminated */
 619                 (prog_void *)&cmd_pt_list_arg0,
 620                 (prog_void *)&cmd_pt_list_arg1,
 621                 (prog_void *)&cmd_pt_list_arg2,
 622                 (prog_void *)&cmd_pt_list_arg3,
 623                 (prog_void *)&cmd_pt_list_arg4,
 624                 NULL,
 625         },
 626 };
 627
 628 /* append */
 629
 630 prog_char str_pt_list_arg1_append[] = "append";
 631 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
 632
 633 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
 634 parse_pgm_inst_t cmd_pt_list_append = {
 635         .f = cmd_pt_list_parsed,  /* function to call */
 636         .data = NULL,      /* 2nd arg of func */
 637         .help_str = help_pt_list_append,
 638         .tokens = {        /* token list, NULL terminated */
 639                 (prog_void *)&cmd_pt_list_arg0,
 640                 (prog_void *)&cmd_pt_list_arg1_append,
 641                 (prog_void *)&cmd_pt_list_arg3,
 642                 (prog_void *)&cmd_pt_list_arg4,
 643                 NULL,
 644         },
 645 };
 646
 647 /* del */
 648
 649 prog_char str_pt_list_del_arg[] = "del";
 650 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
 651
 652 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
 653 parse_pgm_inst_t cmd_pt_list_del = {
 654         .f = cmd_pt_list_parsed,  /* function to call */
 655         .data = NULL,      /* 2nd arg of func */
 656         .help_str = help_pt_list_del,
 657         .tokens = {        /* token list, NULL terminated */
 658                 (prog_void *)&cmd_pt_list_arg0,
 659                 (prog_void *)&cmd_pt_list_del_arg,
 660                 (prog_void *)&cmd_pt_list_arg2,
 661                 NULL,
 662         },
 663 };
 664 /* show */
 665
 666 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
 667 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
 668
 669 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
 670 parse_pgm_inst_t cmd_pt_list_show = {
 671         .f = cmd_pt_list_parsed,  /* function to call */
 672         .data = NULL,      /* 2nd arg of func */
 673         .help_str = help_pt_list_show,
 674         .tokens = {        /* token list, NULL terminated */
 675                 (prog_void *)&cmd_pt_list_arg0,
 676                 (prog_void *)&cmd_pt_list_show_arg,
 677                 NULL,
 678         },
 679 };
 680
 681
 682
 683 /**********************************************************/
 684 /* Goto function */
 685
 686 /* this structure is filled when cmd_goto is parsed successfully */
 687 struct cmd_goto_result {
 688         fixed_string_t arg0;
 689         fixed_string_t arg1;
 690         int32_t arg2;
 691         int32_t arg3;
 692         int32_t arg4;
 693 };
 694
 695 /* function called when cmd_goto is parsed successfully */
 696 static void cmd_goto_parsed(void * parsed_result, void * data)
 697 {
 698         struct cmd_goto_result * res = parsed_result;
 699         uint8_t err;
 700         microseconds t1, t2;
 701
 702         interrupt_traj_reset();
 703         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
 704                 trajectory_a_rel(&mainboard.traj, res->arg2);
 705         }
 706         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
 707                 trajectory_d_rel(&mainboard.traj, res->arg2);
 708         }
 709         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
 710                 trajectory_a_abs(&mainboard.traj, res->arg2);
 711         }
 712         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
 713                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
 714         }
 715         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
 716                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
 717         }
 718         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
 719                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
 720         }
 721         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
 722                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 723         }
 724         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
 725 #if 0
 726                 err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
 727                 if (err != END_TRAJ && err != END_NEAR)
 728                         strat_hardstop();
 729 #else
 730                 printf_P(PSTR("not implemented\r\n"));
 731                 return;
 732 #endif
 733         }
 734         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
 735 #if 0
 736                 err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
 737                 if (err != END_TRAJ && err != END_NEAR)
 738                         strat_hardstop();
 739 #else
 740                 printf_P(PSTR("not implemented\r\n"));
 741                 return;
 742 #endif
 743         }
 744         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
 745                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 746         }
 747         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
 748                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 749         }
 750         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
 751                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
 752         }
 753         t1 = time_get_us2();
 754         while ((err = test_traj_end(0xFF)) == 0) {
 755                 t2 = time_get_us2();
 756                 if (t2 - t1 > 200000) {
 757                         dump_cs_debug("angle", &mainboard.angle.cs);
 758                         dump_cs_debug("distance", &mainboard.distance.cs);
 759                         t1 = t2;
 760                 }
 761         }
 762         if (err != END_TRAJ && err != END_NEAR)
 763                 strat_hardstop();
 764         printf_P(PSTR("returned %s\r\n"), get_err(err));
 765 }
 766
 767 prog_char str_goto_arg0[] = "goto";
 768 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
 769 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
 770 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
 771 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
 772
 773 /* 1 params */
 774 prog_char help_goto1[] = "Change orientation of the mainboard";
 775 parse_pgm_inst_t cmd_goto1 = {
 776         .f = cmd_goto_parsed,  /* function to call */
 777         .data = NULL,      /* 2nd arg of func */
 778         .help_str = help_goto1,
 779         .tokens = {        /* token list, NULL terminated */
 780                 (prog_void *)&cmd_goto_arg0,
 781                 (prog_void *)&cmd_goto_arg1_a,
 782                 (prog_void *)&cmd_goto_arg2,
 783                 NULL,
 784         },
 785 };
 786
 787 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";
 788 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
 789 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
 790
 791 /* 2 params */
 792 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
 793 parse_pgm_inst_t cmd_goto2 = {
 794         .f = cmd_goto_parsed,  /* function to call */
 795         .data = NULL,      /* 2nd arg of func */
 796         .help_str = help_goto2,
 797         .tokens = {        /* token list, NULL terminated */
 798                 (prog_void *)&cmd_goto_arg0,
 799                 (prog_void *)&cmd_goto_arg1_b,
 800                 (prog_void *)&cmd_goto_arg2,
 801                 (prog_void *)&cmd_goto_arg3,
 802                 NULL,
 803         },
 804 };
 805
 806 /**********************************************************/
 807 /* Position tests */
 808
 809 /* this structure is filled when cmd_position is parsed successfully */
 810 struct cmd_position_result {
 811         fixed_string_t arg0;
 812         fixed_string_t arg1;
 813         int32_t arg2;
 814         int32_t arg3;
 815         int32_t arg4;
 816 };
 817
 818 #define AUTOPOS_SPEED_FAST 200
 819 static void auto_position(void)
 820 {
 821         uint8_t err;
 822         uint16_t old_spdd, old_spda;
 823
 824         interrupt_traj_reset();
 825         strat_get_speed(&old_spdd, &old_spda);
 826         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
 827         strat_set_acc(3, 3);
 828
 829         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
 830         if (err == END_INTR)
 831                 goto intr;
 832         strat_reset_pos(ROBOT_WIDTH/2 + 100,
 833                         COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
 834                         COLOR_A(-90) + ROBOT_ANGLE_FRONT);
 835         strat_hardstop();
 836
 837         trajectory_d_rel(&mainboard.traj, -180);
 838         err = wait_traj_end(END_INTR|END_TRAJ);
 839         if (err == END_INTR)
 840                 goto intr;
 841
 842         time_wait_ms(250);
 843         trajectory_a_rel(&mainboard.traj, COLOR_A(-90));
 844         err = wait_traj_end(END_INTR|END_TRAJ);
 845         if (err == END_INTR)
 846                 goto intr;
 847
 848         time_wait_ms(250);
 849         err = strat_calib(300, END_INTR|END_TRAJ|END_BLOCKING);
 850         if (err == END_INTR)
 851                 goto intr;
 852         strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
 853                         DO_NOT_SET_POS,
 854                         180 + ROBOT_ANGLE_FRONT);
 855         strat_hardstop();
 856
 857         trajectory_d_rel(&mainboard.traj, -170);
 858         err = wait_traj_end(END_INTR|END_TRAJ);
 859         if (err == END_INTR)
 860                 goto intr;
 861         time_wait_ms(250);
 862
 863         trajectory_a_rel(&mainboard.traj, COLOR_A(-110));
 864         err = wait_traj_end(END_INTR|END_TRAJ);
 865         if (err == END_INTR)
 866                 goto intr;
 867         time_wait_ms(250);
 868
 869         strat_set_speed(old_spdd, old_spda);
 870         return;
 871
 872 intr:
 873         strat_hardstop();
 874         strat_set_speed(old_spdd, old_spda);
 875 }
 876
 877 /* function called when cmd_position is parsed successfully */
 878 static void cmd_position_parsed(void * parsed_result, void * data)
 879 {
 880         struct cmd_position_result * res = parsed_result;
 881
 882         /* display raw position values */
 883         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 884                 position_set(&mainboard.pos, 0, 0, 0);
 885         }
 886         else if (!strcmp_P(res->arg1, PSTR("set"))) {
 887                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
 888         }
 889         else if (!strcmp_P(res->arg1, PSTR("autoset_blue")) ||
 890                  !strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
 891                 mainboard.our_color = I2C_COLOR_BLUE;
 892 #ifndef HOST_VERSION
 893                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
 894                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
 895                 beacon_set_color(I2C_COLOR_YELLOW);
 896 #endif
 897                 auto_position();
 898         }
 899         else if (!strcmp_P(res->arg1, PSTR("autoset_yellow")) ||
 900                  !strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
 901                 mainboard.our_color = I2C_COLOR_YELLOW;
 902 #ifndef HOST_VERSION
 903                 i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
 904                 i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
 905                 beacon_set_color(I2C_COLOR_BLUE);
 906 #endif
 907                 auto_position();
 908         }
 909
 910         if (!strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
 911                 strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
 912                 prepare_hill(I2C_COLOR_BLUE, 340);
 913         }
 914         else if (!strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
 915                 strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
 916                 prepare_hill(I2C_COLOR_YELLOW, 340);
 917         }
 918
 919         /* else it's just a "show" */
 920         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
 921                  position_get_x_double(&mainboard.pos),
 922                  position_get_y_double(&mainboard.pos),
 923                  DEG(position_get_a_rad_double(&mainboard.pos)));
 924 }
 925
 926 prog_char str_position_arg0[] = "position";
 927 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
 928 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow#autoset_or_blue#autoset_or_yellow";
 929 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
 930
 931 prog_char help_position[] = "Show/reset (x,y,a) position";
 932 parse_pgm_inst_t cmd_position = {
 933         .f = cmd_position_parsed,  /* function to call */
 934         .data = NULL,      /* 2nd arg of func */
 935         .help_str = help_position,
 936         .tokens = {        /* token list, NULL terminated */
 937                 (prog_void *)&cmd_position_arg0,
 938                 (prog_void *)&cmd_position_arg1,
 939                 NULL,
 940         },
 941 };
 942
 943
 944 prog_char str_position_arg1_set[] = "set";
 945 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
 946 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
 947 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
 948 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
 949
 950 prog_char help_position_set[] = "Set (x,y,a) position";
 951 parse_pgm_inst_t cmd_position_set = {
 952         .f = cmd_position_parsed,  /* function to call */
 953         .data = NULL,      /* 2nd arg of func */
 954         .help_str = help_position_set,
 955         .tokens = {        /* token list, NULL terminated */
 956                 (prog_void *)&cmd_position_arg0,
 957                 (prog_void *)&cmd_position_arg1_set,
 958                 (prog_void *)&cmd_position_arg2,
 959                 (prog_void *)&cmd_position_arg3,
 960                 (prog_void *)&cmd_position_arg4,
 961                 NULL,
 962         },
 963 };
 964
 965
 966 /**********************************************************/
 967 /* strat configuration */
 968
 969 /* this structure is filled when cmd_strat_db is parsed successfully */
 970 struct cmd_strat_db_result {
 971         fixed_string_t arg0;
 972         fixed_string_t arg1;
 973 };
 974
 975 /* function called when cmd_strat_db is parsed successfully */
 976 static void cmd_strat_db_parsed(void *parsed_result, void *data)
 977 {
 978         struct cmd_strat_db_result *res = parsed_result;
 979
 980         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 981                 strat_db_init();
 982         }
 983         strat_db.dump_enabled = 1;
 984         strat_db_dump(__FUNCTION__);
 985 }
 986
 987 prog_char str_strat_db_arg0[] = "strat_db";
 988 parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0);
 989 prog_char str_strat_db_arg1[] = "show#reset";
 990 parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1);
 991
 992 prog_char help_strat_db[] = "reset/show strat_db";
 993 parse_pgm_inst_t cmd_strat_db = {
 994         .f = cmd_strat_db_parsed,  /* function to call */
 995         .data = NULL,      /* 2nd arg of func */
 996         .help_str = help_strat_db,
 997         .tokens = {        /* token list, NULL terminated */
 998                 (prog_void *)&cmd_strat_db_arg0,
 999                 (prog_void *)&cmd_strat_db_arg1,
1000                 NULL,
1001         },
1002 };
1003
1004 /**********************************************************/
1005 /* strat configuration */
1006
1007 /* this structure is filled when cmd_strat_conf is parsed successfully */
1008 struct cmd_strat_conf_result {
1009         fixed_string_t arg0;
1010         fixed_string_t arg1;
1011 };
1012
1013 /* function called when cmd_strat_conf is parsed successfully */
1014 static void cmd_strat_conf_parsed(void *parsed_result, void *data)
1015 {
1016         //      struct cmd_strat_conf_result *res = parsed_result;
1017         strat_conf.dump_enabled = 1;
1018         strat_conf_dump(__FUNCTION__);
1019 }
1020
1021 prog_char str_strat_conf_arg0[] = "strat_conf";
1022 parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0);
1023 prog_char str_strat_conf_arg1[] = "show#base";
1024 parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1);
1025
1026 prog_char help_strat_conf[] = "configure strat options";
1027 parse_pgm_inst_t cmd_strat_conf = {
1028         .f = cmd_strat_conf_parsed,  /* function to call */
1029         .data = NULL,      /* 2nd arg of func */
1030         .help_str = help_strat_conf,
1031         .tokens = {        /* token list, NULL terminated */
1032                 (prog_void *)&cmd_strat_conf_arg0,
1033                 (prog_void *)&cmd_strat_conf_arg1,
1034                 NULL,
1035         },
1036 };
1037
1038 /**********************************************************/
1039 /* strat configuration */
1040
1041 /* this structure is filled when cmd_strat_conf2 is parsed successfully */
1042 struct cmd_strat_conf2_result {
1043         fixed_string_t arg0;
1044         fixed_string_t arg1;
1045         fixed_string_t arg2;
1046 };
1047
1048 /* function called when cmd_strat_conf2 is parsed successfully */
1049 static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
1050 {
1051         struct cmd_strat_conf2_result *res = parsed_result;
1052         uint8_t on, bit = 0;
1053
1054         if (!strcmp_P(res->arg2, PSTR("on")))
1055                 on = 1;
1056         else
1057                 on = 0;
1058
1059         if (!strcmp_P(res->arg1, PSTR("our_orange")))
1060                 bit = STRAT_CONF_OUR_ORANGE;
1061         else if (!strcmp_P(res->arg1, PSTR("wait_obstacle")))
1062                 bit = STRAT_CONF_WAIT_OBSTACLE;
1063         else if (!strcmp_P(res->arg1, PSTR("straight_begin")))
1064                 bit = STRAT_CONF_STRAIGHT_BEGIN;
1065
1066         if (on)
1067                 strat_conf.flags |= bit;
1068         else
1069                 strat_conf.flags &= (~bit);
1070
1071         strat_conf.dump_enabled = 1;
1072         strat_conf_dump(__FUNCTION__);
1073 }
1074
1075 prog_char str_strat_conf2_arg0[] = "strat_conf";
1076 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
1077 prog_char str_strat_conf2_arg1[] = "our_orange#wait_obstacle#straight_begin";
1078 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
1079 prog_char str_strat_conf2_arg2[] = "on#off";
1080 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
1081
1082
1083 prog_char help_strat_conf2[] = "configure strat options";
1084 parse_pgm_inst_t cmd_strat_conf2 = {
1085         .f = cmd_strat_conf2_parsed,  /* function to call */
1086         .data = NULL,      /* 2nd arg of func */
1087         .help_str = help_strat_conf2,
1088         .tokens = {        /* token list, NULL terminated */
1089                 (prog_void *)&cmd_strat_conf2_arg0,
1090                 (prog_void *)&cmd_strat_conf2_arg1,
1091                 (prog_void *)&cmd_strat_conf2_arg2,
1092                 NULL,
1093         },
1094 };
1095
1096 /**********************************************************/
1097 /* strat configuration */
1098
1099 /* this structure is filled when cmd_strat_conf3 is parsed successfully */
1100 struct cmd_strat_conf3_result {
1101         fixed_string_t arg0;
1102         fixed_string_t arg1;
1103         uint8_t arg2;
1104 };
1105
1106 /* function called when cmd_strat_conf3 is parsed successfully */
1107 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
1108 {
1109         struct cmd_strat_conf3_result *res = parsed_result;
1110
1111         if (!strcmp_P(res->arg1, PSTR("orphan_tomato"))) {
1112                 if (res->arg2 > 90)
1113                         res->arg2 = 90;
1114                 strat_conf.orphan_tomato = res->arg2;
1115         }
1116         else if (!strcmp_P(res->arg1, PSTR("opp_orange"))) {
1117                 if (res->arg2 > 90)
1118                         res->arg2 = 90;
1119                 strat_conf.opp_orange = res->arg2;
1120         }
1121         strat_conf.dump_enabled = 1;
1122         strat_conf_dump(__FUNCTION__);
1123 }
1124
1125 prog_char str_strat_conf3_arg0[] = "strat_conf";
1126 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
1127 prog_char str_strat_conf3_arg1[] = "orphan_tomato#opp_orange";
1128 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
1129 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT8);
1130
1131 prog_char help_strat_conf3[] = "configure strat options";
1132 parse_pgm_inst_t cmd_strat_conf3 = {
1133         .f = cmd_strat_conf3_parsed,  /* function to call */
1134         .data = NULL,      /* 2nd arg of func */
1135         .help_str = help_strat_conf3,
1136         .tokens = {        /* token list, NULL terminated */
1137                 (prog_void *)&cmd_strat_conf3_arg0,
1138                 (prog_void *)&cmd_strat_conf3_arg1,
1139                 (prog_void *)&cmd_strat_conf3_arg2,
1140                 NULL,
1141         },
1142 };
1143
1144
1145 /**********************************************************/
1146 /* Subtraj */
1147
1148
1149 /* function called when cmd_test is parsed successfully */
1150 static void subtraj_test(void)
1151 {
1152 }
1153
1154
1155 /* this structure is filled when cmd_subtraj is parsed successfully */
1156 struct cmd_subtraj_result {
1157         fixed_string_t arg0;
1158         fixed_string_t arg1;
1159         int32_t arg2;
1160         int32_t arg3;
1161         int32_t arg4;
1162         int32_t arg5;
1163 };
1164
1165 /* function called when cmd_subtraj is parsed successfully */
1166 static void cmd_subtraj_parsed(void *parsed_result, void *data)
1167 {
1168         struct cmd_subtraj_result *res = parsed_result;
1169
1170         if (!strcmp_P(res->arg1, PSTR("test")))
1171                 subtraj_test();
1172         else if (!strcmp_P(res->arg1, PSTR("orange_yellow")))
1173                 run_to_the_hills(I2C_COLOR_YELLOW);
1174         else if (!strcmp_P(res->arg1, PSTR("orange_blue")))
1175                 run_to_the_hills(I2C_COLOR_BLUE);
1176         else if (!strcmp_P(res->arg1, PSTR("tomato"))) {
1177                 position_set(&mainboard.pos, 2625,
1178                              COLOR_Y(1847), COLOR_A(0.00));
1179                 get_orphan_tomatoes();
1180         }
1181
1182         trajectory_hardstop(&mainboard.traj);
1183 }
1184
1185 prog_char str_subtraj_arg0[] = "subtraj";
1186 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
1187 prog_char str_subtraj_arg1[] = "test#orange_yellow#orange_blue#tomato";
1188 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
1189 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
1190 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
1191 parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32);
1192 parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32);
1193
1194 prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)";
1195 parse_pgm_inst_t cmd_subtraj = {
1196         .f = cmd_subtraj_parsed,  /* function to call */
1197         .data = NULL,      /* 2nd arg of func */
1198         .help_str = help_subtraj,
1199         .tokens = {        /* token list, NULL terminated */
1200                 (prog_void *)&cmd_subtraj_arg0,
1201                 (prog_void *)&cmd_subtraj_arg1,
1202                 (prog_void *)&cmd_subtraj_arg2,
1203                 (prog_void *)&cmd_subtraj_arg3,
1204                 (prog_void *)&cmd_subtraj_arg4,
1205                 (prog_void *)&cmd_subtraj_arg5,
1206                 NULL,
1207         },
1208 };
1209