X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Ftests%2Fhostsim%2Fcommands_mainboard.c;h=ca84489a004813a9073acdedf8db65890268d951;hp=0de0b2ded2b658be6b3173f854d771a147cb5af5;hb=1714f4ee916fca95ce24120ea6e698237913f947;hpb=87ccd3af8abb0da3e0fa98dc8e9216fc7b676f97

diff --git a/projects/microb2010/tests/hostsim/commands_mainboard.c b/projects/microb2010/tests/hostsim/commands_mainboard.c
index 0de0b2d..ca84489 100644
--- a/projects/microb2010/tests/hostsim/commands_mainboard.c
+++ b/projects/microb2010/tests/hostsim/commands_mainboard.c
@@ -30,7 +30,7 @@
 #include <aversive/eeprom.h>
 
 #include <uart.h>
-#include <time.h>
+#include <clock_time.h>
 
 #include <pid.h>
 #include <quadramp.h>
@@ -2197,6 +2197,44 @@ 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 */
 
@@ -2214,9 +2252,17 @@ 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);
-	printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), 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);
 }
 
 prog_char str_test_arg0[] = "test";