#include <stdio.h>
#include <string.h>
+#include <math.h>
+#include <hostsim.h>
#include <aversive/pgmspace.h>
#include <aversive/wait.h>
#include <aversive/error.h>
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
-#include <time.h>
+#include <clock_time.h>
#include <spi.h>
#include <i2c.h>
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.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_MECHBOARD_ADDR, I2C_COLOR_RED);
- i2c_set_color(I2C_SENSORBOARD_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_MECHBOARD_ADDR, I2C_COLOR_GREEN);
- i2c_set_color(I2C_SENSORBOARD_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("Check that lintel is loaded\r\n"));
- while(!cmdline_keypressed());
-
- printf_P(PSTR("Press a key when beacon ready\r\n"));
- i2c_sensorboard_set_beacon(0);
- while(!cmdline_keypressed());
- i2c_sensorboard_set_beacon(1);
-
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_MECHBOARD_ADDR, I2C_COLOR_RED);
- i2c_set_color(I2C_SENSORBOARD_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_MECHBOARD_ADDR, I2C_COLOR_GREEN);
- i2c_set_color(I2C_SENSORBOARD_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";
};
/**********************************************************/
-/* Mechboard_Show */
+/* Cobboard_Show */
-/* this structure is filled when cmd_mechboard_show is parsed successfully */
-struct cmd_mechboard_show_result {
+/* this structure is filled when cmd_cobboard_show is parsed successfully */
+struct cmd_cobboard_show_result {
fixed_string_t arg0;
fixed_string_t arg1;
};
-/* function called when cmd_mechboard_show is parsed successfully */
-static void cmd_mechboard_show_parsed(void * parsed_result, void * data)
+/* function called when cmd_cobboard_show is parsed successfully */
+static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
{
- printf_P(PSTR("mode = %x\r\n"), mechboard.mode);
- printf_P(PSTR("status = %x\r\n"), mechboard.status);
- printf_P(PSTR("lintel_count = %d\r\n"), mechboard.lintel_count);
-
- printf_P(PSTR("column_count = %d\r\n"), get_column_count());
- printf_P(PSTR("left1=%d left2=%d right1=%d right2=%d\r\n"),
- pump_left1_is_full(), pump_left2_is_full(),
- pump_right1_is_full(), pump_right2_is_full());
-
- printf_P(PSTR("pump_left1 = %d\r\n"), mechboard.pump_left1);
- printf_P(PSTR("pump_left2 = %d\r\n"), mechboard.pump_left2);
- printf_P(PSTR("pump_right1 = %d\r\n"), mechboard.pump_right1);
- printf_P(PSTR("pump_right2 = %d\r\n"), mechboard.pump_right2);
-
- printf_P(PSTR("servo_lintel_left = %d\r\n"), mechboard.servo_lintel_left);
- printf_P(PSTR("servo_lintel_right = %d\r\n"), mechboard.servo_lintel_right);
-
+#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_mechboard_show_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg0, str_mechboard_show_arg0);
-prog_char str_mechboard_show_arg1[] = "show";
-parse_pgm_token_string_t cmd_mechboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_show_result, arg1, str_mechboard_show_arg1);
+prog_char str_cobboard_show_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_cobboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg0, str_cobboard_show_arg0);
+prog_char str_cobboard_show_arg1[] = "show";
+parse_pgm_token_string_t cmd_cobboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_show_result, arg1, str_cobboard_show_arg1);
-prog_char help_mechboard_show[] = "show mechboard status";
-parse_pgm_inst_t cmd_mechboard_show = {
- .f = cmd_mechboard_show_parsed, /* function to call */
+prog_char help_cobboard_show[] = "show cobboard status";
+parse_pgm_inst_t cmd_cobboard_show = {
+ .f = cmd_cobboard_show_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
- .help_str = help_mechboard_show,
+ .help_str = help_cobboard_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_show_arg0,
- (prog_void *)&cmd_mechboard_show_arg1,
+ (prog_void *)&cmd_cobboard_show_arg0,
+ (prog_void *)&cmd_cobboard_show_arg1,
NULL,
},
};
/**********************************************************/
-/* Mechboard_Setmode1 */
+/* Cobboard_Setmode1 */
-/* this structure is filled when cmd_mechboard_setmode1 is parsed successfully */
-struct cmd_mechboard_setmode1_result {
+/* this structure is filled when cmd_cobboard_setmode1 is parsed successfully */
+struct cmd_cobboard_setmode1_result {
fixed_string_t arg0;
fixed_string_t arg1;
};
-/* function called when cmd_mechboard_setmode1 is parsed successfully */
-static void cmd_mechboard_setmode1_parsed(void *parsed_result, void *data)
+/* function called when cmd_cobboard_setmode1 is parsed successfully */
+static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
{
- struct cmd_mechboard_setmode1_result *res = parsed_result;
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ struct cmd_cobboard_setmode1_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("init")))
- i2c_mechboard_mode_init();
- else if (!strcmp_P(res->arg1, PSTR("manual")))
- i2c_mechboard_mode_manual();
- else if (!strcmp_P(res->arg1, PSTR("pickup")))
- i2c_mechboard_mode_pickup();
- else if (!strcmp_P(res->arg1, PSTR("lazy_harvest")))
- i2c_mechboard_mode_lazy_harvest();
- else if (!strcmp_P(res->arg1, PSTR("harvest")))
- i2c_mechboard_mode_harvest();
- else if (!strcmp_P(res->arg1, PSTR("prepare_get_lintel")))
- i2c_mechboard_mode_prepare_get_lintel();
- else if (!strcmp_P(res->arg1, PSTR("get_lintel")))
- i2c_mechboard_mode_get_lintel();
- else if (!strcmp_P(res->arg1, PSTR("put_lintel")))
- i2c_mechboard_mode_put_lintel();
- else if (!strcmp_P(res->arg1, PSTR("init")))
- i2c_mechboard_mode_init();
+ i2c_cobboard_mode_init();
else if (!strcmp_P(res->arg1, PSTR("eject")))
- i2c_mechboard_mode_init();
- else if (!strcmp_P(res->arg1, PSTR("clear")))
- i2c_mechboard_mode_clear();
- else if (!strcmp_P(res->arg1, PSTR("loaded")))
- i2c_mechboard_mode_loaded();
- else if (!strcmp_P(res->arg1, PSTR("store")))
- i2c_mechboard_mode_store();
- else if (!strcmp_P(res->arg1, PSTR("manivelle")))
- i2c_mechboard_mode_manivelle();
- else if (!strcmp_P(res->arg1, PSTR("lazy_pickup")))
- i2c_mechboard_mode_lazy_pickup();
+ i2c_cobboard_mode_eject();
+#endif
}
-prog_char str_mechboard_setmode1_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg0, str_mechboard_setmode1_arg0);
-prog_char str_mechboard_setmode1_arg1[] = "manivelle#init#manual#pickup#prepare_get_lintel#get_lintel#put_lintel1#eject#clear#harvest#lazy_harvest#store#lazy_pickup";
-parse_pgm_token_string_t cmd_mechboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode1_result, arg1, str_mechboard_setmode1_arg1);
+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#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_mechboard_setmode1[] = "set mechboard mode (mode)";
-parse_pgm_inst_t cmd_mechboard_setmode1 = {
- .f = cmd_mechboard_setmode1_parsed, /* function to call */
+prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
+parse_pgm_inst_t cmd_cobboard_setmode1 = {
+ .f = cmd_cobboard_setmode1_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
- .help_str = help_mechboard_setmode1,
+ .help_str = help_cobboard_setmode1,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_setmode1_arg0,
- (prog_void *)&cmd_mechboard_setmode1_arg1,
+ (prog_void *)&cmd_cobboard_setmode1_arg0,
+ (prog_void *)&cmd_cobboard_setmode1_arg1,
NULL,
},
};
/**********************************************************/
-/* Mechboard_Setmode2 */
+/* Cobboard_Setmode2 */
-/* this structure is filled when cmd_mechboard_setmode2 is parsed successfully */
-struct cmd_mechboard_setmode2_result {
+/* this structure is filled when cmd_cobboard_setmode2 is parsed successfully */
+struct cmd_cobboard_setmode2_result {
fixed_string_t arg0;
fixed_string_t arg1;
fixed_string_t arg2;
};
-/* function called when cmd_mechboard_setmode2 is parsed successfully */
-static void cmd_mechboard_setmode2_parsed(void * parsed_result, void * data)
+/* function called when cmd_cobboard_setmode2 is parsed successfully */
+static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
{
- struct cmd_mechboard_setmode2_result *res = parsed_result;
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ struct cmd_cobboard_setmode2_result *res = parsed_result;
uint8_t side = I2C_LEFT_SIDE;
if (!strcmp_P(res->arg2, PSTR("left")))
side = I2C_LEFT_SIDE;
else if (!strcmp_P(res->arg2, PSTR("right")))
side = I2C_RIGHT_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("center")))
- side = I2C_CENTER_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("auto")))
- side = I2C_AUTO_SIDE;
-
- if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
- i2c_mechboard_mode_prepare_pickup(side);
- else if (!strcmp_P(res->arg1, PSTR("push_temple_disc")))
- i2c_mechboard_mode_push_temple_disc(side);
+
+ 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_mechboard_setmode2_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0);
-prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc";
-parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1);
-prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center";
-parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2);
+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[] = "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);
-prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)";
-parse_pgm_inst_t cmd_mechboard_setmode2 = {
- .f = cmd_mechboard_setmode2_parsed, /* function to call */
+prog_char help_cobboard_setmode2[] = "set cobboard mode (mode, side)";
+parse_pgm_inst_t cmd_cobboard_setmode2 = {
+ .f = cmd_cobboard_setmode2_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
- .help_str = help_mechboard_setmode2,
+ .help_str = help_cobboard_setmode2,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_setmode2_arg0,
- (prog_void *)&cmd_mechboard_setmode2_arg1,
- (prog_void *)&cmd_mechboard_setmode2_arg2,
+ (prog_void *)&cmd_cobboard_setmode2_arg0,
+ (prog_void *)&cmd_cobboard_setmode2_arg1,
+ (prog_void *)&cmd_cobboard_setmode2_arg2,
NULL,
},
};
/**********************************************************/
-/* Mechboard_Setmode3 */
+/* Cobboard_Setmode3 */
-/* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */
-struct cmd_mechboard_setmode3_result {
+/* this structure is filled when cmd_cobboard_setmode3 is parsed successfully */
+struct cmd_cobboard_setmode3_result {
fixed_string_t arg0;
fixed_string_t arg1;
uint8_t level;
};
-/* function called when cmd_mechboard_setmode3 is parsed successfully */
-static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data)
+/* function called when cmd_cobboard_setmode3 is parsed successfully */
+static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
{
- struct cmd_mechboard_setmode3_result *res = parsed_result;
- if (!strcmp_P(res->arg1, PSTR("autobuild")))
- i2c_mechboard_mode_simple_autobuild(res->level);
- else if (!strcmp_P(res->arg1, PSTR("prepare_build")))
- i2c_mechboard_mode_prepare_build_both(res->level);
- else if (!strcmp_P(res->arg1, PSTR("prepare_inside")))
- i2c_mechboard_mode_prepare_inside_both(res->level);
- else if (!strcmp_P(res->arg1, PSTR("push_temple")))
- i2c_mechboard_mode_push_temple(res->level);
+#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_mechboard_setmode3_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0);
-prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple";
-parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1);
-parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8);
+prog_char str_cobboard_setmode3_arg0[] = "cobboard";
+parse_pgm_token_string_t cmd_cobboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg0, str_cobboard_setmode3_arg0);
+prog_char str_cobboard_setmode3_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_cobboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode3_result, arg1, str_cobboard_setmode3_arg1);
+parse_pgm_token_num_t cmd_cobboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_cobboard_setmode3_result, level, UINT8);
-prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)";
-parse_pgm_inst_t cmd_mechboard_setmode3 = {
- .f = cmd_mechboard_setmode3_parsed, /* function to call */
+prog_char help_cobboard_setmode3[] = "set cobboard mode (mode, level)";
+parse_pgm_inst_t cmd_cobboard_setmode3 = {
+ .f = cmd_cobboard_setmode3_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
- .help_str = help_mechboard_setmode3,
+ .help_str = help_cobboard_setmode3,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_setmode3_arg0,
- (prog_void *)&cmd_mechboard_setmode3_arg1,
- (prog_void *)&cmd_mechboard_setmode3_arg2,
+ (prog_void *)&cmd_cobboard_setmode3_arg0,
+ (prog_void *)&cmd_cobboard_setmode3_arg1,
+ (prog_void *)&cmd_cobboard_setmode3_arg2,
NULL,
},
};
/**********************************************************/
-/* Mechboard_Setmode4 */
+/* Ballboard_Show */
-/* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */
-struct cmd_mechboard_setmode4_result {
+/* this structure is filled when cmd_ballboard_show is parsed successfully */
+struct cmd_ballboard_show_result {
fixed_string_t arg0;
fixed_string_t arg1;
- uint8_t level_l;
- uint8_t count_l;
- uint8_t dist_l;
- uint8_t level_r;
- uint8_t count_r;
- uint8_t dist_r;
- uint8_t do_lintel;
};
-/* function called when cmd_mechboard_setmode4 is parsed successfully */
-static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data)
+/* function called when cmd_ballboard_show is parsed successfully */
+static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
{
- struct cmd_mechboard_setmode4_result *res = parsed_result;
- i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l,
- res->level_r, res->count_r, res->dist_r,
- res->do_lintel);
+#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_mechboard_setmode4_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0);
-prog_char str_mechboard_setmode4_arg1[] = "autobuild";
-parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8);
-
-prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)";
-parse_pgm_inst_t cmd_mechboard_setmode4 = {
- .f = cmd_mechboard_setmode4_parsed, /* function to call */
+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_mechboard_setmode4,
+ .help_str = help_ballboard_show,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_setmode4_arg0,
- (prog_void *)&cmd_mechboard_setmode4_arg1,
- (prog_void *)&cmd_mechboard_setmode4_arg2,
- (prog_void *)&cmd_mechboard_setmode4_arg3,
- (prog_void *)&cmd_mechboard_setmode4_arg4,
- (prog_void *)&cmd_mechboard_setmode4_arg5,
- (prog_void *)&cmd_mechboard_setmode4_arg6,
- (prog_void *)&cmd_mechboard_setmode4_arg7,
- (prog_void *)&cmd_mechboard_setmode4_arg8,
+ (prog_void *)&cmd_ballboard_show_arg0,
+ (prog_void *)&cmd_ballboard_show_arg1,
NULL,
},
};
/**********************************************************/
-/* Mechboard_Setmode5 */
+/* Ballboard_Setmode1 */
-/* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */
-struct cmd_mechboard_setmode5_result {
+/* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
+struct cmd_ballboard_setmode1_result {
fixed_string_t arg0;
fixed_string_t arg1;
- fixed_string_t arg2;
- fixed_string_t arg3;
};
-/* function called when cmd_mechboard_setmode5 is parsed successfully */
-static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data)
+/* function called when cmd_ballboard_setmode1 is parsed successfully */
+static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
{
- struct cmd_mechboard_setmode5_result *res = parsed_result;
- uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
-
- if (!strcmp_P(res->arg2, PSTR("left")))
- side = I2C_LEFT_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("right")))
- side = I2C_RIGHT_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("center")))
- side = I2C_CENTER_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("auto")))
- side = I2C_AUTO_SIDE;
-
- if (!strcmp_P(res->arg3, PSTR("harvest")))
- next_mode = I2C_MECHBOARD_MODE_HARVEST;
- else if (!strcmp_P(res->arg3, PSTR("lazy_harvest")))
- next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
- else if (!strcmp_P(res->arg3, PSTR("pickup")))
- next_mode = I2C_MECHBOARD_MODE_PICKUP;
- else if (!strcmp_P(res->arg3, PSTR("clear")))
- next_mode = I2C_MECHBOARD_MODE_CLEAR;
- else if (!strcmp_P(res->arg3, PSTR("store")))
- next_mode = I2C_MECHBOARD_MODE_STORE;
- else if (!strcmp_P(res->arg3, PSTR("lazy_pickup")))
- next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
-
- if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
- i2c_mechboard_mode_prepare_pickup_next(side, next_mode);
-}
-
-prog_char str_mechboard_setmode5_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0);
-prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup";
-parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1);
-prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center";
-parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2);
-prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear";
-parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3);
-
-prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)";
-parse_pgm_inst_t cmd_mechboard_setmode5 = {
- .f = cmd_mechboard_setmode5_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_mechboard_setmode5,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_setmode5_arg0,
- (prog_void *)&cmd_mechboard_setmode5_arg1,
- (prog_void *)&cmd_mechboard_setmode5_arg2,
- (prog_void *)&cmd_mechboard_setmode5_arg3,
- NULL,
- },
-};
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ struct cmd_ballboard_setmode1_result *res = parsed_result;
-/**********************************************************/
-/* pickup wheels */
-
-/* this structure is filled when cmd_pickup_wheels is parsed successfully */
-struct cmd_pickup_wheels_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
+ 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);
-/* function called when cmd_pickup_wheels is parsed successfully */
-static void cmd_pickup_wheels_parsed(void *parsed_result, void *data)
-{
- struct cmd_pickup_wheels_result *res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("on")))
- pickup_wheels_on();
- else
- pickup_wheels_off();
+ /* other commands */
+#endif
}
-prog_char str_pickup_wheels_arg0[] = "pickup_wheels";
-parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0);
-prog_char str_pickup_wheels_arg1[] = "on#off";
-parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1);
+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_pickup_wheels[] = "Enable/disable pickup wheels";
-parse_pgm_inst_t cmd_pickup_wheels = {
- .f = cmd_pickup_wheels_parsed, /* function to call */
+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_pickup_wheels,
+ .help_str = help_ballboard_setmode1,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pickup_wheels_arg0,
- (prog_void *)&cmd_pickup_wheels_arg1,
+ (prog_void *)&cmd_ballboard_setmode1_arg0,
+ (prog_void *)&cmd_ballboard_setmode1_arg1,
NULL,
},
};
/**********************************************************/
-/* Beacon_Start */
+/* Ballboard_Setmode2 */
-/* this structure is filled when cmd_beacon_start is parsed successfully */
-struct cmd_beacon_start_result {
+/* 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_beacon_start is parsed successfully */
-static void cmd_beacon_start_parsed(void *parsed_result, void *data)
+/* function called when cmd_ballboard_setmode2 is parsed successfully */
+static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
{
- struct cmd_beacon_start_result *res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("start")))
- i2c_sensorboard_set_beacon(1);
- else
- i2c_sensorboard_set_beacon(0);
+#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_beacon_start_arg0[] = "beacon";
-parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
-prog_char str_beacon_start_arg1[] = "start#stop";
-parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
+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_beacon_start[] = "Beacon enabled/disable";
-parse_pgm_inst_t cmd_beacon_start = {
- .f = cmd_beacon_start_parsed, /* function to call */
+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_beacon_start,
+ .help_str = help_ballboard_setmode2,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_beacon_start_arg0,
- (prog_void *)&cmd_beacon_start_arg1,
+ (prog_void *)&cmd_ballboard_setmode2_arg0,
+ (prog_void *)&cmd_ballboard_setmode2_arg1,
+ (prog_void *)&cmd_ballboard_setmode2_arg2,
NULL,
},
};
/**********************************************************/
-/* Pump_Current */
+/* Ballboard_Setmode3 */
-/* this structure is filled when cmd_pump_current is parsed successfully */
-struct cmd_pump_current_result {
+/* 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_pump_current is parsed successfully */
-static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result,
- __attribute__((unused)) void *data)
+/* function called when cmd_ballboard_setmode3 is parsed successfully */
+static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
{
- printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"),
- sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4),
- mechboard.pump_right1_current, mechboard.pump_right2_current);
+#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_pump_current_arg0[] = "pump_current";
-parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0);
-prog_char str_pump_current_arg1[] = "show";
-parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1);
+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_pump_current[] = "dump pump current";
-parse_pgm_inst_t cmd_pump_current = {
- .f = cmd_pump_current_parsed, /* function to call */
+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_pump_current,
+ .help_str = help_ballboard_setmode3,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pump_current_arg0,
- (prog_void *)&cmd_pump_current_arg1,
+ (prog_void *)&cmd_ballboard_setmode3_arg0,
+ (prog_void *)&cmd_ballboard_setmode3_arg1,
+ (prog_void *)&cmd_ballboard_setmode3_arg2,
NULL,
},
};
/**********************************************************/
-/* Build_Test */
+/* Servo_Balls */
-/* this structure is filled when cmd_build_test is parsed successfully */
-struct cmd_build_test_result {
+/* 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_build_test is parsed successfully */
-static void cmd_build_test_parsed(void *parsed_result, void *data)
+/* function called when cmd_servo_balls is parsed successfully */
+static void cmd_servo_balls_parsed(void *parsed_result,
+ __attribute__((unused)) void *data)
{
- //struct cmd_build_test_result *res = parsed_result;
-
- printf_P(PSTR("lintel must be there\r\n"));
- i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
- I2C_MECHBOARD_MODE_HARVEST);
- wait_ms(500);
-
- printf_P(PSTR("Insert 4 colums\r\n"));
- while (get_column_count() != 4);
-
- i2c_mechboard_mode_prepare_build_both(0);
- trajectory_d_rel(&mainboard.traj, 200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- wait_ms(500);
-
- i2c_mechboard_mode_simple_autobuild(0);
- wait_ms(100);
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
- trajectory_d_rel(&mainboard.traj, -200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
- I2C_MECHBOARD_MODE_HARVEST);
-
- while (get_column_count() != 3);
-
- i2c_mechboard_mode_prepare_build_both(3);
- trajectory_d_rel(&mainboard.traj, 200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- wait_ms(500);
-
- i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST,
- 3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0);
- i2cproto_wait_update();
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
- trajectory_d_rel(&mainboard.traj, -200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
- wait_ms(500);
+ struct cmd_servo_balls_result *res = parsed_result;
- i2c_mechboard_mode_harvest();
- while (get_column_count() != 3);
-
- i2c_mechboard_mode_prepare_build_both(5);
- trajectory_d_rel(&mainboard.traj, 200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- wait_ms(1000);
-
- i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST,
- 5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0);
- i2cproto_wait_update();
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
- trajectory_d_rel(&mainboard.traj, -200);
+ if (!strcmp_P(res->arg1, PSTR("deploy")))
+ support_balls_deploy();
+ else if (!strcmp_P(res->arg1, PSTR("pack")))
+ support_balls_pack();
}
-prog_char str_build_test_arg0[] = "build_test";
-parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0);
+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_build_test[] = "Build_Test function";
-parse_pgm_inst_t cmd_build_test = {
- .f = cmd_build_test_parsed, /* function to call */
+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_build_test,
+ .help_str = help_servo_balls,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_build_test_arg0,
+ (prog_void *)&cmd_servo_balls_arg0,
+ (prog_void *)&cmd_servo_balls_arg1,
NULL,
},
};
-
/**********************************************************/
-/* Column_Test */
+/* Clitoid */
-/* this structure is filled when cmd_column_test is parsed successfully */
-struct cmd_column_test_result {
+/* this structure is filled when cmd_clitoid is parsed successfully */
+struct cmd_clitoid_result {
fixed_string_t arg0;
- uint8_t level;
- int16_t dist;
- int8_t a1;
- int8_t a2;
- int8_t a3;
- int16_t arm_dist;
- int8_t nb_col;
+ float alpha_deg;
+ float beta_deg;
+ float R_mm;
+ float Vd;
+ float Amax;
+ float d_inter_mm;
};
-/* function called when cmd_column_test is parsed successfully */
-static void cmd_column_test_parsed(void *parsed_result, void *data)
+/**
+ * do a superb curve joining line1 to line2 which is composed of:
+ * - a clothoid starting from line1
+ * - a circle
+ * - another clothoid up to line2
+ *
+ * the function assumes that the initial linear speed is Vd and
+ * angular speed is 0.
+ *
+ * - alpha: total angle
+ * - beta: circular part of angle (lower than alpha)
+ * - R: the radius of the circle (must be != 0)
+ * - Vd: linear speed to use (in imp per cs period)
+ * - Amax: maximum angular acceleration
+ * - d_inter: distance in mm until the intersection of the
+ * 2 lines
+ *
+ * return 0 on success: in this case these parameters are filled:
+ * - Aa_out: the angular acceleration to configure in quadramp
+ * - remain_d_mm_out: remaining distance before start to turn
+ */
+uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm,
+ double Vd, double Amax, double d_inter_mm)
{
- struct cmd_column_test_result *res = parsed_result;
- uint8_t level = res->level, debug = 0;
- uint8_t c, push = 0;
-
- /* default conf */
- if (data) {
- res->dist = 70;
- res->a1 = -20;
- res->a2 = 40;
- res->a3 = -20;
- res->arm_dist = 220;
- res->nb_col = 2;
+ double Vd_mm_s;
+ double Va, Va_rd_s;
+ double t, d_mm, alpha_rad, beta_rad;
+ double remain_d_mm;
+ double Aa, Aa_rd_s2;
+ line_t line1, line2;
+ double x, y, a_rad;
+ point_t robot, intersect, pt2, center, proj;
+ vect_t v;
+
+ /* param check */
+ if (fabs(alpha_deg) <= fabs(beta_deg)) {
+ DEBUG(E_USER_STRAT, "alpha is smaller than beta");
+ return END_ERROR;
}
- if (!strcmp_P(res->arg0, PSTR("column_test_debug")))
- debug = 1;
- if (!strcmp_P(res->arg0, PSTR("column_test_push")))
- push = 1;
-
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
-
- /* Go to disc */
-
- trajectory_d_rel(&mainboard.traj, 200);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- /* go back, insert colums */
-
- trajectory_d_rel(&mainboard.traj, -200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
- I2C_MECHBOARD_MODE_HARVEST);
- printf_P(PSTR("Insert 4 colums\r\n"));
- while (get_column_count() != 4);
-
- /* build with left arm */
-
- i2c_mechboard_mode_prepare_inside_both(level);
- trajectory_d_rel(&mainboard.traj, 200-(res->dist));
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- if (debug)
- c = cmdline_getchar_wait();
-
- trajectory_a_rel(&mainboard.traj, res->a1);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- if (debug)
- c = cmdline_getchar_wait();
-
- i2c_mechboard_mode_prepare_build_select(level, -1);
- time_wait_ms(200);
- if (debug)
- c = cmdline_getchar_wait();
- i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist,
- 0, 0, res->arm_dist, 0);
- while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
- if (debug)
- c = cmdline_getchar_wait();
- i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1);
-
- if (debug)
- c = cmdline_getchar_wait();
- /* build with right arm */
-
- trajectory_a_rel(&mainboard.traj, res->a2);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- if (debug)
- c = cmdline_getchar_wait();
- /* only ok for nb_col == 2 */
- if ((level + res->nb_col) >= 7)
- i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1);
- else
- i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col);
- time_wait_ms(200);
- if (debug)
- c = cmdline_getchar_wait();
- i2c_mechboard_mode_autobuild(0, 0, res->arm_dist,
- level + res->nb_col, res->nb_col,
- res->arm_dist, 0);
- while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
-
- if (push) {
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, -100);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
- time_wait_ms(500);
- trajectory_d_rel(&mainboard.traj, 100);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- }
- else if (level == 1 || level == 0) {
- trajectory_d_rel(&mainboard.traj, -100);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- i2c_mechboard_mode_push_temple(level);
- time_wait_ms(400);
- strat_set_speed(200, SPEED_ANGLE_SLOW);
- trajectory_d_rel(&mainboard.traj, 120);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+ /* get angular speed Va */
+ Vd_mm_s = Vd * (CS_HZ/DIST_IMP_MM);
+ DEBUG(E_USER_STRAT, "Vd_mm_s=%2.2f", Vd_mm_s);
+ Va_rd_s = Vd_mm_s / R_mm;
+ Va = Va_rd_s * (DIST_IMP_MM * EXT_TRACK_MM / (2 * CS_HZ));
+ DEBUG(E_USER_STRAT, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va);
+
+ /* process 't', the time in seconds that we will take to do
+ * the first clothoid */
+ alpha_rad = RAD(alpha_deg);
+ beta_rad = RAD(beta_deg);
+ t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s);
+ DEBUG(E_USER_STRAT, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f",
+ R_mm, alpha_rad, beta_rad, t);
+
+ /* process the angular acceleration */
+ Aa_rd_s2 = Va_rd_s / t;
+ Aa = Aa_rd_s2 * (DIST_IMP_MM * EXT_TRACK_MM /
+ (2 * CS_HZ * CS_HZ));
+ DEBUG(E_USER_STRAT, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa);
+
+ /* exit if the robot cannot physically do it */
+ if (Aa > Amax) {
+ DEBUG(E_USER_STRAT, "greater than max acceleration");
+ return END_ERROR;
}
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
- if (debug)
- c = cmdline_getchar_wait();
- i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2);
-
- if (debug)
- c = cmdline_getchar_wait();
-
- trajectory_a_rel(&mainboard.traj, res->a3);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- if (debug)
- c = cmdline_getchar_wait();
- /* go back, insert colums */
-
- trajectory_d_rel(&mainboard.traj, -100);
-
- return;
-}
-
-prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push";
-parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0);
-parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8);
-
-prog_char help_column_test[] = "Column_Test function (level)";
-parse_pgm_inst_t cmd_column_test = {
- .f = cmd_column_test_parsed, /* function to call */
- .data = (void *)1, /* 2nd arg of func */
- .help_str = help_column_test,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_column_test_arg0,
- (prog_void *)&cmd_column_test_arg1,
- NULL,
- },
-};
-
-parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16);
-parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8);
-parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8);
-parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8);
-parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16);
-parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8);
-
-prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)";
-parse_pgm_inst_t cmd_column_test2 = {
- .f = cmd_column_test_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_column_test2,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_column_test_arg0,
- (prog_void *)&cmd_column_test_arg1,
- (prog_void *)&cmd_column_test_arg2,
- (prog_void *)&cmd_column_test_arg3,
- (prog_void *)&cmd_column_test_arg4,
- (prog_void *)&cmd_column_test_arg5,
- (prog_void *)&cmd_column_test_arg6,
- (prog_void *)&cmd_column_test_arg7,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Pickup_Test */
-
-/* this structure is filled when cmd_pickup_test is parsed successfully */
-struct cmd_pickup_test_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- int16_t dist;
-};
-
-/* return red or green sensor */
-#define COLOR_IR_SENSOR() \
- ({ \
- uint8_t __ret = 0; \
- if (side == I2C_RIGHT_SIDE) \
- __ret = sensor_get(S_DISP_RIGHT); \
- else \
- __ret = sensor_get(S_DISP_LEFT); \
- __ret; \
- }) \
-/* column dispensers */
-#define COL_SCAN_MARGIN 200
-/* distance between the wheel axis and the IR sensor */
-
-/* function called when cmd_pickup_test is parsed successfully */
-static void cmd_pickup_test_parsed(void *parsed_result, void *data)
-{
- uint8_t err, side, first_try = 1;
- int8_t cols_count_before, cols_count_after, cols;
- struct cmd_pickup_test_result *res = parsed_result;
- int16_t pos1, pos2, pos;
- microseconds us;
- int16_t dist = res->dist;
- uint8_t timeout = 0;
-
- if (!strcmp_P(res->arg1, PSTR("left")))
- side = I2C_LEFT_SIDE;
+ /* the robot position */
+ x = position_get_x_double(&mainboard.pos);
+ y = position_get_y_double(&mainboard.pos);
+ a_rad = position_get_a_rad_double(&mainboard.pos);
+
+ /* define line1 and line2 */
+ robot.x = x;
+ robot.y = y;
+ intersect.x = x + cos(a_rad) * d_inter_mm;
+ intersect.y = y + sin(a_rad) * d_inter_mm;
+ pts2line(&robot, &intersect, &line1);
+ pt2.x = intersect.x + cos(a_rad + alpha_rad);
+ pt2.y = intersect.y + sin(a_rad + alpha_rad);
+ pts2line(&intersect, &pt2, &line2);
+ DEBUG(E_USER_STRAT, "intersect=(%2.2f, %2.2f)",
+ intersect.x, intersect.y);
+
+ /* the center of the circle is at (d_mm, d_mm) when we have to
+ * start the clothoid */
+ d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) *
+ sqrt(M_PI) / 2.;
+ DEBUG(E_USER_STRAT, "d_mm=%2.2f", d_mm);
+
+ /* translate line1 */
+ v.x = intersect.x - robot.x;
+ v.y = intersect.y - robot.y;
+ if (a_rad > 0)
+ vect_rot_trigo(&v);
else
- side = I2C_RIGHT_SIDE;
-
- i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
- cols_count_before = get_column_count();
- position_set(&mainboard.pos, 0, 0, 0);
-
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, -1000);
- err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
- if (err) /* we should not reach end */
- goto fail;
- pos1 = position_get_x_s16(&mainboard.pos);
- printf_P(PSTR("pos1 = %d\r\n"), pos1);
-
- err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
- if (err)
- goto fail;
- pos2 = position_get_x_s16(&mainboard.pos);
- printf_P(PSTR("pos2 = %d\r\n"), pos2);
-
- pos = ABS(pos1 - pos2);
- printf_P(PSTR("pos = %d\r\n"), pos);
-
- trajectory_d_rel(&mainboard.traj, -dist + pos/2);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- if (side == I2C_LEFT_SIDE)
- trajectory_a_rel(&mainboard.traj, 90);
- else
- trajectory_a_rel(&mainboard.traj, -90);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- pickup_wheels_on();
- retry:
- if (first_try)
- i2c_mechboard_mode_lazy_harvest();
+ vect_rot_retro(&v);
+ vect_resize(&v, d_mm);
+ line_translate(&line1, &v);
+
+ /* translate line2 */
+ v.x = intersect.x - pt2.x;
+ v.y = intersect.y - pt2.y;
+ if (a_rad > 0)
+ vect_rot_trigo(&v);
else
- i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
- first_try = 0;
-
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, 300);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
- err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
-
- trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (!TRAJ_SUCCESS(err))
- goto fail;
-
- position_set(&mainboard.pos, 0, 0, 0);
- if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
- strat_eject_col(90, 0);
- goto retry;
+ vect_rot_retro(&v);
+ vect_resize(&v, d_mm);
+ line_translate(&line2, &v);
+
+ /* find the center of the circle, at the intersection of the
+ * new translated lines */
+ if (intersect_line(&line1, &line2, ¢er) != 1) {
+ DEBUG(E_USER_STRAT, "cannot find circle center");
+ return END_ERROR;
}
-
- /* start to pickup with finger / arms */
-
- printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__);
- i2c_mechboard_mode_pickup();
- WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
- I2C_MECHBOARD_MODE_PICKUP, 100);
- us = time_get_us2();
- cols = get_column_count();
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) {
- if (get_column_count() != cols) {
- cols = get_column_count();
- us = time_get_us2();
- }
- if ((get_column_count() - cols_count_before) >= 4) {
- printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__);
- break;
- }
- /* 1 second timeout */
- if (time_get_us2() - us > 1500000L) {
- printf_P(PSTR("%s timeout\r\n"), __FUNCTION__);
- timeout = 1;
- break;
- }
+ DEBUG(E_USER_STRAT, "center=(%2.2f,%2.2f)", center.x, center.y);
+
+ /* project center of circle on line1 */
+ proj_pt_line(¢er, &line1, &proj);
+ DEBUG(E_USER_STRAT, "proj=(%2.2f,%2.2f)", proj.x, proj.y);
+
+ /* process remaining distance before start turning */
+ remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm);
+ DEBUG(E_USER_STRAT, "remain_d=%2.2f", remain_d_mm);
+ if (remain_d_mm < 0) {
+ DEBUG(E_USER_STRAT, "too late, cannot turn");
+ return END_ERROR;
}
- /* eject if we found a bad color column */
-
- if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
- strat_eject_col(90, 0);
- goto retry;
- }
-
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, -250);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
-
- cols_count_after = get_column_count();
- cols = cols_count_after - cols_count_before;
- DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
-
- pickup_wheels_off();
- i2c_mechboard_mode_clear();
-
- wait_ms(1000);
- return;
- fail:
- printf_P(PSTR("failed\r\n"));
- strat_hardstop();
+ return END_TRAJ;
}
-prog_char str_pickup_test_arg0[] = "pickup_test";
-parse_pgm_token_string_t cmd_pickup_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg0, str_pickup_test_arg0);
-prog_char str_pickup_test_arg1[] = "left#right";
-parse_pgm_token_string_t cmd_pickup_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_test_result, arg1, str_pickup_test_arg1);
-parse_pgm_token_num_t cmd_pickup_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pickup_test_result, dist, INT16);
-
-prog_char help_pickup_test[] = "Pickup_Test function";
-parse_pgm_inst_t cmd_pickup_test = {
- .f = cmd_pickup_test_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_pickup_test,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pickup_test_arg0,
- (prog_void *)&cmd_pickup_test_arg1,
- (prog_void *)&cmd_pickup_test_arg2,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Lintel_Test */
-
-/* this structure is filled when cmd_lintel_test is parsed successfully */
-struct cmd_lintel_test_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_lintel_test is parsed successfully */
-static void cmd_lintel_test_parsed(void *parsed_result, void *data)
+/* function called when cmd_test is parsed successfully */
+static void cmd_clitoid_parsed(void *parsed_result, void *data)
{
- uint8_t err, first_try = 1, right_ok, left_ok;
- int16_t left_cur, right_cur;
-
- i2c_mechboard_mode_prepare_get_lintel();
- time_wait_ms(500);
- retry:
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, 500);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (!TRAJ_SUCCESS(err) && err != END_BLOCKING)
- goto fail;
-
- i2c_mechboard_mode_get_lintel();
- time_wait_ms(500);
-
- left_cur = sensor_get_adc(ADC_CSENSE3);
- left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN);
- right_cur = mechboard.pump_right1_current;
- right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN);
-
- printf_P(PSTR("left_ok=%d (%d), right_ok=%d (%d)\r\n"),
- left_ok, left_cur, right_ok, right_cur);
- if (first_try) {
- if (!right_ok && !left_ok) {
- i2c_mechboard_mode_prepare_get_lintel();
- time_wait_ms(300);
- }
- else if (right_ok && !left_ok) {
- i2c_mechboard_mode_prepare_get_lintel();
- time_wait_ms(300);
- strat_set_speed(500, 500);
- trajectory_d_a_rel(&mainboard.traj, -150, 30);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- trajectory_d_a_rel(&mainboard.traj, -140, -30);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- first_try = 0;
- goto retry;
- }
- else if (!right_ok && left_ok) {
- i2c_mechboard_mode_prepare_get_lintel();
- time_wait_ms(300);
- strat_set_speed(500, 500);
- trajectory_d_a_rel(&mainboard.traj, -150, -30);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- trajectory_d_a_rel(&mainboard.traj, -140, 30);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- first_try = 0;
- goto retry;
- }
- /* else, lintel is ok */
- else {
- i2c_mechboard_mode_put_lintel();
- }
- }
- else {
- if (right_ok && left_ok) {
- /* lintel is ok */
- i2c_mechboard_mode_put_lintel();
- }
- else {
- i2c_mechboard_mode_prepare_get_lintel();
- time_wait_ms(300);
- }
- }
-
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, -250);
- err = wait_traj_end(TRAJ_FLAGS_STD);
- return;
-
-fail:
- printf_P(PSTR("fail\r\n"));
- return;
+ struct cmd_clitoid_result *res = parsed_result;
+ clitoid(res->alpha_deg, res->beta_deg, res->R_mm,
+ res->Vd, res->Amax, res->d_inter_mm);
}
-prog_char str_lintel_test_arg0[] = "lintel_test";
-parse_pgm_token_string_t cmd_lintel_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_lintel_test_result, arg0, str_lintel_test_arg0);
-
-prog_char help_lintel_test[] = "Lintel_Test function";
-parse_pgm_inst_t cmd_lintel_test = {
- .f = cmd_lintel_test_parsed, /* function to call */
+prog_char str_clitoid_arg0[] = "clitoid";
+parse_pgm_token_string_t cmd_clitoid_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
+ arg0, str_clitoid_arg0);
+parse_pgm_token_num_t cmd_clitoid_alpha_deg =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ alpha_deg, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_beta_deg =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ beta_deg, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_R_mm =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ R_mm, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_Vd =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ Vd, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_Amax =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ Amax, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
+ TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+ d_inter_mm, FLOAT);
+
+prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
+parse_pgm_inst_t cmd_clitoid = {
+ .f = cmd_clitoid_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
- .help_str = help_lintel_test,
+ .help_str = help_clitoid,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_lintel_test_arg0,
+ (prog_void *)&cmd_clitoid_arg0,
+ (prog_void *)&cmd_clitoid_alpha_deg,
+ (prog_void *)&cmd_clitoid_beta_deg,
+ (prog_void *)&cmd_clitoid_R_mm,
+ (prog_void *)&cmd_clitoid_Vd,
+ (prog_void *)&cmd_clitoid_Amax,
+ (prog_void *)&cmd_clitoid_d_inter_mm,
NULL,
},
};
-/**********************************************************/
-/* Scan_Test */
+//////////////////////
-/* this structure is filled when cmd_scan_test is parsed successfully */
-struct cmd_scan_test_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- int16_t start_dist;
- int16_t scan_dist;
- int16_t scan_speed;
- int16_t center_x;
- int16_t center_y;
- uint8_t level;
-};
+// 500 -- 5
+// 400 -- 3
+#define TEST_SPEED 400
+#define TEST_ACC 3
-#define SCAN_MODE_CHECK_TEMPLE 0
-#define SCAN_MODE_SCAN_COL 1
-#define SCAN_MODE_SCAN_TEMPLE 2
-#define SCAN_MODE_TRAJ_ONLY 3
-
-/* function called when cmd_scan_test is parsed successfully */
-static void cmd_scan_test_parsed(void *parsed_result, void *data)
+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, mode=0, c;
- int16_t pos1x, pos1y, dist;
- struct cmd_scan_test_result *res = parsed_result;
- int16_t back_mm = 0;
-
- int16_t ckpt_rel_x = 0, ckpt_rel_y = 0;
-
- double center_abs_x, center_abs_y;
- double ckpt_rel_d, ckpt_rel_a;
- double ckpt_abs_x, ckpt_abs_y;
-
- if (!strcmp_P(res->arg1, PSTR("traj_only")))
- mode = SCAN_MODE_TRAJ_ONLY;
- else if (!strcmp_P(res->arg1, PSTR("check_temple")))
- mode = SCAN_MODE_CHECK_TEMPLE;
- else if (!strcmp_P(res->arg1, PSTR("scan_col")))
- mode = SCAN_MODE_SCAN_COL;
- else if (!strcmp_P(res->arg1, PSTR("scan_temple")))
- mode = SCAN_MODE_SCAN_TEMPLE;
-
- /* go to disc */
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
- trajectory_d_rel(&mainboard.traj, 400);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (err != END_BLOCKING)
- return;
-
- /* save absolute position of disc */
- rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y);
-
- /* go back and prepare to scan */
- strat_set_speed(1000, 1000);
- trajectory_d_a_rel(&mainboard.traj, -140, 130);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (!TRAJ_SUCCESS(err))
- return;
-
- /* prepare scanner arm */
- if (mode != SCAN_MODE_TRAJ_ONLY)
- i2c_sensorboard_scanner_prepare();
- time_wait_ms(250);
-
- strat_set_speed(res->scan_speed, 1000);
-
- pos1x = position_get_x_s16(&mainboard.pos);
- pos1y = position_get_y_s16(&mainboard.pos);
- trajectory_d_rel(&mainboard.traj, -res->scan_dist);
-
- while (1) {
- err = test_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (err != 0)
- break;
-
- dist = distance_from_robot(pos1x, pos1y);
-
- if (dist > res->start_dist)
- break;
-
- if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) {
- err = END_ERROR;
- break;
- }
- }
-
- if (err) {
- if (TRAJ_SUCCESS(err))
- err = END_ERROR; /* should not reach end */
- strat_hardstop();
- trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (mode != SCAN_MODE_TRAJ_ONLY)
- i2c_sensorboard_scanner_stop();
- return;
- }
-
- /* start the scanner */
+ uint8_t err;
+ 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_P(PSTR("%s()\r\n"), __FUNCTION__);
+
+ strat_set_speed(TEST_SPEED, TEST_SPEED);
+ quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
+
+ 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;
+ angle = simple_modulo_2pi(angle);
+ angle = DEG(angle);
+ printf_P(PSTR("(%d,%d,%d) "),
+ position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ position_get_a_deg_s16(&mainboard.pos));
+ printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
+ distance, angle);
- if (mode != SCAN_MODE_TRAJ_ONLY)
- i2c_sensorboard_scanner_start();
+ /* take some margin on dist to avoid deceleration */
+ trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
- err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN,
+ /* circle exit condition */
+ err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
TRAJ_FLAGS_NO_NEAR);
- if (err == 0)
- err = END_ERROR;
- if (!TRAJ_SUCCESS(err)) {
- strat_hardstop();
- trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- if (mode != SCAN_MODE_TRAJ_ONLY)
- i2c_sensorboard_scanner_stop();
- return;
- }
-
- if (mode == SCAN_MODE_TRAJ_ONLY)
- return;
-
- wait_scan_done(10000);
-
- i2c_sensorboard_scanner_stop();
-
- if (mode == SCAN_MODE_CHECK_TEMPLE) {
- i2c_sensorboard_scanner_algo_check(res->level,
- res->center_x, res->center_y);
- i2cproto_wait_update();
- wait_scan_done(10000);
- scanner_dump_state();
-
- if (sensorboard.dropzone_h == -1) {
- printf_P(PSTR("-- try to build a temple\r\n"));
- res->center_x = 15;
- res->center_y = 13;
- mode = SCAN_MODE_SCAN_TEMPLE;
- }
- }
-
- if (mode == SCAN_MODE_SCAN_TEMPLE) {
- i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC,
- res->center_x,
- res->center_y);
- i2cproto_wait_update();
- wait_scan_done(10000);
- scanner_dump_state();
-
- if (sensorboard.dropzone_h == -1 ||
- strat_scan_get_checkpoint(mode, &ckpt_rel_x,
- &ckpt_rel_y, &back_mm)) {
- printf_P(PSTR("-- try to build a column\r\n"));
- mode = SCAN_MODE_SCAN_COL;
- }
- }
-
- if (mode == SCAN_MODE_SCAN_COL) {
- i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
- res->center_x, res->center_y);
- i2cproto_wait_update();
- wait_scan_done(10000);
- scanner_dump_state();
-
- if (sensorboard.dropzone_h == -1 ||
- strat_scan_get_checkpoint(mode, &ckpt_rel_x,
- &ckpt_rel_y, &back_mm)) {
- return;
- }
- }
-
- if (sensorboard.dropzone_h == -1)
- return;
-
- if (mode == SCAN_MODE_CHECK_TEMPLE) {
- ckpt_rel_x = 220;
- ckpt_rel_y = 100;
- }
-
-
- printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y);
-
- rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y);
- abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a);
-
- printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y);
-
- printf_P(PSTR("ok ? (y/n)\r\n"));
-
- c = cmdline_getchar_wait();
-
- if (c != 'y')
- return;
-
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
- /* intermediate checkpoint for some positions */
- if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) {
- trajectory_goto_xy_rel(&mainboard.traj, 200, 100);
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- if (!TRAJ_SUCCESS(err))
- return;
- }
-
- trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y);
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- if (!TRAJ_SUCCESS(err))
- return;
-
- trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y);
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- if (!TRAJ_SUCCESS(err))
- return;
-
- c = cmdline_getchar_wait();
-
- pos1x = position_get_x_s16(&mainboard.pos);
- pos1y = position_get_y_s16(&mainboard.pos);
-
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
- trajectory_d_rel(&mainboard.traj, 200);
- err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200,
- TRAJ_FLAGS_SMALL_DIST);
- if (err == 0) {
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
- trajectory_d_rel(&mainboard.traj, 400);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- }
- if (err != END_BLOCKING)
- return;
-
- if (back_mm) {
- trajectory_d_rel(&mainboard.traj, -back_mm);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- }
-}
-
-prog_char str_scan_test_arg0[] = "scan_test";
-parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0);
-prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple";
-parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1);
-parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16);
-parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16);
-parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16);
-parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16);
-parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16);
-
-prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)";
-parse_pgm_inst_t cmd_scan_test = {
- .f = cmd_scan_test_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_scan_test,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_scan_test_arg0,
- (prog_void *)&cmd_scan_test_arg1,
- (prog_void *)&cmd_scan_test_arg2,
- (prog_void *)&cmd_scan_test_arg3,
- (prog_void *)&cmd_scan_test_arg4,
- (prog_void *)&cmd_scan_test_arg5,
- (prog_void *)&cmd_scan_test_arg6,
- NULL,
- },
-};
-
-prog_char str_scan_test_arg1b[] = "check_temple";
-parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b);
-parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8);
-prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)";
-parse_pgm_inst_t cmd_scan_test2 = {
- .f = cmd_scan_test_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_scan_test,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_scan_test_arg0,
- (prog_void *)&cmd_scan_test_arg1b,
- (prog_void *)&cmd_scan_test_arg2,
- (prog_void *)&cmd_scan_test_arg3,
- (prog_void *)&cmd_scan_test_arg4,
- (prog_void *)&cmd_scan_test_arg5,
- (prog_void *)&cmd_scan_test_arg6,
- (prog_void *)&cmd_scan_test_arg7,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Time_Monitor */
-
-/* this structure is filled when cmd_time_monitor is parsed successfully */
-struct cmd_time_monitor_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_time_monitor is parsed successfully */
-static void cmd_time_monitor_parsed(void *parsed_result, void *data)
-{
- struct cmd_time_monitor_result *res = parsed_result;
- uint16_t seconds;
-
- if (!strcmp_P(res->arg1, PSTR("reset"))) {
- eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
- }
- seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
- printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
-}
-
-prog_char str_time_monitor_arg0[] = "time_monitor";
-parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
-prog_char str_time_monitor_arg1[] = "show#reset";
-parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
-
-prog_char help_time_monitor[] = "Show since how long we are running";
-parse_pgm_inst_t cmd_time_monitor = {
- .f = cmd_time_monitor_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_time_monitor,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_time_monitor_arg0,
- (prog_void *)&cmd_time_monitor_arg1,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Scanner */
-
-/* this structure is filled when cmd_scanner is parsed successfully */
-struct cmd_scanner_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_scanner is parsed successfully */
-static void cmd_scanner_parsed(void *parsed_result, void *data)
-{
- struct cmd_scanner_result *res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("prepare"))) {
- i2c_sensorboard_scanner_prepare();
- }
- else if (!strcmp_P(res->arg1, PSTR("stop"))) {
- i2c_sensorboard_scanner_stop();
- }
- else if (!strcmp_P(res->arg1, PSTR("start"))) {
- i2c_sensorboard_scanner_start();
- }
- else if (!strcmp_P(res->arg1, PSTR("algo_col"))) {
- i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
- 15, 15);
- }
- else if (!strcmp_P(res->arg1, PSTR("algo_check"))) {
- i2c_sensorboard_scanner_algo_check(2, 15, 15); // XXX
- }
- else if (!strcmp_P(res->arg1, PSTR("calib"))) {
- i2c_sensorboard_scanner_calib();
- }
- else if (!strcmp_P(res->arg1, PSTR("show"))) {
- scanner_dump_state();
- }
+ strat_set_speed(500, 500);
+ printf_P(PSTR("(%d,%d,%d) "),
+ position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ position_get_a_deg_s16(&mainboard.pos));
+ printf_P(PSTR("line\r\n"));
+ trajectory_line_abs(&mainboard.traj,
+ line2x1, line2y1,
+ line2x2, line2y2, 150.);
}
-prog_char str_scanner_arg0[] = "scanner";
-parse_pgm_token_string_t cmd_scanner_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg0, str_scanner_arg0);
-prog_char str_scanner_arg1[] = "prepare#start#algo_col#algo_check#stop#show#calib";
-parse_pgm_token_string_t cmd_scanner_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scanner_result, arg1, str_scanner_arg1);
-
-prog_char help_scanner[] = "send commands to scanner";
-parse_pgm_inst_t cmd_scanner = {
- .f = cmd_scanner_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_scanner,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_scanner_arg0,
- (prog_void *)&cmd_scanner_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Build_Z1 */
-
-/* this structure is filled when cmd_build_z1 is parsed successfully */
-struct cmd_build_z1_result {
- fixed_string_t arg0;
- uint8_t level;
- int16_t d1;
- int16_t d2;
- int16_t d3;
-};
-
-/* function called when cmd_build_z1 is parsed successfully */
-static void cmd_build_z1_parsed(void *parsed_result, void *data)
+static void halfturn(double line1x1, double line1y1,
+ double line1x2, double line1y2,
+ double line2x1, double line2y1,
+ double line2x2, double line2y2,
+ double radius, double dist, double dir)
{
- struct cmd_build_z1_result *res = parsed_result;
-
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
- trajectory_d_rel(&mainboard.traj, 400);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- trajectory_d_rel(&mainboard.traj, -200);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
- I2C_MECHBOARD_MODE_HARVEST);
-
- while (get_column_count() != 4);
-
- i2c_mechboard_mode_prepare_build_both(res->level);
- time_wait_ms(500);
-
- trajectory_d_rel(&mainboard.traj, 400);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
- trajectory_d_rel(&mainboard.traj, -res->d1);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- i2c_mechboard_mode_autobuild(res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
- res->level, 2, I2C_AUTOBUILD_DEFAULT_DIST,
- 1);
- WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
- I2C_MECHBOARD_MODE_AUTOBUILD, 100);
- WAIT_COND_OR_TIMEOUT(get_mechboard_mode() !=
- I2C_MECHBOARD_MODE_AUTOBUILD, 10000);
-
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
- trajectory_d_rel(&mainboard.traj, -res->d2);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- i2c_mechboard_mode_push_temple(1);
- time_wait_ms(400);
- strat_set_speed(200, SPEED_ANGLE_SLOW);
- trajectory_d_rel(&mainboard.traj, res->d3);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-}
-
-prog_char str_build_z1_arg0[] = "build_z1";
-parse_pgm_token_string_t cmd_build_z1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_z1_result, arg0, str_build_z1_arg0);
-parse_pgm_token_num_t cmd_build_z1_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, level, UINT8);
-parse_pgm_token_num_t cmd_build_z1_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d1, INT16);
-parse_pgm_token_num_t cmd_build_z1_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d2, INT16);
-parse_pgm_token_num_t cmd_build_z1_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_build_z1_result, d3, INT16);
-
-prog_char help_build_z1[] = "Build_Z1 function (level, d1, d2, d3)";
-parse_pgm_inst_t cmd_build_z1 = {
- .f = cmd_build_z1_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_build_z1,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_build_z1_arg0,
- (prog_void *)&cmd_build_z1_arg1,
- (prog_void *)&cmd_build_z1_arg2,
- (prog_void *)&cmd_build_z1_arg3,
- (prog_void *)&cmd_build_z1_arg4,
- NULL,
- },
-};
-
-#ifdef TEST_BEACON
-/**********************************************************/
-/* Beacon_Opp_Dump */
+ uint8_t err;
+ double speed_d, speed_a;
+ double distance, angle;
+
+ printf_P(PSTR("%s()\r\n"), __FUNCTION__);
+
+ strat_set_speed(TEST_SPEED, TEST_SPEED);
+ quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
+
+ 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 = dir * M_PI/2.;
+ distance = angle * radius;
+ if (distance < 0)
+ distance = -distance;
+ angle = simple_modulo_2pi(angle);
+ angle = DEG(angle);
+
+ /* take some margin on dist to avoid deceleration */
+ DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
+ distance, angle);
+ trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
+
+ /* circle exit condition */
+ err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+ TRAJ_FLAGS_NO_NEAR);
-/* this structure is filled when cmd_beacon_opp_dump is parsed successfully */
-struct cmd_beacon_opp_dump_result {
- fixed_string_t arg0;
-};
+ DEBUG(E_USER_STRAT, "miniline");
+ err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
+ dist, TRAJ_FLAGS_NO_NEAR);
+ DEBUG(E_USER_STRAT, "circle2");
+ /* take some margin on dist to avoid deceleration */
+ trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
-void beacon_dump_samples(void);
+ err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+ TRAJ_FLAGS_NO_NEAR);
-/* function called when cmd_beacon_opp_dump is parsed successfully */
-static void cmd_beacon_opp_dump_parsed(void *parsed_result, void *data)
-{
- beacon_dump_samples();
+ strat_set_speed(500, 500);
+ DEBUG(E_USER_STRAT, "line");
+ trajectory_line_abs(&mainboard.traj,
+ line2x1, line2y1,
+ line2x2, line2y2, 150.);
}
-prog_char str_beacon_opp_dump_arg0[] = "beacon_opp_dump";
-parse_pgm_token_string_t cmd_beacon_opp_dump_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_opp_dump_result, arg0, str_beacon_opp_dump_arg0);
-
-prog_char help_beacon_opp_dump[] = "Dump beacon samples";
-parse_pgm_inst_t cmd_beacon_opp_dump = {
- .f = cmd_beacon_opp_dump_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_beacon_opp_dump,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_beacon_opp_dump_arg0,
- NULL,
- },
-};
-#endif
-
/**********************************************************/
/* Test */
struct cmd_test_result {
fixed_string_t arg0;
int32_t radius;
+ int32_t dist;
};
-void circle_get_da_speed_from_radius(struct trajectory *traj,
- double radius_mm,
- double *speed_d,
- double *speed_a);
+
/* 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;
- double d,a;
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
- circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
- printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
+ // 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
+ printf_P(PSTR("%s()\r\n"), __FUNCTION__);
+ while (!cmdline_keypressed()) {
+ /****** PASS1 */
+
+#define DIST_HARD_TURN 260
+#define RADIUS_HARD_TURN 100
+#define DIST_EASY_TURN 190
+#define RADIUS_EASY_TURN 190
+#define DIST_HALF_TURN 225
+#define RADIUS_HALF_TURN 130
+
+ /* hard turn */
+ line2line(375, 597, 375, 1847,
+ 375, 1847, 1050, 1472,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /* easy left and easy right !*/
+ line2line(825, 1596, 1050, 1472,
+ 1050, 1472, 1500, 1722,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+ line2line(1050, 1472, 1500, 1722,
+ 1500, 1722, 2175, 1347,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+ line2line(1500, 1722, 2175, 1347,
+ 2175, 1347, 2175, 847,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+ /* half turns */
+ halfturn(2175, 1347, 2175, 722,
+ 2625, 722, 2625, 1597,
+ RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
+ halfturn(2625, 847, 2625, 1722,
+ 2175, 1722, 2175, 1097,
+ RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
+
+ /* easy turns */
+ line2line(2175, 1597, 2175, 1097,
+ 2175, 1097, 1500, 722,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+ line2line(2175, 1097, 1500, 722,
+ 1500, 722, 1050, 972,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+ line2line(1500, 722, 1050, 972,
+ 1050, 972, 375, 597,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+ /* hard turn */
+ line2line(1050, 972, 375, 597,
+ 375, 597, 375, 1097,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /****** PASS2 */
+
+ /* easy turn */
+ line2line(375, 597, 375, 1097,
+ 375, 1097, 1050, 1472,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+ /* hard turn */
+ line2line(375, 1097, 1050, 1472,
+ 1050, 1472, 375, 1847,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /* hard turn */
+ line2line(1050, 1472, 375, 1847,
+ 375, 1847, 375, 1347,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /* easy turn */
+ line2line(375, 1847, 375, 1347,
+ 375, 1347, 1050, 972,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+ /* hard turn */
+ line2line(375, 1347, 1050, 972,
+ 1050, 972, 375, 597,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /* hard turn */
+ line2line(1050, 972, 375, 597,
+ 375, 597, 375, 1847,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ }
+ trajectory_hardstop(&mainboard.traj);
}
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