X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Fhostsim%2Fcommands_mainboard.c;h=02382575e6ddb5182687997cc25820f5e70c6560;hp=0de0b2ded2b658be6b3173f854d771a147cb5af5;hb=a15451f8535228e9193b5dc042929ca0a7f79d9c;hpb=87ccd3af8abb0da3e0fa98dc8e9216fc7b676f97 diff --git a/projects/microb2010/tests/hostsim/commands_mainboard.c b/projects/microb2010/tests/hostsim/commands_mainboard.c index 0de0b2d..0238257 100644 --- a/projects/microb2010/tests/hostsim/commands_mainboard.c +++ b/projects/microb2010/tests/hostsim/commands_mainboard.c @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include @@ -296,15 +296,15 @@ static void cmd_start_parsed(void *parsed_result, void *data) 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")); @@ -324,7 +324,7 @@ static void cmd_start_parsed(void *parsed_result, void *data) 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); @@ -560,15 +560,15 @@ static void cmd_color_parsed(void *parsed_result, void *data) 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 @@ -576,7 +576,7 @@ static void cmd_color_parsed(void *parsed_result, void *data) 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"; @@ -1427,7 +1427,7 @@ struct cmd_pickup_test_result { int16_t dist; }; -/* return red or green sensor */ +/* return yellow or blue sensor */ #define COLOR_IR_SENSOR() \ ({ \ uint8_t __ret = 0; \ @@ -2197,13 +2197,50 @@ parse_pgm_inst_t cmd_beacon_opp_dump = { }; #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, @@ -2212,16 +2249,52 @@ void circle_get_da_speed_from_radius(struct trajectory *traj, /* 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; + uint8_t err; 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); + +#if 0 + strat_set_speed(500, 500); + circle_get_da_speed_from_radius(&mainboard.traj, 225, &d, &a); + trajectory_line_abs(&mainboard.traj, + 375, 347, + 375, 1847, 100.); + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(945), + TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(d, a); + trajectory_d_a_rel(&mainboard.traj, 350, -61); + + err = WAIT_COND_OR_TRAJ_END(x_is_more_than(600), + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + trajectory_line_abs(&mainboard.traj, + 375, 1097, + 1725, 1847, 100.); +#else + strat_set_speed(500, 500); + circle_get_da_speed_from_radius(&mainboard.traj, 225, &d, &a); + trajectory_line_abs(&mainboard.traj, + 375, 347, + 375, 1847, 100.); + err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1445), + TRAJ_FLAGS_NO_NEAR); + /* circle */ + strat_set_speed(d, a); + trajectory_d_a_rel(&mainboard.traj, 700, -121); + + err = WAIT_COND_OR_TRAJ_END(x_is_more_than(750), + TRAJ_FLAGS_NO_NEAR); + + strat_set_speed(500, 500); + trajectory_line_abs(&mainboard.traj, + 825, 1596, + 1950, 972, 100.); +#endif } 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 = { @@ -2230,7 +2303,6 @@ 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, }, };