#include <stdio.h>
#include <string.h>
+#include <hostsim.h>
#include <aversive/pgmspace.h>
#include <aversive/wait.h>
#include <aversive/error.h>
#include "../common/eeprom_mapping.h"
#include "main.h"
+#include "robotsim.h"
#include "sensor.h"
#include "cmdline.h"
#include "strat.h"
mainboard.flags |= bit;
else if (!strcmp_P(res->arg2, PSTR("off"))) {
if (!strcmp_P(res->arg1, PSTR("cs"))) {
+#ifdef HOST_VERSION
+ robotsim_pwm(LEFT_PWM, 0);
+ robotsim_pwm(RIGHT_PWM, 0);
+#else
pwm_ng_set(LEFT_PWM, 0);
pwm_ng_set(RIGHT_PWM, 0);
+#endif
}
mainboard.flags &= (~bit);
}
/* function called when cmd_spi_test is parsed successfully */
static void cmd_spi_test_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
uint16_t i = 0, ret = 0, ret2 = 0;
if (mainboard.flags & DO_ENCODERS) {
i++;
} while(!cmdline_keypressed());
+#endif
}
prog_char str_spi_test_arg0[] = "spi_test";
/* function called when cmd_start is parsed successfully */
static void cmd_start_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_start_result *res = parsed_result;
uint8_t old_level = gen.log_level;
gen.log_level = 0;
}
- if (!strcmp_P(res->color, PSTR("red"))) {
- mainboard.our_color = I2C_COLOR_RED;
- i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED);
- i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_RED);
+ if (!strcmp_P(res->color, PSTR("yellow"))) {
+ mainboard.our_color = I2C_COLOR_YELLOW;
+ i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
+ i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
}
- else if (!strcmp_P(res->color, PSTR("green"))) {
- mainboard.our_color = I2C_COLOR_GREEN;
- i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN);
- i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_GREEN);
+ else if (!strcmp_P(res->color, PSTR("blue"))) {
+ mainboard.our_color = I2C_COLOR_BLUE;
+ i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
+ i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
}
strat_start();
gen.logs[NB_LOGS] = 0;
gen.log_level = old_level;
+#endif
}
prog_char str_start_arg0[] = "start";
parse_pgm_token_string_t cmd_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_start_result, arg0, str_start_arg0);
-prog_char str_start_color[] = "green#red";
+prog_char str_start_color[] = "blue#yellow";
parse_pgm_token_string_t cmd_start_color = TOKEN_STRING_INITIALIZER(struct cmd_start_result, color, str_start_color);
prog_char str_start_debug[] = "debug#match";
parse_pgm_token_string_t cmd_start_debug = TOKEN_STRING_INITIALIZER(struct cmd_start_result, debug, str_start_debug);
static void print_cs(void)
{
- printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
- "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"),
+ printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
+ "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
cs_get_consign(&mainboard.distance.cs),
cs_get_consign(&mainboard.angle.cs),
cs_get_filtered_consign(&mainboard.distance.cs),
static void print_pid(void)
{
- printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
- "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
+ "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
/* function called when cmd_color is parsed successfully */
static void cmd_color_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
- if (!strcmp_P(res->color, PSTR("red"))) {
- mainboard.our_color = I2C_COLOR_RED;
- i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_RED);
- i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_RED);
+ if (!strcmp_P(res->color, PSTR("yellow"))) {
+ mainboard.our_color = I2C_COLOR_YELLOW;
+ i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
+ i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
}
- else if (!strcmp_P(res->color, PSTR("green"))) {
- mainboard.our_color = I2C_COLOR_GREEN;
- i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_GREEN);
- i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_GREEN);
+ else if (!strcmp_P(res->color, PSTR("blue"))) {
+ mainboard.our_color = I2C_COLOR_BLUE;
+ i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
+ i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
}
printf_P(PSTR("Done\r\n"));
+#endif
}
prog_char str_color_arg0[] = "color";
parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
-prog_char str_color_color[] = "green#red";
+prog_char str_color_color[] = "blue#yellow";
parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
prog_char help_color[] = "Set our color";
{
// struct cmd_rs_result *res = parsed_result;
do {
- printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "),
+ printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
cs_get_consign(&mainboard.angle.cs),
cs_get_filtered_feedback(&mainboard.angle.cs),
cs_get_out(&mainboard.angle.cs));
- printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "),
+ printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
cs_get_consign(&mainboard.distance.cs),
cs_get_filtered_feedback(&mainboard.distance.cs),
cs_get_out(&mainboard.distance.cs));
- printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
+ printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
mainboard.pwm_r);
wait_ms(100);
} while(!cmdline_keypressed());
/* function called when cmd_i2cdebug is parsed successfully */
static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
i2c_debug();
i2c_protocol_debug();
+#endif
}
prog_char str_i2cdebug_arg0[] = "i2cdebug";
/* function called when cmd_cobboard_show is parsed successfully */
static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
printf_P(PSTR("status = %x\r\n"), cobboard.status);
+ printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
+ printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
+ printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
+#endif
}
prog_char str_cobboard_show_arg0[] = "cobboard";
/* function called when cmd_cobboard_setmode1 is parsed successfully */
static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_cobboard_setmode1_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("init")))
i2c_cobboard_mode_init();
- else if (!strcmp_P(res->arg1, PSTR("manual")))
- i2c_cobboard_mode_manual();
- else if (!strcmp_P(res->arg1, PSTR("harvest")))
- i2c_cobboard_mode_harvest();
+ else if (!strcmp_P(res->arg1, PSTR("eject")))
+ i2c_cobboard_mode_eject();
+#endif
}
prog_char str_cobboard_setmode1_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
-prog_char str_cobboard_setmode1_arg1[] = "init#manual#harvest";
+prog_char str_cobboard_setmode1_arg1[] = "init#eject";
parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
/* function called when cmd_cobboard_setmode2 is parsed successfully */
static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_cobboard_setmode2_result *res = parsed_result;
uint8_t side = I2C_LEFT_SIDE;
else if (!strcmp_P(res->arg2, PSTR("right")))
side = I2C_RIGHT_SIDE;
- if (!strcmp_P(res->arg1, PSTR("yyy")))
- printf("faux\r\n");
- else if (!strcmp_P(res->arg1, PSTR("xxx")))
- printf("faux\r\n");
+ if (!strcmp_P(res->arg1, PSTR("deploy")))
+ i2c_cobboard_mode_deploy(side);
+ else if (!strcmp_P(res->arg1, PSTR("harvest")))
+ i2c_cobboard_mode_harvest(side);
+ else if (!strcmp_P(res->arg1, PSTR("pack")))
+ i2c_cobboard_mode_pack(side);
+#endif
}
prog_char str_cobboard_setmode2_arg0[] = "cobboard";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
-prog_char str_cobboard_setmode2_arg1[] = "xxx";
+prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
prog_char str_cobboard_setmode2_arg2[] = "left#right";
parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
/* function called when cmd_cobboard_setmode3 is parsed successfully */
static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_cobboard_setmode3_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("xxx")))
printf("faux\r\n");
+#endif
}
prog_char str_cobboard_setmode3_arg0[] = "cobboard";
},
};
+/**********************************************************/
+/* Ballboard_Show */
+
+/* this structure is filled when cmd_ballboard_show is parsed successfully */
+struct cmd_ballboard_show_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_ballboard_show is parsed successfully */
+static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
+{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
+ printf_P(PSTR("status = %x\r\n"), ballboard.status);
+ printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
+#endif
+}
+
+prog_char str_ballboard_show_arg0[] = "ballboard";
+parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
+prog_char str_ballboard_show_arg1[] = "show";
+parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
+
+prog_char help_ballboard_show[] = "show ballboard status";
+parse_pgm_inst_t cmd_ballboard_show = {
+ .f = cmd_ballboard_show_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ballboard_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_ballboard_show_arg0,
+ (prog_void *)&cmd_ballboard_show_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Ballboard_Setmode1 */
+
+/* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
+struct cmd_ballboard_setmode1_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_ballboard_setmode1 is parsed successfully */
+static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
+{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ struct cmd_ballboard_setmode1_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("init")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
+ else if (!strcmp_P(res->arg1, PSTR("off")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
+ else if (!strcmp_P(res->arg1, PSTR("eject")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
+ else if (!strcmp_P(res->arg1, PSTR("harvest")))
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
+
+ /* other commands */
+#endif
+}
+
+prog_char str_ballboard_setmode1_arg0[] = "ballboard";
+parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
+prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
+parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
+
+prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
+parse_pgm_inst_t cmd_ballboard_setmode1 = {
+ .f = cmd_ballboard_setmode1_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ballboard_setmode1,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_ballboard_setmode1_arg0,
+ (prog_void *)&cmd_ballboard_setmode1_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Ballboard_Setmode2 */
+
+/* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
+struct cmd_ballboard_setmode2_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_ballboard_setmode2 is parsed successfully */
+static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
+{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ struct cmd_ballboard_setmode2_result *res = parsed_result;
+ uint8_t mode = I2C_BALLBOARD_MODE_INIT;
+
+ if (!strcmp_P(res->arg2, PSTR("left"))) {
+ if (!strcmp_P(res->arg1, PSTR("prepare")))
+ mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
+ else if (!strcmp_P(res->arg1, PSTR("take")))
+ mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
+ }
+ else {
+ if (!strcmp_P(res->arg1, PSTR("prepare")))
+ mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
+ else if (!strcmp_P(res->arg1, PSTR("take")))
+ mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
+ }
+ i2c_ballboard_set_mode(mode);
+#endif
+}
+
+prog_char str_ballboard_setmode2_arg0[] = "ballboard";
+parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
+prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
+parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
+prog_char str_ballboard_setmode2_arg2[] = "left#right";
+parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
+
+prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
+parse_pgm_inst_t cmd_ballboard_setmode2 = {
+ .f = cmd_ballboard_setmode2_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ballboard_setmode2,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_ballboard_setmode2_arg0,
+ (prog_void *)&cmd_ballboard_setmode2_arg1,
+ (prog_void *)&cmd_ballboard_setmode2_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Ballboard_Setmode3 */
+
+/* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
+struct cmd_ballboard_setmode3_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t level;
+};
+
+/* function called when cmd_ballboard_setmode3 is parsed successfully */
+static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
+{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ struct cmd_ballboard_setmode3_result *res = parsed_result;
+ if (!strcmp_P(res->arg1, PSTR("xxx")))
+ printf("faux\r\n");
+#endif
+}
+
+prog_char str_ballboard_setmode3_arg0[] = "ballboard";
+parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
+prog_char str_ballboard_setmode3_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
+parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
+
+prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
+parse_pgm_inst_t cmd_ballboard_setmode3 = {
+ .f = cmd_ballboard_setmode3_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ballboard_setmode3,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_ballboard_setmode3_arg0,
+ (prog_void *)&cmd_ballboard_setmode3_arg1,
+ (prog_void *)&cmd_ballboard_setmode3_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Servo_Balls */
+
+/* this structure is filled when cmd_servo_balls is parsed successfully */
+struct cmd_servo_balls_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_servo_balls is parsed successfully */
+static void cmd_servo_balls_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
+{
+ struct cmd_servo_balls_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("deploy")))
+ support_balls_deploy();
+ else if (!strcmp_P(res->arg1, PSTR("pack")))
+ support_balls_pack();
+}
+
+prog_char str_servo_balls_arg0[] = "support_balls";
+parse_pgm_token_string_t cmd_servo_balls_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
+prog_char str_servo_balls_arg1[] = "deploy#pack";
+parse_pgm_token_string_t cmd_servo_balls_arg1 =
+ TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
+
+prog_char help_servo_balls[] = "control support balls";
+parse_pgm_inst_t cmd_servo_balls = {
+ .f = cmd_servo_balls_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_servo_balls,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_servo_balls_arg0,
+ (prog_void *)&cmd_servo_balls_arg1,
+ NULL,
+ },
+};
+
/**********************************************************/
/* Test */
struct cmd_test_result {
fixed_string_t arg0;
int32_t radius;
+ int32_t dist;
};
+/* function called when cmd_test is parsed successfully */
+static void line2line(double line1x1, double line1y1,
+ double line1x2, double line1y2,
+ double line2x1, double line2y1,
+ double line2x2, double line2y2,
+ double radius, double dist)
+{
+ uint8_t err;
+ int32_t dist_imp_target;
+ double speed_d, speed_a;
+ double distance, angle;
+ double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
+ double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
+
+ printf("%s()\n", __FUNCTION__);
+ strat_set_speed(500, 500);
+ circle_get_da_speed_from_radius(&mainboard.traj, radius,
+ &speed_d, &speed_a);
+ trajectory_line_abs(&mainboard.traj,
+ line1x1, line1y1,
+ line1x2, line1y2, 150.);
+ err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
+ dist, TRAJ_FLAGS_NO_NEAR);
+ /* circle */
+ strat_set_speed(speed_d, speed_a);
+ angle = line2_angle - line1_angle;
+ distance = angle * radius;
+ if (distance < 0)
+ distance = -distance;
+ dist_imp_target = rs_get_distance(&mainboard.rs) +
+ distance * mainboard.pos.phys.distance_imp_per_mm;
+ angle = DEG(angle);
+ distance += 100; /* take some margin to avoid deceleration */
+ trajectory_d_a_rel(&mainboard.traj, distance, angle);
+
+ err = WAIT_COND_OR_TRAJ_END(rs_get_distance(&mainboard.rs) > dist_imp_target,
+ TRAJ_FLAGS_NO_NEAR);
+
+ strat_set_speed(500, 500);
+ trajectory_line_abs(&mainboard.traj,
+ line2x1, line2y1,
+ line2x2, line2y2, 150.);
+}
+
/* function called when cmd_test is parsed successfully */
static void cmd_test_parsed(void *parsed_result, void *data)
{
+ // struct cmd_test_result *res = parsed_result;
+#ifdef HOST_VERSION
+ strat_reset_pos(400, 400, 90);
+ mainboard.angle.on = 1;
+ mainboard.distance.on = 1;
+#endif
+ line2line(375, 347, 375, 1847,
+ 375, 1847, 1050, 1472,
+ 100, 200);
+ line2line(825, 1596, 1050, 1472,
+ 1050, 1472, 1500, 1722,
+ 180, 120);
+ line2line(1050, 1472, 1500, 1722,
+ 1500, 1722, 2175, 1347,
+ 180, 120);
+ line2line(1500, 1722, 2175, 1347,
+ 2175, 1347, 2175, 847,
+ 150, 120);
+ line2line(2175, 1347, 2175, 847,
+ 2175, 847, 2400, 722,
+ 150, 120);
+ line2line(2175, 847, 2400, 722,
+ 2400, 722, 2625, 847,
+ 150, 100);
+ line2line(2400, 722, 2625, 847,
+ 2625, 847, 2625, 1847,
+ 150, 100);
+ line2line(2625, 847, 2625, 1847,
+ 2625, 1847, 375, 597,
+ 100, 200);
}
prog_char str_test_arg0[] = "test";
parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
+parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
prog_char help_test[] = "Test function";
parse_pgm_inst_t cmd_test = {
.help_str = help_test,
.tokens = { /* token list, NULL terminated */
(prog_void *)&cmd_test_arg0,
- (prog_void *)&cmd_test_arg1,
NULL,
},
};
git.droids-corp.org / aversive.git / blobdiff