From fca037e03b1b0fe462ad19e848cd77fce7719e3c Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 9 Jan 2010 17:38:30 +0100 Subject: [PATCH 1/1] test circle and test 1000pts encoder --- projects/microb2009/mainboard/.config | 33 ++++++++++++- .../microb2009/mainboard/commands_mainboard.c | 48 ++++++++++++++++--- projects/microb2009/mainboard/commands_traj.c | 4 +- projects/microb2009/mainboard/cs.c | 11 ++++- projects/microb2009/mainboard/strat_utils.c | 12 ----- projects/microb2009/mainboard/strat_utils.h | 1 - 6 files changed, 83 insertions(+), 26 deletions(-) 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); -- 2.20.1