From e0908eab4d4b00cf19083f91a9921b3e8d0cae63 Mon Sep 17 00:00:00 2001 From: zer0 Date: Wed, 20 Jan 2010 00:16:02 +0100 Subject: [PATCH] command circle set coef --- .../trajectory_manager/trajectory_manager.c | 12 +++- .../trajectory_manager/trajectory_manager.h | 11 +++- .../trajectory_manager_core.c | 47 +++++++++++++-- projects/microb2009/mainboard/commands.c | 4 ++ projects/microb2009/mainboard/commands_traj.c | 59 +++++++++++++++++++ projects/microb2009/microb_cmd/microbcmd.py | 34 +++++++++++ projects/microb2010/mainboard/commands.c | 4 ++ projects/microb2010/mainboard/commands_traj.c | 59 +++++++++++++++++++ 8 files changed, 219 insertions(+), 11 deletions(-) diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.c b/modules/devices/robot/trajectory_manager/trajectory_manager.c index 5fe3865..773388e 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.c @@ -77,7 +77,7 @@ void trajectory_set_robot_params(struct trajectory *traj, } /** set speed consign */ -void trajectory_set_speed( struct trajectory *traj, int16_t d_speed, int16_t a_speed) +void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed) { uint8_t flags; IRQ_LOCK(flags); @@ -98,5 +98,11 @@ void trajectory_set_windows(struct trajectory *traj, double d_win, IRQ_UNLOCK(flags); } - - +/** set corrective coef for circle */ +void trajectory_set_circle_coef(struct trajectory *traj, double coef) +{ + uint8_t flags; + IRQ_LOCK(flags); + traj->circle_coef = coef ; + IRQ_UNLOCK(flags); +} diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index 3ebd586..f18e272 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -70,7 +70,8 @@ struct trajectory { double a_win_rad; /**<< angle window (for END_NEAR) */ double a_start_rad;/**<< in xy consigns, start to move in distance * when a_target < a_start */ - + double circle_coef;/**<< corrective circle coef */ + uint16_t d_speed; /**<< distance speed consign */ uint16_t a_speed; /**<< angle speed consign */ @@ -78,7 +79,7 @@ struct trajectory { struct robot_system *robot; /**<< associated robot_system */ struct cs *csm_angle; /**<< associated control system (angle) */ struct cs *csm_distance; /**<< associated control system (distance) */ - + int8_t scheduler_task; /**<< id of current task (-1 if no running task) */ }; @@ -108,6 +109,12 @@ void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_sp void trajectory_set_windows(struct trajectory *traj, double d_win, double a_win_deg, double a_start_deg); +/** + * Set coef for circle trajectory. The objective of this value is to + * fix the radius of the circle which is not correctly what we asked. + */ +void trajectory_set_circle_coef(struct trajectory *traj, double coef); + /** return true if the position consign is equal to the filtered * position consign (after quadramp filter), for angle and * distance. */ diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index cc2a58a..85b2e57 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -485,12 +485,32 @@ 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; + static int32_t d_prev, a_prev; + int32_t d_speed, a_speed; + int32_t d_pos, a_pos; + + d_pos = rs_get_distance(traj->robot); + a_pos = rs_get_angle(traj->robot); + d_speed = d_pos - d_prev; + a_speed = a_pos - a_prev; + d_prev = d_pos; + a_prev = a_pos; /* These vectors contain target position of the robot in * its own coordinates */ vect2_cart v2cart_pos; vect2_pol v2pol_target; + int32_t delta_d, delta_a; + double coef_deriv = traj->circle_coef; + double new_radius; + struct quadramp_filter * q_d, * q_a; + + q_d = traj->csm_distance->consign_filter_params; + q_a = traj->csm_angle->consign_filter_params; + delta_a = a_speed;//q_a->previous_var; + delta_d = d_speed;//q_d->previous_var; + /* step 1 : process new commands to quadramps */ /* process the command vector from current position to the @@ -500,17 +520,31 @@ void trajectory_manager_circle_event(struct trajectory *traj) vect2_cart2pol(&v2cart_pos, &v2pol_target); v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); + /* pas trop mal, mais oscille */ + //new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv; + if (v2pol_target.r > traj->target.circle.radius/2) + new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv * traj->target.circle.radius / v2pol_target.r; + else + new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv ; + + /* oscille a mort */ + //new_radius = traj->target.circle.radius - traj->target.circle.radius * delta_a * delta_a * coef_deriv; + + /* ? */ + //new_radius = traj->target.circle.radius - traj->target.circle.radius * delta_a * coef_deriv; + + /* 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° */ - if (v2pol_target.r > traj->target.circle.radius) { - angle_to_center_rad = traj->target.circle.radius / v2pol_target.r; + if (v2pol_target.r > new_radius) { + angle_to_center_rad = new_radius / v2pol_target.r; angle_to_center_rad *= (M_PI / 2); } else { angle_to_center_rad = 1. - (v2pol_target.r / - (2 * traj->target.circle.radius)); + (2 * new_radius)); angle_to_center_rad *= M_PI; } @@ -548,9 +582,10 @@ void trajectory_manager_circle_event(struct trajectory *traj) EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 - ", a_speed=%" PRIi32, - rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), - rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj)); + ", 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), + new_radius); cs_set_consign(traj->csm_angle, a_consign); cs_set_consign(traj->csm_distance, d_consign); diff --git a/projects/microb2009/mainboard/commands.c b/projects/microb2009/mainboard/commands.c index 61fa530..bbcaacf 100644 --- a/projects/microb2009/mainboard/commands.c +++ b/projects/microb2009/mainboard/commands.c @@ -99,6 +99,8 @@ extern parse_pgm_inst_t cmd_traj_speed; extern parse_pgm_inst_t cmd_traj_speed_show; extern parse_pgm_inst_t cmd_trajectory; extern parse_pgm_inst_t cmd_trajectory_show; +extern parse_pgm_inst_t cmd_circle_coef; +extern parse_pgm_inst_t cmd_circle_coef_show; extern parse_pgm_inst_t cmd_rs_gains; extern parse_pgm_inst_t cmd_rs_gains_show; extern parse_pgm_inst_t cmd_track; @@ -198,6 +200,8 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_traj_speed_show, (parse_pgm_inst_t *)&cmd_trajectory, (parse_pgm_inst_t *)&cmd_trajectory_show, + (parse_pgm_inst_t *)&cmd_circle_coef, + (parse_pgm_inst_t *)&cmd_circle_coef_show, (parse_pgm_inst_t *)&cmd_rs_gains, (parse_pgm_inst_t *)&cmd_rs_gains_show, (parse_pgm_inst_t *)&cmd_track, diff --git a/projects/microb2009/mainboard/commands_traj.c b/projects/microb2009/mainboard/commands_traj.c index 82477a3..170dce8 100644 --- a/projects/microb2009/mainboard/commands_traj.c +++ b/projects/microb2009/mainboard/commands_traj.c @@ -125,6 +125,65 @@ parse_pgm_inst_t cmd_traj_speed_show = { }, }; +/**********************************************************/ +/* circle coef configuration */ + +/* this structure is filled when cmd_circle_coef is parsed successfully */ +struct cmd_circle_coef_result { + fixed_string_t arg0; + fixed_string_t arg1; + float circle_coef; +}; + + +/* function called when cmd_circle_coef is parsed successfully */ +static void cmd_circle_coef_parsed(void *parsed_result, void *data) +{ + struct cmd_circle_coef_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + trajectory_set_circle_coef(&mainboard.traj, res->circle_coef); + } + + printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef); +} + +prog_char str_circle_coef_arg0[] = "circle_coef"; +parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0); +prog_char str_circle_coef_arg1[] = "set"; +parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1); +parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT); + +prog_char help_circle_coef[] = "Set circle coef"; +parse_pgm_inst_t cmd_circle_coef = { + .f = cmd_circle_coef_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_circle_coef, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_circle_coef_arg0, + (prog_void *)&cmd_circle_coef_arg1, + (prog_void *)&cmd_circle_coef_val, + NULL, + }, +}; + +/* show */ + +prog_char str_circle_coef_show_arg[] = "show"; +parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg); + +prog_char help_circle_coef_show[] = "Show circle coef"; +parse_pgm_inst_t cmd_circle_coef_show = { + .f = cmd_circle_coef_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_circle_coef_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_circle_coef_arg0, + (prog_void *)&cmd_circle_coef_show_arg, + NULL, + }, +}; + /**********************************************************/ /* trajectory window configuration */ diff --git a/projects/microb2009/microb_cmd/microbcmd.py b/projects/microb2009/microb_cmd/microbcmd.py index 5080927..ee0dbaa 100755 --- a/projects/microb2009/microb_cmd/microbcmd.py +++ b/projects/microb2009/microb_cmd/microbcmd.py @@ -12,6 +12,7 @@ import numpy import shlex import time import math +import re import warnings warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__) @@ -899,6 +900,39 @@ class Interp(cmd.Cmd): time.sleep(1) self.ser.write("pwm s3(3C) 250\n") + def do_circle(self, arg): + arg = re.sub(" *", " ", arg) + arg = arg.strip() + arg = arg.split(" ") + if len(arg)!=4: + print "radius coef dspeed aspeed" + return + r = int(arg[0]) + c = float(arg[1]) + dspeed = int(arg[2]) + aspeed = int(arg[3]) + + self.ser.write("event cs on\n") + time.sleep(0.3) + + self.ser.write("traj_speed distance %d\n"%dspeed) + time.sleep(0.3) + self.ser.write("traj_speed angle %d\n"%aspeed) + time.sleep(0.3) + + self.ser.write("goto d_rel 300\n") + time.sleep(3) + + self.ser.write("position reset\n") + time.sleep(0.3) + self.ser.write("circle_coef set %f\n"%c) + time.sleep(0.3) + + self.ser.write("goto circle_rel 700 460 %d 360\n"%r) + self.ser.flushInput() + time.sleep(0.3) + + if __name__ == "__main__": try: import readline,atexit diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index 107606e..26060d6 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -99,6 +99,8 @@ extern parse_pgm_inst_t cmd_traj_speed; extern parse_pgm_inst_t cmd_traj_speed_show; extern parse_pgm_inst_t cmd_trajectory; extern parse_pgm_inst_t cmd_trajectory_show; +extern parse_pgm_inst_t cmd_circle_coef; +extern parse_pgm_inst_t cmd_circle_coef_show; extern parse_pgm_inst_t cmd_rs_gains; extern parse_pgm_inst_t cmd_rs_gains_show; extern parse_pgm_inst_t cmd_track; @@ -197,6 +199,8 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_traj_speed_show, (parse_pgm_inst_t *)&cmd_trajectory, (parse_pgm_inst_t *)&cmd_trajectory_show, + (parse_pgm_inst_t *)&cmd_circle_coef, + (parse_pgm_inst_t *)&cmd_circle_coef_show, (parse_pgm_inst_t *)&cmd_rs_gains, (parse_pgm_inst_t *)&cmd_rs_gains_show, (parse_pgm_inst_t *)&cmd_track, diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index f3b38fd..ea124d3 100644 --- a/projects/microb2010/mainboard/commands_traj.c +++ b/projects/microb2010/mainboard/commands_traj.c @@ -125,6 +125,65 @@ parse_pgm_inst_t cmd_traj_speed_show = { }, }; +/**********************************************************/ +/* circle coef configuration */ + +/* this structure is filled when cmd_circle_coef is parsed successfully */ +struct cmd_circle_coef_result { + fixed_string_t arg0; + fixed_string_t arg1; + float circle_coef; +}; + + +/* function called when cmd_circle_coef is parsed successfully */ +static void cmd_circle_coef_parsed(void *parsed_result, void *data) +{ + struct cmd_circle_coef_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("set"))) { + trajectory_set_circle_coef(&mainboard.traj, res->circle_coef); + } + + printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef); +} + +prog_char str_circle_coef_arg0[] = "circle_coef"; +parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0); +prog_char str_circle_coef_arg1[] = "set"; +parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1); +parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT); + +prog_char help_circle_coef[] = "Set circle coef"; +parse_pgm_inst_t cmd_circle_coef = { + .f = cmd_circle_coef_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_circle_coef, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_circle_coef_arg0, + (prog_void *)&cmd_circle_coef_arg1, + (prog_void *)&cmd_circle_coef_val, + NULL, + }, +}; + +/* show */ + +prog_char str_circle_coef_show_arg[] = "show"; +parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg); + +prog_char help_circle_coef_show[] = "Show circle coef"; +parse_pgm_inst_t cmd_circle_coef_show = { + .f = cmd_circle_coef_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_circle_coef_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_circle_coef_arg0, + (prog_void *)&cmd_circle_coef_show_arg, + NULL, + }, +}; + /**********************************************************/ /* trajectory window configuration */ -- 2.20.1