extern parse_pgm_inst_t cmd_time_monitor;
extern parse_pgm_inst_t cmd_strat_event;
extern parse_pgm_inst_t cmd_test;
+extern parse_pgm_inst_t cmd_climb;
/* commands_traj.c */
extern parse_pgm_inst_t cmd_traj_speed;
(parse_pgm_inst_t *)&cmd_time_monitor,
(parse_pgm_inst_t *)&cmd_strat_event,
(parse_pgm_inst_t *)&cmd_test,
+ (parse_pgm_inst_t *)&cmd_climb,
/* commands_traj.c */
(parse_pgm_inst_t *)&cmd_traj_speed,
NULL,
},
};
+
+
+
+
+/**********************************************************/
+/* Climb */
+
+/* this structure is filled when cmd_climb is parsed successfully */
+struct cmd_climb_result {
+ fixed_string_t arg0;
+ int32_t radius;
+ int32_t dist;
+};
+
+
+//#define DIM_BALLS_DOWN 620
+//#define DIM_BALLS_UP 700
+
+#define DIM_BALLS_DOWN 530
+#define DIM_BALLS_UP 700
+
+/* function called when cmd_climb is parsed successfully */
+static void cmd_climb_parsed(void *parsed_result, void *data)
+{
+ uint8_t err;
+
+ printf_P(PSTR("starting Clifenger\r\n"));
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW );
+
+ trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ trajectory_a_abs(&mainboard.traj, 0);
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+
+
+ trajectory_goto_xy_abs(&mainboard.traj, 1250, 250);
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_DOWN, 0xFF);
+ printf_P(PSTR("ball_pack\r\n"));
+ support_balls_pack();
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_UP, 0xFF);
+ printf_P(PSTR("ball_deploy\r\n"));
+ support_balls_deploy();
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+ /* reach top, go down */
+ trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_UP, 0xFF);
+ printf_P(PSTR("ball_pack\r\n"));
+ support_balls_pack();
+ err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_DOWN, 0xFF);
+ printf_P(PSTR("ball_deploy\r\n"));
+ support_balls_deploy();
+
+ err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+}
+
+prog_char str_climb_arg0[] = "climb";
+parse_pgm_token_string_t cmd_climb_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_climb_result, arg0, str_climb_arg0);
+parse_pgm_token_num_t cmd_climb_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, radius, INT32);
+parse_pgm_token_num_t cmd_climb_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, dist, INT32);
+
+prog_char help_climb[] = "Climb function";
+parse_pgm_inst_t cmd_climb = {
+ .f = cmd_climb_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_climb,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_climb_arg0,
+ NULL,
+ },
+};