From c1e5f415846c14626b8b46d8a4b113b33f476b3c Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 30 Jan 2010 00:30:59 +0100 Subject: [PATCH] other proto for circle --- .../trajectory_manager_core.c | 131 +++++++++--------- projects/microb2009/mainboard/commands.c | 2 + projects/microb2010/tests/hostsim/cmdline.c | 6 +- projects/microb2010/tests/hostsim/commands.c | 2 + .../microb2010/tests/hostsim/commands_gen.c | 2 +- .../tests/hostsim/commands_mainboard.c | 38 +++++ projects/microb2010/tests/hostsim/cs.c | 2 +- projects/microb2010/tests/hostsim/display.py | 4 +- projects/microb2010/tests/hostsim/main.c | 2 +- 9 files changed, 116 insertions(+), 73 deletions(-) diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index caa986c..f47068c 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -475,6 +475,33 @@ void trajectory_manager_xy_event(struct trajectory *traj) cs_set_consign(traj->csm_distance, d_consign); } +/* + * Compute the fastest distance and angle speeds matching the radius + * from current traj_speed + */ +/* static */void circle_get_da_speed_from_radius(struct trajectory *traj, + double radius_mm, + double *speed_d, + double *speed_a) +{ + /* speed_d = coef * speed_a */ + double coef; + double speed_d2, speed_a2; + + coef = 2. * radius_mm / traj->position->phys.track_mm; + + speed_d2 = traj->a_speed * coef; + if (speed_d2 < traj->d_speed) { + *speed_d = speed_d2; + *speed_a = traj->a_speed; + } + else { + speed_a2 = traj->d_speed / coef; + *speed_d = traj->d_speed; + *speed_a = speed_a2; + } +} + /* trajectory event for circles */ /* XXX static */ void trajectory_manager_circle_event(struct trajectory *traj) @@ -485,6 +512,8 @@ void trajectory_manager_circle_event(struct trajectory *traj) double a = position_get_a_rad_double(traj->position); int32_t d_consign = 0, a_consign = 0; double angle_to_center_rad; + double coef_p, coef_d; + double d_speed, a_speed; /* These vectors contain target position of the robot in * its own coordinates */ @@ -500,58 +529,57 @@ void trajectory_manager_circle_event(struct trajectory *traj) vect2_cart2pol(&v2cart_pos, &v2pol_target); v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); - /* wanted direction of center of circle: - * if we are far, go in the center direction, - * if we are at radius, we want to see the center at 90° - * if we are nearer than radius, angle to center is > 90° */ + /* radius consign */ radius = traj->target.circle.radius; - if (v2pol_target.r > radius) { - angle_to_center_rad = radius / v2pol_target.r; - angle_to_center_rad *= (M_PI / 2); - } - else { - angle_to_center_rad = 1. - (v2pol_target.r / - (2 * radius)); - angle_to_center_rad *= M_PI; - } - /* XXX check flags */ - v2pol_target.theta -= angle_to_center_rad; + coef_p = v2pol_target.r / radius; + coef_p = 1. * coef_p; - /* If the robot is correctly oriented to start moving in distance */ - /* here limit dist speed depending on v2pol_target.theta */ - if (ABS(v2pol_target.theta) > traj->a_start_rad) - set_quadramp_speed(traj, 0, traj->a_speed); - else { - set_quadramp_speed(traj, traj->d_speed, traj->a_speed); - } + angle_to_center_rad = v2pol_target.theta - (M_PI / 2.); + angle_to_center_rad = simple_modulo_2pi(angle_to_center_rad); + if (angle_to_center_rad > 0.5) + angle_to_center_rad = 0.5; + if (angle_to_center_rad < -0.5) + angle_to_center_rad = -0.5; + coef_d = exp(5*angle_to_center_rad); + coef_d = coef_d; + + circle_get_da_speed_from_radius(traj, radius / (coef_p * coef_d), + &d_speed, &a_speed); + set_quadramp_speed(traj, d_speed, a_speed); + + EVT_DEBUG(E_TRAJECTORY, "angle=%2.2f radius=%2.2f r=%2.2f coef_p=%2.2f coef_d=%2.2f " + "d_speed=%2.2f a_speed=%2.2f", + angle_to_center_rad, radius, v2pol_target.r, + coef_p, coef_d, d_speed, a_speed); /* XXX check flags */ - d_consign = 40000 + rs_get_distance(traj->robot); + d_consign = 400000 + rs_get_distance(traj->robot); + a_consign = 400000 + rs_get_angle(traj->robot); /* angle consign */ - a_consign = (int32_t)(v2pol_target.theta * - (traj->position->phys.distance_imp_per_mm) * - (traj->position->phys.track_mm) / 2.0); - a_consign += rs_get_angle(traj->robot); +/* a_consign = (int32_t)(v2pol_target.theta * */ +/* (traj->position->phys.distance_imp_per_mm) * */ +/* (traj->position->phys.track_mm) / 2.0); */ +/* a_consign += rs_get_angle(traj->robot); */ /* step 2 : update state, or delete event if we reached the * destination */ - /* output angle -> delete event */ - if (a_consign >= traj->target.circle.dest_angle) { - a_consign = traj->target.circle.dest_angle; - delete_event(traj); - } +/* /\* output angle -> delete event *\/ */ +/* if (a_consign >= traj->target.circle.dest_angle) { */ +/* a_consign = traj->target.circle.dest_angle; */ +/* delete_event(traj); */ +/* } */ /* step 3 : send the processed commands to cs */ - EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 - ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 - ", a_speed=%" PRIi32 "radius = %f", - rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), - rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj), - radius); +/* EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 */ +/* ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 */ +/* ", a_speed=%" PRIi32 ", radius = %f", */ +/* rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), */ +/* rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj), */ +/* radius); */ cs_set_consign(traj->csm_angle, a_consign); cs_set_consign(traj->csm_distance, d_consign); @@ -620,33 +648,6 @@ void trajectory_circle_rel(struct trajectory *traj, schedule_event(traj); } -/* - * Compute the fastest distance and angle speeds matching the radius - * from current traj_speed - */ -/* static */void circle_get_da_speed_from_radius(struct trajectory *traj, - double radius_mm, - double *speed_d, - double *speed_a) -{ - /* speed_d = coef * speed_a */ - double coef; - double speed_d2, speed_a2; - - coef = 2. * radius_mm / traj->position->phys.track_mm; - - speed_d2 = traj->a_speed * coef; - if (speed_d2 < traj->d_speed) { - *speed_d = speed_d2; - *speed_a = traj->a_speed; - } - else { - speed_a2 = traj->d_speed / coef; - *speed_d = traj->d_speed; - *speed_a = speed_a2; - } -} - /* return the distance in millimeters that corresponds to an angle in * degree and a radius in mm */ /* static */double circle_get_dist_from_degrees(double radius_mm, double a_deg) diff --git a/projects/microb2009/mainboard/commands.c b/projects/microb2009/mainboard/commands.c index bbcaacf..25a4745 100644 --- a/projects/microb2009/mainboard/commands.c +++ b/projects/microb2009/mainboard/commands.c @@ -92,6 +92,7 @@ extern parse_pgm_inst_t cmd_build_z1; #ifdef TEST_BEACON extern parse_pgm_inst_t cmd_beacon_opp_dump; #endif +extern parse_pgm_inst_t cmd_circle_radius; extern parse_pgm_inst_t cmd_test; /* commands_traj.c */ @@ -193,6 +194,7 @@ parse_pgm_ctx_t main_ctx[] = { #ifdef TEST_BEACON (parse_pgm_inst_t *)&cmd_beacon_opp_dump, #endif + (parse_pgm_inst_t *)&cmd_circle_radius, (parse_pgm_inst_t *)&cmd_test, /* commands_traj.c */ diff --git a/projects/microb2010/tests/hostsim/cmdline.c b/projects/microb2010/tests/hostsim/cmdline.c index 77392ec..edf7601 100644 --- a/projects/microb2010/tests/hostsim/cmdline.c +++ b/projects/microb2010/tests/hostsim/cmdline.c @@ -115,7 +115,7 @@ void mylog(struct error * e, ...) if (e->severity > ERROR_SEVERITY_ERROR) { if (gen.log_level < e->severity) return; - + for (i=0; ierr_num) break; @@ -126,12 +126,12 @@ void mylog(struct error * e, ...) va_start(ap, e); tv = time_get_time(); printf_P(PSTR("%d.%.3d: "), (int)tv.s, (int)(tv.us/1000UL)); - + printf_P(PSTR("(%d,%d,%d) "), position_get_x_s16(&mainboard.pos), position_get_y_s16(&mainboard.pos), position_get_a_deg_s16(&mainboard.pos)); - + vfprintf_P(stdout, e->text, ap); printf_P(PSTR("\r\n")); va_end(ap); diff --git a/projects/microb2010/tests/hostsim/commands.c b/projects/microb2010/tests/hostsim/commands.c index cb620ca..140ef29 100644 --- a/projects/microb2010/tests/hostsim/commands.c +++ b/projects/microb2010/tests/hostsim/commands.c @@ -92,6 +92,7 @@ extern parse_pgm_inst_t cmd_build_z1; #ifdef TEST_BEACON extern parse_pgm_inst_t cmd_beacon_opp_dump; #endif +extern parse_pgm_inst_t cmd_circle_radius; extern parse_pgm_inst_t cmd_test; /* commands_traj.c */ @@ -193,6 +194,7 @@ parse_pgm_ctx_t main_ctx[] = { #ifdef TEST_BEACON (parse_pgm_inst_t *)&cmd_beacon_opp_dump, #endif + (parse_pgm_inst_t *)&cmd_circle_radius, (parse_pgm_inst_t *)&cmd_test, /* commands_traj.c */ diff --git a/projects/microb2010/tests/hostsim/commands_gen.c b/projects/microb2010/tests/hostsim/commands_gen.c index 92f8969..f4fd855 100644 --- a/projects/microb2010/tests/hostsim/commands_gen.c +++ b/projects/microb2010/tests/hostsim/commands_gen.c @@ -471,7 +471,7 @@ prog_char str_log_arg0[] = "log"; parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0); prog_char str_log_arg1[] = "level"; parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1); -parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32); +parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT8); prog_char help_log[] = "Set log options: level (0 -> 5)"; parse_pgm_inst_t cmd_log = { diff --git a/projects/microb2010/tests/hostsim/commands_mainboard.c b/projects/microb2010/tests/hostsim/commands_mainboard.c index 3b2d82b..ca84489 100644 --- a/projects/microb2010/tests/hostsim/commands_mainboard.c +++ b/projects/microb2010/tests/hostsim/commands_mainboard.c @@ -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 */ diff --git a/projects/microb2010/tests/hostsim/cs.c b/projects/microb2010/tests/hostsim/cs.c index 9766ea9..678ebb8 100644 --- a/projects/microb2010/tests/hostsim/cs.c +++ b/projects/microb2010/tests/hostsim/cs.c @@ -168,7 +168,7 @@ void microb_cs_init(void) position_init(&mainboard.pos); position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM); position_set_related_robot_system(&mainboard.pos, &mainboard.rs); - //position_set_centrifugal_coef(&mainboard.pos, 0.000016); + position_set_centrifugal_coef(&mainboard.pos, 0.000016); position_use_ext(&mainboard.pos); /* TRAJECTORY MANAGER */ diff --git a/projects/microb2010/tests/hostsim/display.py b/projects/microb2010/tests/hostsim/display.py index d21cc9e..fac65cf 100644 --- a/projects/microb2010/tests/hostsim/display.py +++ b/projects/microb2010/tests/hostsim/display.py @@ -33,7 +33,7 @@ def square(sz): return sq sq1 = square(250) -sq1 = square(500) +sq2 = square(500) robot_trail = curve() robot_trail_list = [] @@ -49,7 +49,7 @@ def set_robot(x, y, a): 0) # save position - save_pos.append(robot.pos.x, robot.pos, a) + save_pos.append((robot.pos.x, robot.pos, a)) pos = robot.pos.x, robot.pos.y, 0.3 if pos != last_pos: diff --git a/projects/microb2010/tests/hostsim/main.c b/projects/microb2010/tests/hostsim/main.c index 31a7832..d36fa5f 100644 --- a/projects/microb2010/tests/hostsim/main.c +++ b/projects/microb2010/tests/hostsim/main.c @@ -94,7 +94,7 @@ int main(void) mainboard.flags = DO_ENCODERS | DO_RS | DO_POS | DO_POWER | DO_BD | DO_CS; - strat_reset_pos(1000, 1000, 0); + strat_reset_pos(1000, 1000, -90); cmdline_interact(); -- 2.20.1