projects/microb2010/tests/test_board2008/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.6 2009-05-02 10:08:09 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25
  26 #include <aversive/pgmspace.h>
  27 #include <aversive/wait.h>
  28 #include <aversive/error.h>
  29
  30 #include <uart.h>
  31 #include <pwm_ng.h>
  32 #include <clock_time.h>
  33 #include <encoders_microb.h>
  34
  35 #include <pid.h>
  36 #include <quadramp.h>
  37 #include <control_system_manager.h>
  38 #include <trajectory_manager.h>
  39 #include <blocking_detection_manager.h>
  40 #include <robot_system.h>
  41 #include <position_manager.h>
  42
  43 #include <rdline.h>
  44 #include <parse.h>
  45 #include <parse_string.h>
  46 #include <parse_num.h>
  47
  48 #include "main.h"
  49 #include "strat_base.h"
  50 #include "strat_utils.h"
  51 #include "cs.h"
  52 #include "cmdline.h"
  53
  54 /**********************************************************/
  55 /* Traj_Speeds for trajectory_manager */
  56
  57 /* this structure is filled when cmd_traj_speed is parsed successfully */
  58 struct cmd_traj_speed_result {
  59         fixed_string_t arg0;
  60         fixed_string_t arg1;
  61         uint16_t s;
  62 };
  63
  64 /* function called when cmd_traj_speed is parsed successfully */
  65 static void cmd_traj_speed_parsed(void *parsed_result, void *data)
  66 {
  67         struct cmd_traj_speed_result * res = parsed_result;
  68
  69         if (!strcmp_P(res->arg1, PSTR("angle"))) {
  70                 trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s);
  71         }
  72         else if (!strcmp_P(res->arg1, PSTR("distance"))) {
  73                 trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed);
  74         }
  75         /* else it is a "show" */
  76
  77         printf_P(PSTR("angle %u, distance %u\r\n"),
  78                  mainboard.traj.a_speed,
  79                  mainboard.traj.d_speed);
  80 }
  81
  82 prog_char str_traj_speed_arg0[] = "traj_speed";
  83 parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0);
  84 prog_char str_traj_speed_arg1[] = "angle#distance";
  85 parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1);
  86 parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16);
  87
  88 prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager";
  89 parse_pgm_inst_t cmd_traj_speed = {
  90         .f = cmd_traj_speed_parsed,  /* function to call */
  91         .data = NULL,      /* 2nd arg of func */
  92         .help_str = help_traj_speed,
  93         .tokens = {        /* token list, NULL terminated */
  94                 (prog_void *)&cmd_traj_speed_arg0,
  95                 (prog_void *)&cmd_traj_speed_arg1,
  96                 (prog_void *)&cmd_traj_speed_s,
  97                 NULL,
  98         },
  99 };
 100
 101 /* show */
 102
 103 prog_char str_traj_speed_show_arg[] = "show";
 104 parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg);
 105
 106 prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager";
 107 parse_pgm_inst_t cmd_traj_speed_show = {
 108         .f = cmd_traj_speed_parsed,  /* function to call */
 109         .data = NULL,      /* 2nd arg of func */
 110         .help_str = help_traj_speed_show,
 111         .tokens = {        /* token list, NULL terminated */
 112                 (prog_void *)&cmd_traj_speed_arg0,
 113                 (prog_void *)&cmd_traj_speed_show_arg,
 114                 NULL,
 115         },
 116 };
 117
 118 /**********************************************************/
 119 /* trajectory window configuration */
 120
 121 /* this structure is filled when cmd_trajectory is parsed successfully */
 122 struct cmd_trajectory_result {
 123         fixed_string_t arg0;
 124         fixed_string_t arg1;
 125         float d_win;
 126         float a_win;
 127         float a_start;
 128 };
 129
 130
 131 /* function called when cmd_trajectory is parsed successfully */
 132 static void cmd_trajectory_parsed(void * parsed_result, void * data)
 133 {
 134         struct cmd_trajectory_result * res = parsed_result;
 135
 136         if (!strcmp_P(res->arg1, PSTR("set"))) {
 137                 trajectory_set_windows(&mainboard.traj, res->d_win,
 138                                        res->a_win, res->a_start);
 139         }
 140
 141         printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win,
 142                  DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad));
 143 }
 144
 145 prog_char str_trajectory_arg0[] = "trajectory";
 146 parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0);
 147 prog_char str_trajectory_arg1[] = "set";
 148 parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1);
 149 parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT);
 150 parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT);
 151 parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT);
 152
 153 prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)";
 154 parse_pgm_inst_t cmd_trajectory = {
 155         .f = cmd_trajectory_parsed,  /* function to call */
 156         .data = NULL,      /* 2nd arg of func */
 157         .help_str = help_trajectory,
 158         .tokens = {        /* token list, NULL terminated */
 159                 (prog_void *)&cmd_trajectory_arg0,
 160                 (prog_void *)&cmd_trajectory_arg1,
 161                 (prog_void *)&cmd_trajectory_d,
 162                 (prog_void *)&cmd_trajectory_a,
 163                 (prog_void *)&cmd_trajectory_as,
 164                 NULL,
 165         },
 166 };
 167
 168 /* show */
 169
 170 prog_char str_trajectory_show_arg[] = "show";
 171 parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg);
 172
 173 prog_char help_trajectory_show[] = "Show trajectory window configuration";
 174 parse_pgm_inst_t cmd_trajectory_show = {
 175         .f = cmd_trajectory_parsed,  /* function to call */
 176         .data = NULL,      /* 2nd arg of func */
 177         .help_str = help_trajectory_show,
 178         .tokens = {        /* token list, NULL terminated */
 179                 (prog_void *)&cmd_trajectory_arg0,
 180                 (prog_void *)&cmd_trajectory_show_arg,
 181                 NULL,
 182         },
 183 };
 184
 185 /**********************************************************/
 186 /* rs_gains configuration */
 187
 188 /* this structure is filled when cmd_rs_gains is parsed successfully */
 189 struct cmd_rs_gains_result {
 190         fixed_string_t arg0;
 191         fixed_string_t arg1;
 192         float left;
 193         float right;
 194 };
 195
 196 /* function called when cmd_rs_gains is parsed successfully */
 197 static void cmd_rs_gains_parsed(void * parsed_result, void * data)
 198 {
 199         struct cmd_rs_gains_result * res = parsed_result;
 200
 201         if (!strcmp_P(res->arg1, PSTR("set"))) {
 202                 rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value,
 203                                         LEFT_ENCODER, res->left); // en augmentant on tourne à gauche
 204                 rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value,
 205                                          RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
 206         }
 207         printf_P(PSTR("rs_gains set %f %f"),
 208                  mainboard.rs.left_ext_gain,
 209                  mainboard.rs.right_ext_gain);
 210 /*      f64_print(mainboard.rs.left_ext_gain); */
 211 /*      printf_P(PSTR(" ")); */
 212 /*      f64_print(mainboard.rs.right_ext_gain); */
 213         printf_P(PSTR("\r\n"));
 214 }
 215
 216 prog_char str_rs_gains_arg0[] = "rs_gains";
 217 parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0);
 218 prog_char str_rs_gains_arg1[] = "set";
 219 parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1);
 220 parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT);
 221 parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT);
 222
 223 prog_char help_rs_gains[] = "Set rs_gains (left, right)";
 224 parse_pgm_inst_t cmd_rs_gains = {
 225         .f = cmd_rs_gains_parsed,  /* function to call */
 226         .data = NULL,      /* 2nd arg of func */
 227         .help_str = help_rs_gains,
 228         .tokens = {        /* token list, NULL terminated */
 229                 (prog_void *)&cmd_rs_gains_arg0,
 230                 (prog_void *)&cmd_rs_gains_arg1,
 231                 (prog_void *)&cmd_rs_gains_l,
 232                 (prog_void *)&cmd_rs_gains_r,
 233                 NULL,
 234         },
 235 };
 236
 237 /* show */
 238
 239 prog_char str_rs_gains_show_arg[] = "show";
 240 parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg);
 241
 242 prog_char help_rs_gains_show[] = "Show rs_gains";
 243 parse_pgm_inst_t cmd_rs_gains_show = {
 244         .f = cmd_rs_gains_parsed,  /* function to call */
 245         .data = NULL,      /* 2nd arg of func */
 246         .help_str = help_rs_gains_show,
 247         .tokens = {        /* token list, NULL terminated */
 248                 (prog_void *)&cmd_rs_gains_arg0,
 249                 (prog_void *)&cmd_rs_gains_show_arg,
 250                 NULL,
 251         },
 252 };
 253
 254 /**********************************************************/
 255 /* track configuration */
 256
 257 /* this structure is filled when cmd_track is parsed successfully */
 258 struct cmd_track_result {
 259         fixed_string_t arg0;
 260         fixed_string_t arg1;
 261         float val;
 262 };
 263
 264 /* function called when cmd_track is parsed successfully */
 265 static void cmd_track_parsed(void * parsed_result, void * data)
 266 {
 267         struct cmd_track_result * res = parsed_result;
 268
 269         if (!strcmp_P(res->arg1, PSTR("set"))) {
 270                 position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM);
 271         }
 272         printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm);
 273 }
 274
 275 prog_char str_track_arg0[] = "track";
 276 parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0);
 277 prog_char str_track_arg1[] = "set";
 278 parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1);
 279 parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT);
 280
 281 prog_char help_track[] = "Set track in mm";
 282 parse_pgm_inst_t cmd_track = {
 283         .f = cmd_track_parsed,  /* function to call */
 284         .data = NULL,      /* 2nd arg of func */
 285         .help_str = help_track,
 286         .tokens = {        /* token list, NULL terminated */
 287                 (prog_void *)&cmd_track_arg0,
 288                 (prog_void *)&cmd_track_arg1,
 289                 (prog_void *)&cmd_track_val,
 290                 NULL,
 291         },
 292 };
 293
 294 /* show */
 295
 296 prog_char str_track_show_arg[] = "show";
 297 parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg);
 298
 299 prog_char help_track_show[] = "Show track";
 300 parse_pgm_inst_t cmd_track_show = {
 301         .f = cmd_track_parsed,  /* function to call */
 302         .data = NULL,      /* 2nd arg of func */
 303         .help_str = help_track_show,
 304         .tokens = {        /* token list, NULL terminated */
 305                 (prog_void *)&cmd_track_arg0,
 306                 (prog_void *)&cmd_track_show_arg,
 307                 NULL,
 308         },
 309 };
 310
 311
 312
 313 /**********************************************************/
 314 /* Pt_Lists for testing traj */
 315
 316 #define PT_LIST_SIZE 10
 317 static struct xy_point pt_list[PT_LIST_SIZE];
 318 static uint16_t pt_list_len = 0;
 319
 320 /* this structure is filled when cmd_pt_list is parsed successfully */
 321 struct cmd_pt_list_result {
 322         fixed_string_t arg0;
 323         fixed_string_t arg1;
 324         uint16_t arg2;
 325         int16_t arg3;
 326         int16_t arg4;
 327 };
 328
 329 /* function called when cmd_pt_list is parsed successfully */
 330 static void cmd_pt_list_parsed(void * parsed_result, void * data)
 331 {
 332         struct cmd_pt_list_result * res = parsed_result;
 333         uint8_t i, why=0;
 334
 335         if (!strcmp_P(res->arg1, PSTR("append"))) {
 336                 res->arg2 = pt_list_len;
 337         }
 338
 339         if (!strcmp_P(res->arg1, PSTR("insert")) ||
 340             !strcmp_P(res->arg1, PSTR("append"))) {
 341                 if (res->arg2 > pt_list_len) {
 342                         printf_P(PSTR("Index too large\r\n"));
 343                         return;
 344                 }
 345                 if (pt_list_len >= PT_LIST_SIZE) {
 346                         printf_P(PSTR("List is too large\r\n"));
 347                         return;
 348                 }
 349                 memmove(&pt_list[res->arg2+1], &pt_list[res->arg2],
 350                        PT_LIST_SIZE-1-res->arg2);
 351                 pt_list[res->arg2].x = res->arg3;
 352                 pt_list[res->arg2].y = res->arg4;
 353                 pt_list_len++;
 354         }
 355         else if (!strcmp_P(res->arg1, PSTR("del"))) {
 356                 if (pt_list_len <= 0) {
 357                         printf_P(PSTR("Error: list empty\r\n"));
 358                         return;
 359                 }
 360                 if (res->arg2 > pt_list_len) {
 361                         printf_P(PSTR("Index too large\r\n"));
 362                         return;
 363                 }
 364                 memmove(&pt_list[res->arg2], &pt_list[res->arg2+1],
 365                        (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point));
 366                 pt_list_len--;
 367         }
 368         else if (!strcmp_P(res->arg1, PSTR("reset"))) {
 369                 pt_list_len = 0;
 370         }
 371
 372         /* else it is a "show" or a "start" */
 373         if (pt_list_len == 0) {
 374                 printf_P(PSTR("List empty\r\n"));
 375                 return;
 376         }
 377         for (i=0 ; i<pt_list_len ; i++) {
 378                 printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
 379                 if (!strcmp_P(res->arg1, PSTR("start"))) {
 380                         trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
 381                         why = wait_traj_end(0xFF); /* all */
 382                 }
 383                 else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
 384                         while (1) {
 385                                 //why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF);
 386                                 printf("next point\r\n");
 387                                 if (why != END_OBSTACLE)
 388                                         break;
 389                         }
 390                 }
 391                 if (why & (~(END_TRAJ | END_NEAR)))
 392                         trajectory_stop(&mainboard.traj);
 393         }
 394 }
 395
 396 prog_char str_pt_list_arg0[] = "pt_list";
 397 parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0);
 398 prog_char str_pt_list_arg1[] = "insert";
 399 parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1);
 400 parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16);
 401 parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16);
 402 parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16);
 403
 404 prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)";
 405 parse_pgm_inst_t cmd_pt_list = {
 406         .f = cmd_pt_list_parsed,  /* function to call */
 407         .data = NULL,      /* 2nd arg of func */
 408         .help_str = help_pt_list,
 409         .tokens = {        /* token list, NULL terminated */
 410                 (prog_void *)&cmd_pt_list_arg0,
 411                 (prog_void *)&cmd_pt_list_arg1,
 412                 (prog_void *)&cmd_pt_list_arg2,
 413                 (prog_void *)&cmd_pt_list_arg3,
 414                 (prog_void *)&cmd_pt_list_arg4,
 415                 NULL,
 416         },
 417 };
 418
 419 /* append */
 420
 421 prog_char str_pt_list_arg1_append[] = "append";
 422 parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append);
 423
 424 prog_char help_pt_list_append[] = "Append point in pt_list (x,y)";
 425 parse_pgm_inst_t cmd_pt_list_append = {
 426         .f = cmd_pt_list_parsed,  /* function to call */
 427         .data = NULL,      /* 2nd arg of func */
 428         .help_str = help_pt_list_append,
 429         .tokens = {        /* token list, NULL terminated */
 430                 (prog_void *)&cmd_pt_list_arg0,
 431                 (prog_void *)&cmd_pt_list_arg1_append,
 432                 (prog_void *)&cmd_pt_list_arg3,
 433                 (prog_void *)&cmd_pt_list_arg4,
 434                 NULL,
 435         },
 436 };
 437
 438 /* del */
 439
 440 prog_char str_pt_list_del_arg[] = "del";
 441 parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg);
 442
 443 prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)";
 444 parse_pgm_inst_t cmd_pt_list_del = {
 445         .f = cmd_pt_list_parsed,  /* function to call */
 446         .data = NULL,      /* 2nd arg of func */
 447         .help_str = help_pt_list_del,
 448         .tokens = {        /* token list, NULL terminated */
 449                 (prog_void *)&cmd_pt_list_arg0,
 450                 (prog_void *)&cmd_pt_list_del_arg,
 451                 (prog_void *)&cmd_pt_list_arg2,
 452                 NULL,
 453         },
 454 };
 455 /* show */
 456
 457 prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
 458 parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
 459
 460 prog_char help_pt_list_show[] = "Show, start or reset pt_list";
 461 parse_pgm_inst_t cmd_pt_list_show = {
 462         .f = cmd_pt_list_parsed,  /* function to call */
 463         .data = NULL,      /* 2nd arg of func */
 464         .help_str = help_pt_list_show,
 465         .tokens = {        /* token list, NULL terminated */
 466                 (prog_void *)&cmd_pt_list_arg0,
 467                 (prog_void *)&cmd_pt_list_show_arg,
 468                 NULL,
 469         },
 470 };
 471
 472
 473
 474 /**********************************************************/
 475 /* Goto function */
 476
 477 /* this structure is filled when cmd_goto is parsed successfully */
 478 struct cmd_goto_result {
 479         fixed_string_t arg0;
 480         fixed_string_t arg1;
 481         int32_t arg2;
 482         int32_t arg3;
 483         int32_t arg4;
 484 };
 485
 486 /* function called when cmd_goto is parsed successfully */
 487 static void cmd_goto_parsed(void * parsed_result, void * data)
 488 {
 489         struct cmd_goto_result * res = parsed_result;
 490         uint8_t err;
 491         microseconds t1, t2;
 492
 493         interrupt_traj_reset();
 494         if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
 495                 trajectory_a_rel(&mainboard.traj, res->arg2);
 496         }
 497         else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
 498                 trajectory_d_rel(&mainboard.traj, res->arg2);
 499         }
 500         else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
 501                 trajectory_a_abs(&mainboard.traj, res->arg2);
 502         }
 503         else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
 504                 trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3);
 505         }
 506         else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) {
 507                 trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3);
 508         }
 509         else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
 510                 trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3);
 511         }
 512         else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
 513                 trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 514         }
 515         else if (!strcmp_P(res->arg1, PSTR("avoid"))) {
 516                 //err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF);
 517                 err=0;
 518                 if (err != END_TRAJ && err != END_NEAR)
 519                         strat_hardstop();
 520         }
 521         else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) {
 522                 //err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF);
 523                 err=0;
 524                 if (err != END_TRAJ && err != END_NEAR)
 525                         strat_hardstop();
 526         }
 527         else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) {
 528                 trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 529         }
 530         else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) {
 531                 trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3);
 532         }
 533         else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
 534                 trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
 535         }
 536         t1 = time_get_us2();
 537         while ((err = test_traj_end(0xFF)) == 0) {
 538                 t2 = time_get_us2();
 539                 if (t2 - t1 > 200000) {
 540                         dump_cs_debug("angle", &mainboard.angle.cs);
 541                         dump_cs_debug("distance", &mainboard.distance.cs);
 542                         t1 = t2;
 543                 }
 544         }
 545         if (err != END_TRAJ && err != END_NEAR)
 546                 strat_hardstop();
 547         printf_P(PSTR("returned %s\r\n"), get_err(err));
 548 }
 549
 550 prog_char str_goto_arg0[] = "goto";
 551 parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0);
 552 prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
 553 parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a);
 554 parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32);
 555
 556 /* 1 params */
 557 prog_char help_goto1[] = "Change orientation of the mainboard";
 558 parse_pgm_inst_t cmd_goto1 = {
 559         .f = cmd_goto_parsed,  /* function to call */
 560         .data = NULL,      /* 2nd arg of func */
 561         .help_str = help_goto1,
 562         .tokens = {        /* token list, NULL terminated */
 563                 (prog_void *)&cmd_goto_arg0,
 564                 (prog_void *)&cmd_goto_arg1_a,
 565                 (prog_void *)&cmd_goto_arg2,
 566                 NULL,
 567         },
 568 };
 569
 570 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";
 571 parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b);
 572 parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32);
 573
 574 /* 2 params */
 575 prog_char help_goto2[] = "Go to a (x,y) or (d,a) position";
 576 parse_pgm_inst_t cmd_goto2 = {
 577         .f = cmd_goto_parsed,  /* function to call */
 578         .data = NULL,      /* 2nd arg of func */
 579         .help_str = help_goto2,
 580         .tokens = {        /* token list, NULL terminated */
 581                 (prog_void *)&cmd_goto_arg0,
 582                 (prog_void *)&cmd_goto_arg1_b,
 583                 (prog_void *)&cmd_goto_arg2,
 584                 (prog_void *)&cmd_goto_arg3,
 585                 NULL,
 586         },
 587 };
 588
 589 /**********************************************************/
 590 /* Position tests */
 591
 592 /* this structure is filled when cmd_position is parsed successfully */
 593 struct cmd_position_result {
 594         fixed_string_t arg0;
 595         fixed_string_t arg1;
 596         int32_t arg2;
 597         int32_t arg3;
 598         int32_t arg4;
 599 };
 600
 601 #define AUTOPOS_SPEED_FAST 200
 602 static void auto_position(void)
 603 {
 604         uint8_t err;
 605         uint16_t old_spdd, old_spda;
 606
 607         interrupt_traj_reset();
 608         strat_get_speed(&old_spdd, &old_spda);
 609         strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST);
 610
 611         trajectory_d_rel(&mainboard.traj, -300);
 612         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
 613         if (err == END_INTR)
 614                 goto intr;
 615         wait_ms(100);
 616         strat_reset_pos(ROBOT_LENGTH/2, 0, 0);
 617
 618         trajectory_d_rel(&mainboard.traj, 120);
 619         err = wait_traj_end(END_INTR|END_TRAJ);
 620         if (err == END_INTR)
 621                 goto intr;
 622
 623         trajectory_a_rel(&mainboard.traj, 90);
 624         err = wait_traj_end(END_INTR|END_TRAJ);
 625         if (err == END_INTR)
 626                 goto intr;
 627
 628         trajectory_d_rel(&mainboard.traj, -300);
 629         err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING);
 630         if (err == END_INTR)
 631                 goto intr;
 632         wait_ms(100);
 633         strat_reset_pos(DO_NOT_SET_POS, ROBOT_LENGTH/2,
 634                         90);
 635
 636         trajectory_d_rel(&mainboard.traj, 120);
 637         err = wait_traj_end(END_INTR|END_TRAJ);
 638         if (err == END_INTR)
 639                 goto intr;
 640         wait_ms(100);
 641
 642         trajectory_a_rel(&mainboard.traj, -40);
 643         err = wait_traj_end(END_INTR|END_TRAJ);
 644         if (err == END_INTR)
 645                 goto intr;
 646         wait_ms(100);
 647
 648         strat_set_speed(old_spdd, old_spda);
 649         return;
 650
 651 intr:
 652         strat_hardstop();
 653         strat_set_speed(old_spdd, old_spda);
 654 }
 655
 656 /* function called when cmd_position is parsed successfully */
 657 static void cmd_position_parsed(void * parsed_result, void * data)
 658 {
 659         struct cmd_position_result * res = parsed_result;
 660
 661         /* display raw position values */
 662         if (!strcmp_P(res->arg1, PSTR("reset"))) {
 663                 position_set(&mainboard.pos, 0, 0, 0);
 664         }
 665         else if (!strcmp_P(res->arg1, PSTR("set"))) {
 666                 position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
 667         }
 668         else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
 669                 auto_position();
 670         }
 671         else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
 672                 auto_position();
 673         }
 674
 675         /* else it's just a "show" */
 676         printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
 677                  position_get_x_double(&mainboard.pos),
 678                  position_get_y_double(&mainboard.pos),
 679                  DEG(position_get_a_rad_double(&mainboard.pos)));
 680 }
 681
 682 prog_char str_position_arg0[] = "position";
 683 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
 684 prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_red";
 685 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
 686
 687 prog_char help_position[] = "Show/reset (x,y,a) position";
 688 parse_pgm_inst_t cmd_position = {
 689         .f = cmd_position_parsed,  /* function to call */
 690         .data = NULL,      /* 2nd arg of func */
 691         .help_str = help_position,
 692         .tokens = {        /* token list, NULL terminated */
 693                 (prog_void *)&cmd_position_arg0,
 694                 (prog_void *)&cmd_position_arg1,
 695                 NULL,
 696         },
 697 };
 698
 699
 700 prog_char str_position_arg1_set[] = "set";
 701 parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set);
 702 parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32);
 703 parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32);
 704 parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32);
 705
 706 prog_char help_position_set[] = "Set (x,y,a) position";
 707 parse_pgm_inst_t cmd_position_set = {
 708         .f = cmd_position_parsed,  /* function to call */
 709         .data = NULL,      /* 2nd arg of func */
 710         .help_str = help_position_set,
 711         .tokens = {        /* token list, NULL terminated */
 712                 (prog_void *)&cmd_position_arg0,
 713                 (prog_void *)&cmd_position_arg1_set,
 714                 (prog_void *)&cmd_position_arg2,
 715                 (prog_void *)&cmd_position_arg3,
 716                 (prog_void *)&cmd_position_arg4,
 717                 NULL,
 718         },
 719 };
 720