From: zer0 <zer0@carbon.local>
Date: Sat, 9 Jan 2010 16:38:30 +0000 (+0100)
Subject: test circle and test 1000pts encoder
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=fca037e03b1b0fe462ad19e848cd77fce7719e3c;p=aversive.git

test circle and test 1000pts encoder
---

diff --git a/projects/microb2009/mainboard/.config b/projects/microb2009/mainboard/.config
index 18a0d9a..21ce9e5 100644
--- a/projects/microb2009/mainboard/.config
+++ b/projects/microb2009/mainboard/.config
@@ -1,5 +1,5 @@
 #
-# Automatically generated by make menuconfig: don't edit
+# Automatically generated make config: don't edit
 #
 
 #
@@ -74,6 +74,10 @@ CONFIG_FORMAT_BINARY=y
 #
 # Base modules
 #
+
+#
+# Enable math library in generation options to see all modules
+#
 CONFIG_MODULE_CIRBUF=y
 # CONFIG_MODULE_CIRBUF_LARGE is not set
 CONFIG_MODULE_FIXED_POINT=y
@@ -93,6 +97,10 @@ CONFIG_MODULE_TIME=y
 #
 # Communication modules
 #
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
 CONFIG_MODULE_UART=y
 # CONFIG_MODULE_UART_9BITS is not set
 CONFIG_MODULE_UART_CREATE_CONFIG=y
@@ -161,9 +169,10 @@ CONFIG_MODULE_ENCODERS_SPI=y
 CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y
 
 #
-# Robot specific modules
+# Robot specific modules (fixed point lib may be needed)
 #
 CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
 # CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
 CONFIG_MODULE_POSITION_MANAGER=y
 CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
@@ -176,6 +185,10 @@ CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG=y
 # Control system modules
 #
 CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
+
+#
+# Filters
+#
 CONFIG_MODULE_PID=y
 # CONFIG_MODULE_PID_CREATE_CONFIG is not set
 # CONFIG_MODULE_RAMP is not set
@@ -186,12 +199,20 @@ CONFIG_MODULE_QUADRAMP=y
 #
 # Radio devices
 #
+
+#
+# Some radio devices require SPI to be activated
+#
 # CONFIG_MODULE_CC2420 is not set
 # CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
 
 #
 # Crypto modules
 #
+
+#
+# Crypto modules depend on utils module
+#
 # CONFIG_MODULE_AES is not set
 # CONFIG_MODULE_AES_CTR is not set
 # CONFIG_MODULE_MD5 is not set
@@ -201,12 +222,20 @@ CONFIG_MODULE_QUADRAMP=y
 #
 # Encodings modules
 #
+
+#
+# Encoding modules depend on utils module
+#
 # CONFIG_MODULE_BASE64 is not set
 # CONFIG_MODULE_HAMMING is not set
 
 #
 # Debug modules
 #
+
+#
+# Debug modules depend on utils module
+#
 CONFIG_MODULE_DIAGNOSTIC=y
 CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y
 CONFIG_MODULE_ERROR=y
diff --git a/projects/microb2009/mainboard/commands_mainboard.c b/projects/microb2009/mainboard/commands_mainboard.c
index b379dc1..62c7316 100644
--- a/projects/microb2009/mainboard/commands_mainboard.c
+++ b/projects/microb2009/mainboard/commands_mainboard.c
@@ -2116,10 +2116,10 @@ parse_pgm_inst_t cmd_beacon_opp_dump = {
 #endif
 
 /**********************************************************/
-/* Test */
+/* Circle_Radius */
 
-/* this structure is filled when cmd_test is parsed successfully */
-struct cmd_test_result {
+/* this structure is filled when cmd_circle_radius is parsed successfully */
+struct cmd_circle_radius_result {
 	fixed_string_t arg0;
 	int32_t radius;
 };
@@ -2127,19 +2127,53 @@ 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)
+/* function called when cmd_circle_radius is parsed successfully */
+static void cmd_circle_radius_parsed(void *parsed_result, void *data)
 {
-	struct cmd_test_result *res = parsed_result;
+	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;
+	uint32_t mask;
+};
+
+extern uint32_t encmask;
+
+/* 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;
+	encmask = res->mask;
+}
+
 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_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, mask, UINT32);
 
 prog_char help_test[] = "Test function";
 parse_pgm_inst_t cmd_test = {
diff --git a/projects/microb2009/mainboard/commands_traj.c b/projects/microb2009/mainboard/commands_traj.c
index f3b38fd..715f167 100644
--- a/projects/microb2009/mainboard/commands_traj.c
+++ b/projects/microb2009/mainboard/commands_traj.c
@@ -215,9 +215,9 @@ static void cmd_rs_gains_parsed(void * parsed_result, void * data)
 					 RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
 	}
 	printf_P(PSTR("rs_gains set "));
-	f64_print(mainboard.rs.left_ext_gain);
+	//f64_print(mainboard.rs.left_ext_gain);
 	printf_P(PSTR(" "));
-	f64_print(mainboard.rs.right_ext_gain);
+	//f64_print(mainboard.rs.right_ext_gain);
 	printf_P(PSTR("\r\n"));
 }
 
diff --git a/projects/microb2009/mainboard/cs.c b/projects/microb2009/mainboard/cs.c
index ec70f9a..1fb8346 100644
--- a/projects/microb2009/mainboard/cs.c
+++ b/projects/microb2009/mainboard/cs.c
@@ -136,6 +136,13 @@ void dump_cs(const char *name, struct cs *cs)
 		 cs_get_out(cs));
 }
 
+uint32_t encmask = 0xFFFFFFFF;
+int32_t my_encoders_get_value(void *enc)
+{
+	int32_t tmp = encoders_spi_get_value(enc);
+	return tmp & encmask;
+}
+
 void dump_pid(const char *name, struct pid_filter *pid)
 {
 	printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
@@ -153,9 +160,9 @@ void microb_cs_init(void)
 	rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM);
 	rs_set_right_pwm(&mainboard.rs,  pwm_set_and_save, RIGHT_PWM);
 	/* increase gain to decrease dist, increase left and it will turn more left */
-	rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
+	rs_set_left_ext_encoder(&mainboard.rs, my_encoders_get_value, 
 				LEFT_ENCODER, IMP_COEF * 1.0015);
-	rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
+	rs_set_right_ext_encoder(&mainboard.rs, my_encoders_get_value, 
 				 RIGHT_ENCODER, IMP_COEF * -1.006);
 	/* rs will use external encoders */
 	rs_set_flags(&mainboard.rs, RS_USE_EXT);
diff --git a/projects/microb2009/mainboard/strat_utils.c b/projects/microb2009/mainboard/strat_utils.c
index 81a12d6..37f2b37 100644
--- a/projects/microb2009/mainboard/strat_utils.c
+++ b/projects/microb2009/mainboard/strat_utils.c
@@ -86,18 +86,6 @@ int16_t simple_modulo_360(int16_t a)
 	return a;
 }
 
-/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */  
-double simple_modulo_2pi(double a)
-{
-	if (a < -M_PI) {
-		a += M_2PI;
-	}
-	else if (a > M_PI) {
-		a -= M_2PI;
-	}
-	return a;
-}
-
 /* return the distance to a point in the area */
 int16_t angle_abs_to_rel(int16_t a_abs)
 {
diff --git a/projects/microb2009/mainboard/strat_utils.h b/projects/microb2009/mainboard/strat_utils.h
index 986b601..c4687bb 100644
--- a/projects/microb2009/mainboard/strat_utils.h
+++ b/projects/microb2009/mainboard/strat_utils.h
@@ -43,7 +43,6 @@ struct xy_point {
 int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
 int16_t distance_from_robot(int16_t x, int16_t y);
 int16_t simple_modulo_360(int16_t a);
-double simple_modulo_2pi(double a);
 int16_t angle_abs_to_rel(int16_t a_abs);
 void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs);
 double norm(double x, double y);