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_MECHBOARD_ADDR, I2C_COLOR_YELLOW);
+ i2c_set_color(I2C_SENSORBOARD_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_MECHBOARD_ADDR, I2C_COLOR_BLUE);
+ i2c_set_color(I2C_SENSORBOARD_ADDR, I2C_COLOR_BLUE);
}
printf_P(PSTR("Check that lintel is loaded\r\n"));
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);
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_MECHBOARD_ADDR, I2C_COLOR_YELLOW);
+ i2c_set_color(I2C_SENSORBOARD_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_MECHBOARD_ADDR, I2C_COLOR_BLUE);
+ i2c_set_color(I2C_SENSORBOARD_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";
int16_t dist;
};
-/* return red or green sensor */
+/* return yellow or blue sensor */
#define COLOR_IR_SENSOR() \
({ \
uint8_t __ret = 0; \
};
#endif
+/**********************************************************/
+/* Circle_Radius */
+
+/* this structure is filled when cmd_circle_radius is parsed successfully */
+struct cmd_circle_radius_result {
+ fixed_string_t arg0;
+ int32_t radius;
+};
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+ double radius_mm,
+ double *speed_d,
+ double *speed_a);
+/* function called when cmd_circle_radius is parsed successfully */
+static void cmd_circle_radius_parsed(void *parsed_result, void *data)
+{
+ struct cmd_circle_radius_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);
+}
+
+prog_char str_circle_radius_arg0[] = "circle_radius";
+parse_pgm_token_string_t cmd_circle_radius_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_radius_result, arg0, str_circle_radius_arg0);
+parse_pgm_token_num_t cmd_circle_radius_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_circle_radius_result, radius, INT32);
+
+prog_char help_circle_radius[] = "Circle_Radius function";
+parse_pgm_inst_t cmd_circle_radius = {
+ .f = cmd_circle_radius_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_radius,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_radius_arg0,
+ (prog_void *)&cmd_circle_radius_arg1,
+ NULL,
+ },
+};
+
/**********************************************************/
/* Test */
/* this structure is filled when cmd_test is parsed successfully */
struct cmd_test_result {
fixed_string_t arg0;
- int32_t radius;
};
void circle_get_da_speed_from_radius(struct trajectory *traj,
double radius_mm,
/* 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;
- uint8_t err;
-
- strat_reset_pos(1000, 500, 0);
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
- circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
- trajectory_d_rel(&mainboard.traj, 1000);
- err = WAIT_COND_OR_TRAJ_END(position_get_x_double(&mainboard.pos) > 1500, 0xFF);
- if (err)
- return;
- strat_set_speed(d, a);
- trajectory_d_a_rel(&mainboard.traj, 10000, 1000);
+ trajectory_line_abs(&mainboard.traj, 0, 1050,
+ 1500, 1050, 100.);
}
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);
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,
},
};