X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Fhostsim%2Fcommands_mainboard.c;h=ff808c31aafad73f34c7993da3ca5a20d8c2a0aa;hp=3b2d82b33ab3111eae48b49d0df3899656e0c276;hb=d1cc3debac68c0012f2c5ca33da9bcf0882f4988;hpb=9fdc9cd2902ed50ebefe0b3c9309d4f173d65e2a

diff --git a/projects/microb2010/tests/hostsim/commands_mainboard.c b/projects/microb2010/tests/hostsim/commands_mainboard.c
index 3b2d82b..ff808c3 100644
--- a/projects/microb2010/tests/hostsim/commands_mainboard.c
+++ b/projects/microb2010/tests/hostsim/commands_mainboard.c
@@ -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,24 +2249,12 @@ 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;
-	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 = {
@@ -2238,7 +2263,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,
 	},
 };