X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_traj.c;fp=projects%2Fmicrob2010%2Fmainboard%2Fcommands_traj.c;h=f3b38fd9c8f843bb05898c81152f3d542497ea3a;hp=0000000000000000000000000000000000000000;hb=5918edd6f4f713ef3c8b0b0020dd30a4fb8222ae;hpb=9d2d9100592e18fed985730298215884127fc568 diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c new file mode 100644 index 0000000..f3b38fd --- /dev/null +++ b/projects/microb2010/mainboard/commands_traj.c @@ -0,0 +1,1146 @@ +/* + * Copyright Droids Corporation (2009) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: commands_traj.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "main.h" +#include "cs.h" +#include "cmdline.h" +#include "strat_utils.h" +#include "strat_base.h" +#include "strat_avoid.h" +#include "strat.h" +#include "../common/i2c_commands.h" +#include "i2c_protocol.h" + +/**********************************************************/ +/* Traj_Speeds for trajectory_manager */ + +/* this structure is filled when cmd_traj_speed is parsed successfully */ +struct cmd_traj_speed_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t s; +}; + +/* function called when cmd_traj_speed is parsed successfully */ +static void cmd_traj_speed_parsed(void *parsed_result, void *data) +{ + struct cmd_traj_speed_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("angle"))) { + trajectory_set_speed(&mainboard.traj, mainboard.traj.d_speed, res->s); + } + else if (!strcmp_P(res->arg1, PSTR("distance"))) { + trajectory_set_speed(&mainboard.traj, res->s, mainboard.traj.a_speed); + } + /* else it is a "show" */ + + printf_P(PSTR("angle %u, distance %u\r\n"), + mainboard.traj.a_speed, + mainboard.traj.d_speed); +} + +prog_char str_traj_speed_arg0[] = "traj_speed"; +parse_pgm_token_string_t cmd_traj_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg0, str_traj_speed_arg0); +prog_char str_traj_speed_arg1[] = "angle#distance"; +parse_pgm_token_string_t cmd_traj_speed_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_arg1); +parse_pgm_token_num_t cmd_traj_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_traj_speed_result, s, UINT16); + +prog_char help_traj_speed[] = "Set traj_speed values for trajectory manager"; +parse_pgm_inst_t cmd_traj_speed = { + .f = cmd_traj_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_speed, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_arg1, + (prog_void *)&cmd_traj_speed_s, + NULL, + }, +}; + +/* show */ + +prog_char str_traj_speed_show_arg[] = "show"; +parse_pgm_token_string_t cmd_traj_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_traj_speed_result, arg1, str_traj_speed_show_arg); + +prog_char help_traj_speed_show[] = "Show traj_speed values for trajectory manager"; +parse_pgm_inst_t cmd_traj_speed_show = { + .f = cmd_traj_speed_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_traj_speed_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_traj_speed_arg0, + (prog_void *)&cmd_traj_speed_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* trajectory window configuration */ + +/* this structure is filled when cmd_trajectory is parsed successfully */ +struct cmd_trajectory_result { + fixed_string_t arg0; + fixed_string_t arg1; + float d_win; + float a_win; + float a_start; +}; + + +/* function called when cmd_trajectory is parsed successfully */ +static void cmd_trajectory_parsed(void * parsed_result, void * data) +{ + struct cmd_trajectory_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + trajectory_set_windows(&mainboard.traj, res->d_win, + res->a_win, res->a_start); + } + + printf_P(PSTR("trajectory %2.2f %2.2f %2.2f\r\n"), mainboard.traj.d_win, + DEG(mainboard.traj.a_win_rad), DEG(mainboard.traj.a_start_rad)); +} + +prog_char str_trajectory_arg0[] = "trajectory"; +parse_pgm_token_string_t cmd_trajectory_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg0, str_trajectory_arg0); +prog_char str_trajectory_arg1[] = "set"; +parse_pgm_token_string_t cmd_trajectory_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_arg1); +parse_pgm_token_num_t cmd_trajectory_d = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, d_win, FLOAT); +parse_pgm_token_num_t cmd_trajectory_a = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_win, FLOAT); +parse_pgm_token_num_t cmd_trajectory_as = TOKEN_NUM_INITIALIZER(struct cmd_trajectory_result, a_start, FLOAT); + +prog_char help_trajectory[] = "Set trajectory windows (distance, angle, angle_start)"; +parse_pgm_inst_t cmd_trajectory = { + .f = cmd_trajectory_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_trajectory, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_arg1, + (prog_void *)&cmd_trajectory_d, + (prog_void *)&cmd_trajectory_a, + (prog_void *)&cmd_trajectory_as, + NULL, + }, +}; + +/* show */ + +prog_char str_trajectory_show_arg[] = "show"; +parse_pgm_token_string_t cmd_trajectory_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_trajectory_result, arg1, str_trajectory_show_arg); + +prog_char help_trajectory_show[] = "Show trajectory window configuration"; +parse_pgm_inst_t cmd_trajectory_show = { + .f = cmd_trajectory_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_trajectory_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_trajectory_arg0, + (prog_void *)&cmd_trajectory_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* rs_gains configuration */ + +/* this structure is filled when cmd_rs_gains is parsed successfully */ +struct cmd_rs_gains_result { + fixed_string_t arg0; + fixed_string_t arg1; + float left; + float right; +}; + +/* function called when cmd_rs_gains is parsed successfully */ +static void cmd_rs_gains_parsed(void * parsed_result, void * data) +{ + struct cmd_rs_gains_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, + LEFT_ENCODER, res->left); // en augmentant on tourne à gauche + rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, + RIGHT_ENCODER, res->right); //en augmentant on tourne à droite + } + printf_P(PSTR("rs_gains set ")); + f64_print(mainboard.rs.left_ext_gain); + printf_P(PSTR(" ")); + f64_print(mainboard.rs.right_ext_gain); + printf_P(PSTR("\r\n")); +} + +prog_char str_rs_gains_arg0[] = "rs_gains"; +parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg0, str_rs_gains_arg0); +prog_char str_rs_gains_arg1[] = "set"; +parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_arg1); +parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, left, FLOAT); +parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct cmd_rs_gains_result, right, FLOAT); + +prog_char help_rs_gains[] = "Set rs_gains (left, right)"; +parse_pgm_inst_t cmd_rs_gains = { + .f = cmd_rs_gains_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_rs_gains, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_arg1, + (prog_void *)&cmd_rs_gains_l, + (prog_void *)&cmd_rs_gains_r, + NULL, + }, +}; + +/* show */ + +prog_char str_rs_gains_show_arg[] = "show"; +parse_pgm_token_string_t cmd_rs_gains_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1, str_rs_gains_show_arg); + +prog_char help_rs_gains_show[] = "Show rs_gains"; +parse_pgm_inst_t cmd_rs_gains_show = { + .f = cmd_rs_gains_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_rs_gains_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_rs_gains_arg0, + (prog_void *)&cmd_rs_gains_show_arg, + NULL, + }, +}; + +/**********************************************************/ +/* track configuration */ + +/* this structure is filled when cmd_track is parsed successfully */ +struct cmd_track_result { + fixed_string_t arg0; + fixed_string_t arg1; + float val; +}; + +/* function called when cmd_track is parsed successfully */ +static void cmd_track_parsed(void * parsed_result, void * data) +{ + struct cmd_track_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + position_set_physical_params(&mainboard.pos, res->val, DIST_IMP_MM); + } + printf_P(PSTR("track set %f\r\n"), mainboard.pos.phys.track_mm); +} + +prog_char str_track_arg0[] = "track"; +parse_pgm_token_string_t cmd_track_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg0, str_track_arg0); +prog_char str_track_arg1[] = "set"; +parse_pgm_token_string_t cmd_track_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_arg1); +parse_pgm_token_num_t cmd_track_val = TOKEN_NUM_INITIALIZER(struct cmd_track_result, val, FLOAT); + +prog_char help_track[] = "Set track in mm"; +parse_pgm_inst_t cmd_track = { + .f = cmd_track_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_track, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_arg1, + (prog_void *)&cmd_track_val, + NULL, + }, +}; + +/* show */ + +prog_char str_track_show_arg[] = "show"; +parse_pgm_token_string_t cmd_track_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_track_result, arg1, str_track_show_arg); + +prog_char help_track_show[] = "Show track"; +parse_pgm_inst_t cmd_track_show = { + .f = cmd_track_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_track_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_track_arg0, + (prog_void *)&cmd_track_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* Pt_Lists for testing traj */ + +#define PT_LIST_SIZE 10 +static struct xy_point pt_list[PT_LIST_SIZE]; +static uint16_t pt_list_len = 0; + +/* this structure is filled when cmd_pt_list is parsed successfully */ +struct cmd_pt_list_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint16_t arg2; + int16_t arg3; + int16_t arg4; +}; + +/* function called when cmd_pt_list is parsed successfully */ +static void cmd_pt_list_parsed(void * parsed_result, void * data) +{ + struct cmd_pt_list_result * res = parsed_result; + uint8_t i, why=0; + + if (!strcmp_P(res->arg1, PSTR("append"))) { + res->arg2 = pt_list_len; + } + + if (!strcmp_P(res->arg1, PSTR("insert")) || + !strcmp_P(res->arg1, PSTR("append"))) { + if (res->arg2 > pt_list_len) { + printf_P(PSTR("Index too large\r\n")); + return; + } + if (pt_list_len >= PT_LIST_SIZE) { + printf_P(PSTR("List is too large\r\n")); + return; + } + memmove(&pt_list[res->arg2+1], &pt_list[res->arg2], + PT_LIST_SIZE-1-res->arg2); + pt_list[res->arg2].x = res->arg3; + pt_list[res->arg2].y = res->arg4; + pt_list_len++; + } + else if (!strcmp_P(res->arg1, PSTR("del"))) { + if (pt_list_len <= 0) { + printf_P(PSTR("Error: list empty\r\n")); + return; + } + if (res->arg2 > pt_list_len) { + printf_P(PSTR("Index too large\r\n")); + return; + } + memmove(&pt_list[res->arg2], &pt_list[res->arg2+1], + (PT_LIST_SIZE-1-res->arg2)*sizeof(struct xy_point)); + pt_list_len--; + } + else if (!strcmp_P(res->arg1, PSTR("reset"))) { + pt_list_len = 0; + } + + /* else it is a "show" or a "start" */ + if (pt_list_len == 0) { + printf_P(PSTR("List empty\r\n")); + return; + } + for (i=0 ; iarg1, PSTR("start"))) { + trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y); + why = wait_traj_end(0xFF); /* all */ + } + else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { + while (1) { + why = goto_and_avoid(pt_list[i].x, pt_list[i].y, 0xFF, 0xFF); + printf("next point\r\n"); + if (why != END_OBSTACLE) + break; + } + } + if (why & (~(END_TRAJ | END_NEAR))) + trajectory_stop(&mainboard.traj); + } +} + +prog_char str_pt_list_arg0[] = "pt_list"; +parse_pgm_token_string_t cmd_pt_list_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg0, str_pt_list_arg0); +prog_char str_pt_list_arg1[] = "insert"; +parse_pgm_token_string_t cmd_pt_list_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1); +parse_pgm_token_num_t cmd_pt_list_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg2, UINT16); +parse_pgm_token_num_t cmd_pt_list_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg3, INT16); +parse_pgm_token_num_t cmd_pt_list_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_pt_list_result, arg4, INT16); + +prog_char help_pt_list[] = "Insert point in pt_list (idx,x,y)"; +parse_pgm_inst_t cmd_pt_list = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1, + (prog_void *)&cmd_pt_list_arg2, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, + NULL, + }, +}; + +/* append */ + +prog_char str_pt_list_arg1_append[] = "append"; +parse_pgm_token_string_t cmd_pt_list_arg1_append = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_arg1_append); + +prog_char help_pt_list_append[] = "Append point in pt_list (x,y)"; +parse_pgm_inst_t cmd_pt_list_append = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_append, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_arg1_append, + (prog_void *)&cmd_pt_list_arg3, + (prog_void *)&cmd_pt_list_arg4, + NULL, + }, +}; + +/* del */ + +prog_char str_pt_list_del_arg[] = "del"; +parse_pgm_token_string_t cmd_pt_list_del_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_del_arg); + +prog_char help_pt_list_del[] = "Del or insert point in pt_list (num)"; +parse_pgm_inst_t cmd_pt_list_del = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_del, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_del_arg, + (prog_void *)&cmd_pt_list_arg2, + NULL, + }, +}; +/* show */ + +prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start"; +parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg); + +prog_char help_pt_list_show[] = "Show, start or reset pt_list"; +parse_pgm_inst_t cmd_pt_list_show = { + .f = cmd_pt_list_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_pt_list_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_pt_list_arg0, + (prog_void *)&cmd_pt_list_show_arg, + NULL, + }, +}; + + + +/**********************************************************/ +/* Goto function */ + +/* this structure is filled when cmd_goto is parsed successfully */ +struct cmd_goto_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; +}; + +/* function called when cmd_goto is parsed successfully */ +static void cmd_goto_parsed(void * parsed_result, void * data) +{ + struct cmd_goto_result * res = parsed_result; + uint8_t err; + microseconds t1, t2; + + interrupt_traj_reset(); + if (!strcmp_P(res->arg1, PSTR("a_rel"))) { + trajectory_a_rel(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("d_rel"))) { + trajectory_d_rel(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("a_abs"))) { + trajectory_a_abs(&mainboard.traj, res->arg2); + } + else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) { + trajectory_turnto_xy(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("a_behind_xy"))) { + trajectory_turnto_xy_behind(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) { + trajectory_goto_xy_rel(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) { + trajectory_goto_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("avoid"))) { + err = goto_and_avoid_forward(res->arg2, res->arg3, 0xFF, 0xFF); + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + } + else if (!strcmp_P(res->arg1, PSTR("avoid_bw"))) { + err = goto_and_avoid_backward(res->arg2, res->arg3, 0xFF, 0xFF); + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs_fow"))) { + trajectory_goto_forward_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("xy_abs_back"))) { + trajectory_goto_backward_xy_abs(&mainboard.traj, res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("da_rel"))) { + trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3); + } + t1 = time_get_us2(); + while ((err = test_traj_end(0xFF)) == 0) { + t2 = time_get_us2(); + if (t2 - t1 > 200000) { + dump_cs_debug("angle", &mainboard.angle.cs); + dump_cs_debug("distance", &mainboard.distance.cs); + t1 = t2; + } + } + if (err != END_TRAJ && err != END_NEAR) + strat_hardstop(); + printf_P(PSTR("returned %s\r\n"), get_err(err)); +} + +prog_char str_goto_arg0[] = "goto"; +parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg0, str_goto_arg0); +prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs"; +parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_a); +parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg2, INT32); + +/* 1 params */ +prog_char help_goto1[] = "Change orientation of the mainboard"; +parse_pgm_inst_t cmd_goto1 = { + .f = cmd_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_goto1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_a, + (prog_void *)&cmd_goto_arg2, + NULL, + }, +}; + +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"; +parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_b); +parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg3, INT32); + +/* 2 params */ +prog_char help_goto2[] = "Go to a (x,y) or (d,a) position"; +parse_pgm_inst_t cmd_goto2 = { + .f = cmd_goto_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_goto2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_goto_arg0, + (prog_void *)&cmd_goto_arg1_b, + (prog_void *)&cmd_goto_arg2, + (prog_void *)&cmd_goto_arg3, + NULL, + }, +}; + +/**********************************************************/ +/* Position tests */ + +/* this structure is filled when cmd_position is parsed successfully */ +struct cmd_position_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; +}; + +#define AUTOPOS_SPEED_FAST 200 +static void auto_position(void) +{ + uint8_t err; + uint16_t old_spdd, old_spda; + + interrupt_traj_reset(); + strat_get_speed(&old_spdd, &old_spda); + strat_set_speed(AUTOPOS_SPEED_FAST, AUTOPOS_SPEED_FAST); + + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + if (err == END_INTR) + goto intr; + wait_ms(100); + strat_reset_pos(ROBOT_LENGTH/2, 0, 0); + + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + + trajectory_a_rel(&mainboard.traj, COLOR_A(90)); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + + trajectory_d_rel(&mainboard.traj, -300); + err = wait_traj_end(END_INTR|END_TRAJ|END_BLOCKING); + if (err == END_INTR) + goto intr; + wait_ms(100); + strat_reset_pos(DO_NOT_SET_POS, COLOR_Y(ROBOT_LENGTH/2), + COLOR_A(90)); + + trajectory_d_rel(&mainboard.traj, 120); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + wait_ms(100); + + trajectory_a_rel(&mainboard.traj, COLOR_A(-40)); + err = wait_traj_end(END_INTR|END_TRAJ); + if (err == END_INTR) + goto intr; + wait_ms(100); + + strat_set_speed(old_spdd, old_spda); + return; + +intr: + strat_hardstop(); + strat_set_speed(old_spdd, old_spda); +} + +/* function called when cmd_position is parsed successfully */ +static void cmd_position_parsed(void * parsed_result, void * data) +{ + struct cmd_position_result * res = parsed_result; + + /* display raw position values */ + if (!strcmp_P(res->arg1, PSTR("reset"))) { + position_set(&mainboard.pos, 0, 0, 0); + } + else if (!strcmp_P(res->arg1, PSTR("set"))) { + position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_green"))) { + mainboard.our_color = I2C_COLOR_GREEN; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_GREEN); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_GREEN); + auto_position(); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) { + mainboard.our_color = I2C_COLOR_RED; + i2c_set_color(I2C_MECHBOARD_ADDR, I2C_COLOR_RED); + i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_RED); + auto_position(); + } + + /* else it's just a "show" */ + printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), + position_get_x_double(&mainboard.pos), + position_get_y_double(&mainboard.pos), + DEG(position_get_a_rad_double(&mainboard.pos))); +} + +prog_char str_position_arg0[] = "position"; +parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0); +prog_char str_position_arg1[] = "show#reset#autoset_green#autoset_red"; +parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1); + +prog_char help_position[] = "Show/reset (x,y,a) position"; +parse_pgm_inst_t cmd_position = { + .f = cmd_position_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_position, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1, + NULL, + }, +}; + + +prog_char str_position_arg1_set[] = "set"; +parse_pgm_token_string_t cmd_position_arg1_set = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1_set); +parse_pgm_token_num_t cmd_position_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg2, INT32); +parse_pgm_token_num_t cmd_position_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg3, INT32); +parse_pgm_token_num_t cmd_position_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_position_result, arg4, INT32); + +prog_char help_position_set[] = "Set (x,y,a) position"; +parse_pgm_inst_t cmd_position_set = { + .f = cmd_position_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_position_set, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_position_arg0, + (prog_void *)&cmd_position_arg1_set, + (prog_void *)&cmd_position_arg2, + (prog_void *)&cmd_position_arg3, + (prog_void *)&cmd_position_arg4, + NULL, + }, +}; + + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_infos is parsed successfully */ +struct cmd_strat_infos_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_strat_infos is parsed successfully */ +static void cmd_strat_infos_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_infos_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + strat_reset_infos(); + } + strat_infos.dump_enabled = 1; + strat_dump_infos(__FUNCTION__); +} + +prog_char str_strat_infos_arg0[] = "strat_infos"; +parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0); +prog_char str_strat_infos_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1); + +prog_char help_strat_infos[] = "reset/show strat_infos"; +parse_pgm_inst_t cmd_strat_infos = { + .f = cmd_strat_infos_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_infos, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_infos_arg0, + (prog_void *)&cmd_strat_infos_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf is parsed successfully */ +struct cmd_strat_conf_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_strat_conf is parsed successfully */ +static void cmd_strat_conf_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("base"))) { + strat_infos.conf.flags = 0; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("big3"))) { + strat_infos.conf.flags = + STRAT_CONF_STORE_STATIC2 | + STRAT_CONF_BIG_3_TEMPLE; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("base_check"))) { + strat_infos.conf.flags = 0; + strat_infos.conf.scan_our_min_time = 35; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("big3_check"))) { + strat_infos.conf.flags = + STRAT_CONF_STORE_STATIC2 | + STRAT_CONF_BIG_3_TEMPLE; + strat_infos.conf.scan_our_min_time = 35; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("offensive_early"))) { + strat_infos.conf.flags = + STRAT_CONF_TAKE_ONE_LINTEL | + STRAT_CONF_STORE_STATIC2 | + STRAT_CONF_EARLY_SCAN | + STRAT_CONF_PUSH_OPP_COLS; + strat_infos.conf.scan_our_min_time = 50; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 15; + strat_infos.conf.delay_between_opp_scan = 90; + strat_infos.conf.wait_opponent = 5; + } + else if (!strcmp_P(res->arg1, PSTR("offensive_late"))) { + strat_infos.conf.flags = STRAT_CONF_TAKE_ONE_LINTEL; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 30; + strat_infos.conf.delay_between_opp_scan = 90; + } + else if (!strcmp_P(res->arg1, PSTR("one_on_disc"))) { + strat_infos.conf.flags = + STRAT_CONF_ONLY_ONE_ON_DISC; + strat_infos.conf.scan_our_min_time = 90; + strat_infos.conf.delay_between_our_scan = 90; + strat_infos.conf.scan_opp_min_time = 90; + strat_infos.conf.delay_between_opp_scan = 90; + } + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg0, str_strat_conf_arg0); +prog_char str_strat_conf_arg1[] = "show#base#big3#base_check#big3_check#offensive_early#offensive_late#one_on_disc"; +parse_pgm_token_string_t cmd_strat_conf_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf_result, arg1, str_strat_conf_arg1); + +prog_char help_strat_conf[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf = { + .f = cmd_strat_conf_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf_arg0, + (prog_void *)&cmd_strat_conf_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf2 is parsed successfully */ +struct cmd_strat_conf2_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_strat_conf2 is parsed successfully */ +static void cmd_strat_conf2_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf2_result *res = parsed_result; + uint8_t on, bit = 0; + + if (!strcmp_P(res->arg2, PSTR("on"))) + on = 1; + else + on = 0; + + if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc"))) + bit = STRAT_CONF_ONLY_ONE_ON_DISC; + else if (!strcmp_P(res->arg1, PSTR("bypass_static2"))) + bit = STRAT_CONF_BYPASS_STATIC2; + else if (!strcmp_P(res->arg1, PSTR("take_one_lintel"))) + bit = STRAT_CONF_TAKE_ONE_LINTEL; + else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail"))) + bit = STRAT_CONF_TAKE_ONE_LINTEL; + else if (!strcmp_P(res->arg1, PSTR("store_static2"))) + bit = STRAT_CONF_STORE_STATIC2; + else if (!strcmp_P(res->arg1, PSTR("big3_temple"))) + bit = STRAT_CONF_BIG_3_TEMPLE; + else if (!strcmp_P(res->arg1, PSTR("early_opp_scan"))) + bit = STRAT_CONF_EARLY_SCAN; + else if (!strcmp_P(res->arg1, PSTR("push_opp_cols"))) + bit = STRAT_CONF_PUSH_OPP_COLS; + + if (on) + strat_infos.conf.flags |= bit; + else + strat_infos.conf.flags &= (~bit); + + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf2_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0); +prog_char str_strat_conf2_arg1[] = "push_opp_cols#one_temple_on_disc#bypass_static2#take_one_lintel#skip_when_check_fail#store_static2#big3_temple#early_opp_scan"; +parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1); +prog_char str_strat_conf2_arg2[] = "on#off"; +parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2); + + +prog_char help_strat_conf2[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf2 = { + .f = cmd_strat_conf2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf2_arg0, + (prog_void *)&cmd_strat_conf2_arg1, + (prog_void *)&cmd_strat_conf2_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf3 is parsed successfully */ +struct cmd_strat_conf3_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t arg2; +}; + +/* function called when cmd_strat_conf3 is parsed successfully */ +static void cmd_strat_conf3_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf3_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.scan_opp_min_time = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.delay_between_opp_scan = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.scan_our_min_time = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) { + if (res->arg2 > 90) + res->arg2 = 90; + strat_infos.conf.delay_between_our_scan = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) { + strat_infos.conf.wait_opponent = res->arg2; + } + else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) { + strat_infos.conf.lintel_min_time = res->arg2; + } + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf3_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0); +prog_char str_strat_conf3_arg1[] = "lintel_min_time#scan_opponent_min_time#delay_between_opponent_scan#scan_our_min_time#delay_between_our_scan#wait_opponent"; +parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1); +parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16); + +prog_char help_strat_conf3[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf3 = { + .f = cmd_strat_conf3_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf3, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf3_arg0, + (prog_void *)&cmd_strat_conf3_arg1, + (prog_void *)&cmd_strat_conf3_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* strat configuration */ + +/* this structure is filled when cmd_strat_conf4 is parsed successfully */ +struct cmd_strat_conf4_result { + fixed_string_t arg0; + fixed_string_t arg1; + int16_t arg2; +}; + +/* function called when cmd_strat_conf4 is parsed successfully */ +static void cmd_strat_conf4_parsed(void *parsed_result, void *data) +{ + struct cmd_strat_conf4_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("scan_opponent_angle"))) { + strat_infos.conf.scan_opp_angle = res->arg2; + } + strat_infos.dump_enabled = 1; + strat_dump_conf(); +} + +prog_char str_strat_conf4_arg0[] = "strat_conf"; +parse_pgm_token_string_t cmd_strat_conf4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg0, str_strat_conf4_arg0); +prog_char str_strat_conf4_arg1[] = "scan_opponent_angle"; +parse_pgm_token_string_t cmd_strat_conf4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf4_result, arg1, str_strat_conf4_arg1); +parse_pgm_token_num_t cmd_strat_conf4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf4_result, arg2, UINT16); + +prog_char help_strat_conf4[] = "configure strat options"; +parse_pgm_inst_t cmd_strat_conf4 = { + .f = cmd_strat_conf4_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_conf4, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_conf4_arg0, + (prog_void *)&cmd_strat_conf4_arg1, + (prog_void *)&cmd_strat_conf4_arg2, + NULL, + }, +}; + + +/**********************************************************/ +/* Subtraj */ + +/* this structure is filled when cmd_subtraj is parsed successfully */ +struct cmd_subtraj_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; + int32_t arg4; + int32_t arg5; +}; + +/* function called when cmd_subtraj is parsed successfully */ +static void cmd_subtraj_parsed(void *parsed_result, void *data) +{ + struct cmd_subtraj_result *res = parsed_result; + uint8_t err = 0; + struct column_dispenser *disp; + + if (strcmp_P(res->arg1, PSTR("static")) == 0) { + err = strat_static_columns(res->arg2); + } + else if (strcmp_P(res->arg1, PSTR("static2")) == 0) { + strat_infos.s_cols.configuration = res->arg2; + switch (res->arg2) { + case 1: + position_set(&mainboard.pos, 1398, + COLOR_Y(1297), COLOR_A(-66)); + break; + case 2: + position_set(&mainboard.pos, 1232, + COLOR_Y(1051), COLOR_A(4)); + break; + case 3: + position_set(&mainboard.pos, 1232, + COLOR_Y(1043), COLOR_A(5)); + break; + case 4: + position_set(&mainboard.pos, 1346, + COLOR_Y(852), COLOR_A(57)); + break; + default: + return; + } + if (res->arg2 == 1 && res->arg3 == 1) { + strat_infos.s_cols.flags = STATIC_COL_LINE1_DONE; + } + if (res->arg2 == 1 && res->arg3 == 2) { + strat_infos.s_cols.flags = STATIC_COL_LINE2_DONE; + } + err = strat_static_columns_pass2(); + } + else if (strcmp_P(res->arg1, PSTR("lintel1")) == 0) { + err = strat_goto_lintel_disp(&strat_infos.l1); + } + else if (strcmp_P(res->arg1, PSTR("lintel2")) == 0) { + err = strat_goto_lintel_disp(&strat_infos.l2); + } + else if (strcmp_P(res->arg1, PSTR("coldisp1")) == 0) { + disp = &strat_infos.c1; + err = strat_goto_col_disp(&disp); + } + else if (strcmp_P(res->arg1, PSTR("coldisp2")) == 0) { + disp = &strat_infos.c2; + err = strat_goto_col_disp(&disp); + } + else if (strcmp_P(res->arg1, PSTR("coldisp3")) == 0) { + disp = &strat_infos.c3; + err = strat_goto_col_disp(&disp); + } + else if (strcmp_P(res->arg1, PSTR("disc")) == 0) { + if (res->arg2 == 0) { + printf_P(PSTR("bad level\r\n")); + return; + } + err = strat_goto_disc(res->arg2); + } + + printf_P(PSTR("substrat returned %s\r\n"), get_err(err)); + trajectory_hardstop(&mainboard.traj); +} + +prog_char str_subtraj_arg0[] = "subtraj"; +parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0); +prog_char str_subtraj_arg1[] = "static#disc#lintel1#lintel2#coldisp1#coldisp2#coldisp3#static2"; +parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1); +parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32); +parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32); +parse_pgm_token_num_t cmd_subtraj_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg4, INT32); +parse_pgm_token_num_t cmd_subtraj_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg5, INT32); + +prog_char help_subtraj[] = "Test sub-trajectories (a,b,c,d: specific params)"; +parse_pgm_inst_t cmd_subtraj = { + .f = cmd_subtraj_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_subtraj, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_subtraj_arg0, + (prog_void *)&cmd_subtraj_arg1, + (prog_void *)&cmd_subtraj_arg2, + (prog_void *)&cmd_subtraj_arg3, + (prog_void *)&cmd_subtraj_arg4, + (prog_void *)&cmd_subtraj_arg5, + NULL, + }, +};