From 91987ff2747a521681d087935148964eed6b3556 Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 9 Jan 2010 17:39:50 +0100 Subject: [PATCH] trajectory manager rework --- .../devices/robot/trajectory_manager/Makefile | 3 +- .../trajectory_manager/trajectory_manager.c | 637 +----------------- .../trajectory_manager_core.c | 585 ++++++++++++++++ .../trajectory_manager_core.h | 113 ++++ .../trajectory_manager_utils.c | 145 ++++ .../trajectory_manager_utils.h | 62 ++ 6 files changed, 916 insertions(+), 629 deletions(-) create mode 100644 modules/devices/robot/trajectory_manager/trajectory_manager_core.c create mode 100644 modules/devices/robot/trajectory_manager/trajectory_manager_core.h create mode 100644 modules/devices/robot/trajectory_manager/trajectory_manager_utils.c create mode 100644 modules/devices/robot/trajectory_manager/trajectory_manager_utils.h diff --git a/modules/devices/robot/trajectory_manager/Makefile b/modules/devices/robot/trajectory_manager/Makefile index a4b4c77..1f986cd 100644 --- a/modules/devices/robot/trajectory_manager/Makefile +++ b/modules/devices/robot/trajectory_manager/Makefile @@ -1,7 +1,8 @@ TARGET = trajectory_manager # List C source files here. (C dependencies are automatically generated.) -SRC = trajectory_manager.c +SRC = trajectory_manager.c trajectory_manager_utils.c +SRC += trajectory_manager_core.c ########################################## diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.c b/modules/devices/robot/trajectory_manager/trajectory_manager.c index b77117f..5fe3865 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.c @@ -1,6 +1,6 @@ -/* +/* * Copyright Droids Corporation, Microb Technology, Eirbot (2005) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -19,7 +19,7 @@ * */ -/* Trajectory Manager v2 - zer0 - for Eurobot 2008 */ +/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */ #include #include @@ -36,13 +36,7 @@ #include #include - -#define M_2PI (2*M_PI) - -#define DEG(x) ((x) * (180.0 / M_PI)) -#define RAD(x) ((x) * (M_PI / 180.0)) - -static void trajectory_manager_event(void *param); +#include "trajectory_manager_utils.h" /************ INIT FUNCS */ @@ -59,11 +53,11 @@ void trajectory_init(struct trajectory *traj) } /** structure initialization */ -void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d, +void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d, struct cs *cs_a) { uint8_t flags; - + IRQ_LOCK(flags); traj->csm_distance = cs_d; traj->csm_angle = cs_a; @@ -71,9 +65,9 @@ void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d, } /** structure initialization */ -void trajectory_set_robot_params(struct trajectory *traj, - struct robot_system *rs, - struct robot_position *pos) +void trajectory_set_robot_params(struct trajectory *traj, + struct robot_system *rs, + struct robot_position *pos) { uint8_t flags; IRQ_LOCK(flags); @@ -104,618 +98,5 @@ void trajectory_set_windows(struct trajectory *traj, double d_win, IRQ_UNLOCK(flags); } -/************ STATIC [ AND USEFUL ] FUNCS */ - -/** set speed consign in quadramp filter */ -static void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed) -{ - struct quadramp_filter * q_d, * q_a; - q_d = traj->csm_distance->consign_filter_params; - q_a = traj->csm_angle->consign_filter_params; - quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed)); - quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed)); -} - -/** get angle speed consign in quadramp filter */ -static uint32_t get_quadramp_angle_speed(struct trajectory *traj) -{ - struct quadramp_filter *q_a; - q_a = traj->csm_angle->consign_filter_params; - return q_a->var_1st_ord_pos; -} - -/** get distance speed consign in quadramp filter */ -static uint32_t get_quadramp_distance_speed(struct trajectory *traj) -{ - struct quadramp_filter *q_d; - q_d = traj->csm_distance->consign_filter_params; - return q_d->var_1st_ord_pos; -} - -/** remove event if any */ -static void delete_event(struct trajectory *traj) -{ - set_quadramp_speed(traj, traj->d_speed, traj->a_speed); - if ( traj->scheduler_task != -1) { - DEBUG(E_TRAJECTORY, "Delete event"); - scheduler_del_event(traj->scheduler_task); - traj->scheduler_task = -1; - } -} - -/** schedule the trajectory event */ -static void schedule_event(struct trajectory *traj) -{ - if ( traj->scheduler_task != -1) { - DEBUG(E_TRAJECTORY, "Schedule event, already scheduled"); - } - else { - traj->scheduler_task = - scheduler_add_periodical_event_priority(&trajectory_manager_event, - (void*)traj, - 100000L/SCHEDULER_UNIT, 30); - } -} - -/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */ -static double simple_modulo_2pi(double a) -{ - if (a < -M_PI) { - a += M_2PI; - } - else if (a > M_PI) { - a -= M_2PI; - } - return a; -} - -/** do a modulo 2.pi -> [-Pi,+Pi] */ -static double modulo_2pi(double a) -{ - double res = a - (((int32_t) (a/M_2PI)) * M_2PI); - return simple_modulo_2pi(res); -} - -#include -/** near the target (dist) ? */ -static uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win) -{ - double d = traj->target.pol.distance - rs_get_distance(traj->robot); - d = ABS(d); - d = d / traj->position->phys.distance_imp_per_mm; - return (d < d_win); -} - -/** near the target (dist in x,y) ? */ -static uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win) -{ - double x1 = traj->target.cart.x; - double y1 = traj->target.cart.y; - double x2 = position_get_x_double(traj->position); - double y2 = position_get_y_double(traj->position); - return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win ); -} - -/** near the angle target in radian ? Only valid if - * traj->target.pol.angle is set (i.e. an angle command, not an xy - * command) */ -static uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad) -{ - double a; - - /* convert relative angle from imp to rad */ - a = traj->target.pol.angle - rs_get_angle(traj->robot); - a /= traj->position->phys.distance_imp_per_mm; - a /= traj->position->phys.track_mm; - a *= 2.; - return ABS(a) < a_win_rad; -} - - -/************ SIMPLE TRAJS, NO EVENT */ - -#define UPDATE_D 1 -#define UPDATE_A 2 -#define RESET_D 4 -#define RESET_A 8 - -/** - * update angle and/or distance - * this function is not called directly by the user - * traj : pointer to the trajectory structure - * d_mm : distance in mm - * a_rad : angle in radian - * flags : what to update (UPDATE_A, UPDATE_D) - */ -void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm, - double a_rad, uint8_t state, uint8_t flags) -{ - int32_t a_consign, d_consign; - - DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad); - delete_event(traj); - traj->state = state; - if (flags & UPDATE_A) { - if (flags & RESET_A) { - a_consign = 0; - } - else { - a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) * - (traj->position->phys.track_mm) / 2); - } - a_consign += rs_get_angle(traj->robot); - traj->target.pol.angle = a_consign; - cs_set_consign(traj->csm_angle, a_consign); - } - if (flags & UPDATE_D) { - if (flags & RESET_D) { - d_consign = 0; - } - else { - d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm)); - } - d_consign += rs_get_distance(traj->robot); - traj->target.pol.distance = d_consign; - cs_set_consign(traj->csm_distance, d_consign); - } -} - -/** go straight forward (d is in mm) */ -void trajectory_d_rel(struct trajectory *traj, double d_mm) -{ - __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, - UPDATE_D | UPDATE_A | RESET_A); -} - -/** update distance consign without changing angle consign */ -void trajectory_only_d_rel(struct trajectory *traj, double d_mm) -{ - __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D); -} - -/** turn by 'a' degrees */ -void trajectory_a_rel(struct trajectory *traj, double a_deg_rel) -{ - __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A, - UPDATE_A | UPDATE_D | RESET_D); -} - -/** turn by 'a' degrees */ -void trajectory_a_abs(struct trajectory *traj, double a_deg_abs) -{ - double posa = position_get_a_rad_double(traj->position); - double a; - - a = RAD(a_deg_abs) - posa; - a = modulo_2pi(a); - __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, - UPDATE_A | UPDATE_D | RESET_D); -} - -/** turn the robot until the point x,y is in front of us */ -void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm) -{ - double posx = position_get_x_double(traj->position); - double posy = position_get_y_double(traj->position); - double posa = position_get_a_rad_double(traj->position); - - DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm); - __trajectory_goto_d_a_rel(traj, 0, - simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa), - RUNNING_A, - UPDATE_A | UPDATE_D | RESET_D); -} - -/** turn the robot until the point x,y is behind us */ -void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm) -{ - double posx = position_get_x_double(traj->position); - double posy = position_get_y_double(traj->position); - double posa = position_get_a_rad_double(traj->position); - - DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm); - __trajectory_goto_d_a_rel(traj, 0, - modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI), - RUNNING_A, - UPDATE_A | UPDATE_D | RESET_D); -} - -/** update angle consign without changing distance consign */ -void trajectory_only_a_rel(struct trajectory *traj, double a_deg) -{ - __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A, - UPDATE_A); -} - -/** update angle consign without changing distance consign */ -void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs) -{ - double posa = position_get_a_rad_double(traj->position); - double a; - - a = RAD(a_deg_abs) - posa; - a = modulo_2pi(a); - __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A); -} - -/** turn by 'a' degrees */ -void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg) -{ - __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg), - RUNNING_AD, UPDATE_A | UPDATE_D); -} - -/** set relative angle and distance consign to 0 */ -void trajectory_stop(struct trajectory *traj) -{ - __trajectory_goto_d_a_rel(traj, 0, 0, READY, - UPDATE_A | UPDATE_D | RESET_D | RESET_A); -} - -/** set relative angle and distance consign to 0, and break any - * deceleration ramp in quadramp filter */ -void trajectory_hardstop(struct trajectory *traj) -{ - struct quadramp_filter *q_d, *q_a; - q_d = traj->csm_distance->consign_filter_params; - q_a = traj->csm_angle->consign_filter_params; - __trajectory_goto_d_a_rel(traj, 0, 0, READY, - UPDATE_A | UPDATE_D | RESET_D | RESET_A); - q_d->previous_var = 0; - q_d->previous_out = rs_get_distance(traj->robot); - q_a->previous_var = 0; - q_a->previous_out = rs_get_angle(traj->robot); -} - - -/************ GOTO XY, USE EVENTS */ - -/** goto a x,y point, using a trajectory event */ -void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y) -{ - DEBUG(E_TRAJECTORY, "Goto XY"); - delete_event(traj); - traj->target.cart.x = x; - traj->target.cart.y = y; - traj->state = RUNNING_XY_START; - trajectory_manager_event(traj); - schedule_event(traj); -} - -/** go forward to a x,y point, using a trajectory event */ -void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y) -{ - DEBUG(E_TRAJECTORY, "Goto XY_F"); - delete_event(traj); - traj->target.cart.x = x; - traj->target.cart.y = y; - traj->state = RUNNING_XY_F_START; - trajectory_manager_event(traj); - schedule_event(traj); -} - -/** go backward to a x,y point, using a trajectory event */ -void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y) -{ - DEBUG(E_TRAJECTORY, "Goto XY_B"); - delete_event(traj); - traj->target.cart.x = x; - traj->target.cart.y = y; - traj->state = RUNNING_XY_B_START; - trajectory_manager_event(traj); - schedule_event(traj); -} - -/** go forward to a d,a point, using a trajectory event */ -void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a) -{ - vect2_pol p; - double x = position_get_x_double(traj->position); - double y = position_get_y_double(traj->position); - - DEBUG(E_TRAJECTORY, "Goto DA rel"); - - delete_event(traj); - p.r = d; - p.theta = RAD(a) + position_get_a_rad_double(traj->position); - vect2_pol2cart(&p, &traj->target.cart); - traj->target.cart.x += x; - traj->target.cart.y += y; - - traj->state = RUNNING_XY_START; - trajectory_manager_event(traj); - schedule_event(traj); -} - -/** go forward to a x,y relative point, using a trajectory event */ -void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm) -{ - vect2_cart c; - vect2_pol p; - double x = position_get_x_double(traj->position); - double y = position_get_y_double(traj->position); - - DEBUG(E_TRAJECTORY, "Goto XY rel"); - - delete_event(traj); - c.x = x_rel_mm; - c.y = y_rel_mm; - - vect2_cart2pol(&c, &p); - p.theta += position_get_a_rad_double(traj->position);; - vect2_pol2cart(&p, &traj->target.cart); - - traj->target.cart.x += x; - traj->target.cart.y += y; - - traj->state = RUNNING_XY_START; - trajectory_manager_event(traj); - schedule_event(traj); -} - -/************ FUNCS FOR GETTING TRAJ STATE */ - -/** return true if the position consign is equal to the filtered - * position consign (after quadramp filter), for angle and - * distance. */ -uint8_t trajectory_finished(struct trajectory *traj) -{ - return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) && - cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ; -} - -/** return true if traj is nearly finished */ -uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad) -{ - switch(traj->state) { - - case RUNNING_XY_ANGLE_OK: - case RUNNING_XY_F_ANGLE_OK: - case RUNNING_XY_B_ANGLE_OK: - /* if robot coordinates are near the x,y target */ - return is_robot_in_xy_window(traj, d_win); - - case RUNNING_A: - return is_robot_in_angle_window(traj, a_win_rad); - - case RUNNING_D: - return is_robot_in_dist_window(traj, d_win); - - case RUNNING_AD: - return is_robot_in_dist_window(traj, d_win) && - is_robot_in_angle_window(traj, a_win_rad); - - case RUNNING_XY_START: - case RUNNING_XY_F_START: - case RUNNING_XY_B_START: - case RUNNING_XY_ANGLE: - case RUNNING_XY_F_ANGLE: - case RUNNING_XY_B_ANGLE: - default: - return 0; - } -} - -/*********** *TRAJECTORY EVENT FUNC */ - -/** event called for xy trajectories */ -static void trajectory_manager_event(void * param) -{ - struct trajectory *traj = (struct trajectory *)param; - double coef=1.0; - double x = position_get_x_double(traj->position); - double y = position_get_y_double(traj->position); - double a = position_get_a_rad_double(traj->position); - int32_t d_consign=0, a_consign=0; - - /* These vectors contain target position of the robot in - * its own coordinates */ - vect2_cart v2cart_pos; - vect2_pol v2pol_target; - - /* step 1 : process new commands to quadramps */ - - switch (traj->state) { - case RUNNING_XY_START: - case RUNNING_XY_ANGLE: - case RUNNING_XY_ANGLE_OK: - case RUNNING_XY_F_START: - case RUNNING_XY_F_ANGLE: - case RUNNING_XY_F_ANGLE_OK: - case RUNNING_XY_B_START: - case RUNNING_XY_B_ANGLE: - case RUNNING_XY_B_ANGLE_OK: - - /* process the command vector from current position to - * absolute target, or to the center of the circle. */ - v2cart_pos.x = traj->target.cart.x - x; - v2cart_pos.y = traj->target.cart.y - y; - vect2_cart2pol(&v2cart_pos, &v2pol_target); - v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); - - /* asked to go backwards */ - if (traj->state >= RUNNING_XY_B_START && - traj->state <= RUNNING_XY_B_ANGLE_OK ) { - v2pol_target.r = -v2pol_target.r; - v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI); - } - - /* if we don't need to go forward */ - if (traj->state >= RUNNING_XY_START && - traj->state <= RUNNING_XY_ANGLE_OK ) { - /* If the target is behind the robot, we need to go - * backwards. 0.52 instead of 0.5 because we prefer to - * go forward */ - if ((v2pol_target.theta > 0.52*M_PI) || - (v2pol_target.theta < -0.52*M_PI ) ) { - v2pol_target.r = -v2pol_target.r; - v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI); - } - } - - /* XXX circle */ - - /* 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) // || ABS(v2pol_target.r) < traj->d_win) - set_quadramp_speed(traj, 0, traj->a_speed); - else { - coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad; - set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed); - } - - d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm)); - d_consign += rs_get_distance(traj->robot); - - /* angle consign */ - /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */ - a_consign = (int32_t)(v2pol_target.theta * - (traj->position->phys.distance_imp_per_mm) * - (traj->position->phys.track_mm) / 2.2); - a_consign += rs_get_angle(traj->robot); - - break; - - default: - /* hmmm quite odd, delete the event */ - DEBUG(E_TRAJECTORY, "GNI ???"); - delete_event(traj); - traj->state = READY; - } - - - /* step 2 : update state, or delete event if we reached the - * destination */ - - /* XXX if target is our pos !! */ - - switch (traj->state) { - case RUNNING_XY_START: - case RUNNING_XY_F_START: - case RUNNING_XY_B_START: - /* START -> ANGLE */ - DEBUG(E_TRAJECTORY, "-> ANGLE"); - traj->state ++; - break; - - case RUNNING_XY_ANGLE: - case RUNNING_XY_F_ANGLE: - case RUNNING_XY_B_ANGLE: { - struct quadramp_filter *q_a; - q_a = traj->csm_angle->consign_filter_params; - /* if d_speed is not 0, we are in start_angle_win */ - if (get_quadramp_distance_speed(traj)) { - if(is_robot_in_xy_window(traj, traj->d_win)) { - delete_event(traj); - } - /* ANGLE -> ANGLE_OK */ - traj->state ++; - DEBUG(E_TRAJECTORY, "-> ANGLE_OK"); - } - break; - } - - case RUNNING_XY_ANGLE_OK: - case RUNNING_XY_F_ANGLE_OK: - case RUNNING_XY_B_ANGLE_OK: - /* If we reached the destination */ - if(is_robot_in_xy_window(traj, traj->d_win)) { - delete_event(traj); - } - break; - - default: - break; - } - - /* step 3 : send the processed commands to cs */ - - DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)", - x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta); - - DEBUG(E_TRAJECTORY,"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)); - - cs_set_consign(traj->csm_angle, a_consign); - cs_set_consign(traj->csm_distance, d_consign); -} - -/*********** *CIRCLE */ - -/* - * 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) -{ - double a_rad = RAD(a_deg); - return a_rad * radius_mm; -} - -/* - * Start a circle of specified radius around the specified center - * (relative with d,a). The distance is specified in mm. - */ -void trajectory_circle(struct trajectory *traj, - double center_d_mm, double center_a_rad, - double radius_mm, double dist_mm) -{ -/* double */ - -/* DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */ -/* center_a_rad); */ -/* delete_event(traj); */ -/* traj->state = RUNNING_CIRCLE; */ - - -} - -/* - * Start a circle of specified radius around the specified center - * (absolute). The distance is specified in mm. - */ -void trajectory_circle_abs_dist_mm(struct trajectory *traj, - double x_rel_mm, double y_rel_mm, - double radius_mm, double dist_mm) -{ -} - -/* - * Start a circle of specified radius around the specified center - * (absolute). The distance is specified in degrees. - */ -void trajectory_circle_abs_dist_deg(struct trajectory *traj, - double x_rel_mm, double y_rel_mm, - double radius_mm, double dist_degrees) -{ - -} diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c new file mode 100644 index 0000000..7adde48 --- /dev/null +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -0,0 +1,585 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $ + * + */ + +/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include "trajectory_manager_utils.h" +#include "trajectory_manager_core.h" + +/************ SIMPLE TRAJS, NO EVENT */ + +#define UPDATE_D 1 +#define UPDATE_A 2 +#define RESET_D 4 +#define RESET_A 8 + +/** + * update angle and/or distance + * this function is not called directly by the user + * traj : pointer to the trajectory structure + * d_mm : distance in mm + * a_rad : angle in radian + * flags : what to update (UPDATE_A, UPDATE_D) + */ +void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm, + double a_rad, uint8_t state, uint8_t flags) +{ + int32_t a_consign, d_consign; + + DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad); + delete_event(traj); + traj->state = state; + if (flags & UPDATE_A) { + if (flags & RESET_A) { + a_consign = 0; + } + else { + a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) * + (traj->position->phys.track_mm) / 2); + } + a_consign += rs_get_angle(traj->robot); + traj->target.pol.angle = a_consign; + cs_set_consign(traj->csm_angle, a_consign); + } + if (flags & UPDATE_D) { + if (flags & RESET_D) { + d_consign = 0; + } + else { + d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm)); + } + d_consign += rs_get_distance(traj->robot); + traj->target.pol.distance = d_consign; + cs_set_consign(traj->csm_distance, d_consign); + } +} + +/** go straight forward (d is in mm) */ +void trajectory_d_rel(struct trajectory *traj, double d_mm) +{ + __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, + UPDATE_D | UPDATE_A | RESET_A); +} + +/** update distance consign without changing angle consign */ +void trajectory_only_d_rel(struct trajectory *traj, double d_mm) +{ + __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D); +} + +/** turn by 'a' degrees */ +void trajectory_a_rel(struct trajectory *traj, double a_deg_rel) +{ + __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A, + UPDATE_A | UPDATE_D | RESET_D); +} + +/** turn by 'a' degrees */ +void trajectory_a_abs(struct trajectory *traj, double a_deg_abs) +{ + double posa = position_get_a_rad_double(traj->position); + double a; + + a = RAD(a_deg_abs) - posa; + a = modulo_2pi(a); + __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, + UPDATE_A | UPDATE_D | RESET_D); +} + +/** turn the robot until the point x,y is in front of us */ +void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm) +{ + double posx = position_get_x_double(traj->position); + double posy = position_get_y_double(traj->position); + double posa = position_get_a_rad_double(traj->position); + + DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm); + __trajectory_goto_d_a_rel(traj, 0, + simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa), + RUNNING_A, + UPDATE_A | UPDATE_D | RESET_D); +} + +/** turn the robot until the point x,y is behind us */ +void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm) +{ + double posx = position_get_x_double(traj->position); + double posy = position_get_y_double(traj->position); + double posa = position_get_a_rad_double(traj->position); + + DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm); + __trajectory_goto_d_a_rel(traj, 0, + modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI), + RUNNING_A, + UPDATE_A | UPDATE_D | RESET_D); +} + +/** update angle consign without changing distance consign */ +void trajectory_only_a_rel(struct trajectory *traj, double a_deg) +{ + __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A, + UPDATE_A); +} + +/** update angle consign without changing distance consign */ +void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs) +{ + double posa = position_get_a_rad_double(traj->position); + double a; + + a = RAD(a_deg_abs) - posa; + a = modulo_2pi(a); + __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A); +} + +/** turn by 'a' degrees */ +void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg) +{ + __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg), + RUNNING_AD, UPDATE_A | UPDATE_D); +} + +/** set relative angle and distance consign to 0 */ +void trajectory_stop(struct trajectory *traj) +{ + __trajectory_goto_d_a_rel(traj, 0, 0, READY, + UPDATE_A | UPDATE_D | RESET_D | RESET_A); +} + +/** set relative angle and distance consign to 0, and break any + * deceleration ramp in quadramp filter */ +void trajectory_hardstop(struct trajectory *traj) +{ + struct quadramp_filter *q_d, *q_a; + + q_d = traj->csm_distance->consign_filter_params; + q_a = traj->csm_angle->consign_filter_params; + __trajectory_goto_d_a_rel(traj, 0, 0, READY, + UPDATE_A | UPDATE_D | RESET_D | RESET_A); + + q_d->previous_var = 0; + q_d->previous_out = rs_get_distance(traj->robot); + q_a->previous_var = 0; + q_a->previous_out = rs_get_angle(traj->robot); +} + + +/************ GOTO XY, USE EVENTS */ + +/** goto a x,y point, using a trajectory event */ +void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y) +{ + DEBUG(E_TRAJECTORY, "Goto XY"); + delete_event(traj); + traj->target.cart.x = x; + traj->target.cart.y = y; + traj->state = RUNNING_XY_START; + trajectory_manager_event(traj); + schedule_event(traj); +} + +/** go forward to a x,y point, using a trajectory event */ +void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y) +{ + DEBUG(E_TRAJECTORY, "Goto XY_F"); + delete_event(traj); + traj->target.cart.x = x; + traj->target.cart.y = y; + traj->state = RUNNING_XY_F_START; + trajectory_manager_event(traj); + schedule_event(traj); +} + +/** go backward to a x,y point, using a trajectory event */ +void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y) +{ + DEBUG(E_TRAJECTORY, "Goto XY_B"); + delete_event(traj); + traj->target.cart.x = x; + traj->target.cart.y = y; + traj->state = RUNNING_XY_B_START; + trajectory_manager_event(traj); + schedule_event(traj); +} + +/** go forward to a d,a point, using a trajectory event */ +void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a) +{ + vect2_pol p; + double x = position_get_x_double(traj->position); + double y = position_get_y_double(traj->position); + + DEBUG(E_TRAJECTORY, "Goto DA rel"); + + delete_event(traj); + p.r = d; + p.theta = RAD(a) + position_get_a_rad_double(traj->position); + vect2_pol2cart(&p, &traj->target.cart); + traj->target.cart.x += x; + traj->target.cart.y += y; + + traj->state = RUNNING_XY_START; + trajectory_manager_event(traj); + schedule_event(traj); +} + +/** go forward to a x,y relative point, using a trajectory event */ +void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm) +{ + vect2_cart c; + vect2_pol p; + double x = position_get_x_double(traj->position); + double y = position_get_y_double(traj->position); + + DEBUG(E_TRAJECTORY, "Goto XY rel"); + + delete_event(traj); + c.x = x_rel_mm; + c.y = y_rel_mm; + + vect2_cart2pol(&c, &p); + p.theta += position_get_a_rad_double(traj->position);; + vect2_pol2cart(&p, &traj->target.cart); + + traj->target.cart.x += x; + traj->target.cart.y += y; + + traj->state = RUNNING_XY_START; + trajectory_manager_event(traj); + schedule_event(traj); +} + +/************ FUNCS FOR GETTING TRAJ STATE */ + +/** return true if the position consign is equal to the filtered + * position consign (after quadramp filter), for angle and + * distance. */ +uint8_t trajectory_finished(struct trajectory *traj) +{ + return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) && + cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ; +} + +/** return true if traj is nearly finished */ +uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad) +{ + switch(traj->state) { + + case RUNNING_XY_ANGLE_OK: + case RUNNING_XY_F_ANGLE_OK: + case RUNNING_XY_B_ANGLE_OK: + /* if robot coordinates are near the x,y target */ + return is_robot_in_xy_window(traj, d_win); + + case RUNNING_A: + return is_robot_in_angle_window(traj, a_win_rad); + + case RUNNING_D: + return is_robot_in_dist_window(traj, d_win); + + case RUNNING_AD: + return is_robot_in_dist_window(traj, d_win) && + is_robot_in_angle_window(traj, a_win_rad); + + case RUNNING_XY_START: + case RUNNING_XY_F_START: + case RUNNING_XY_B_START: + case RUNNING_XY_ANGLE: + case RUNNING_XY_F_ANGLE: + case RUNNING_XY_B_ANGLE: + default: + return 0; + } +} + +/*********** *TRAJECTORY EVENT FUNC */ + +/** event called for xy trajectories */ +void trajectory_manager_xy_event(struct trajectory *traj) +{ + double coef = 1.0; + double x = position_get_x_double(traj->position); + double y = position_get_y_double(traj->position); + double a = position_get_a_rad_double(traj->position); + int32_t d_consign=0, a_consign=0; + + /* These vectors contain target position of the robot in + * its own coordinates */ + vect2_cart v2cart_pos; + vect2_pol v2pol_target; + + /* step 1 : process new commands to quadramps */ + + switch (traj->state) { + case RUNNING_XY_START: + case RUNNING_XY_ANGLE: + case RUNNING_XY_ANGLE_OK: + case RUNNING_XY_F_START: + case RUNNING_XY_F_ANGLE: + case RUNNING_XY_F_ANGLE_OK: + case RUNNING_XY_B_START: + case RUNNING_XY_B_ANGLE: + case RUNNING_XY_B_ANGLE_OK: + + /* process the command vector from current position to + * absolute target, or to the center of the circle. */ + v2cart_pos.x = traj->target.cart.x - x; + v2cart_pos.y = traj->target.cart.y - y; + vect2_cart2pol(&v2cart_pos, &v2pol_target); + v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); + + /* asked to go backwards */ + if (traj->state >= RUNNING_XY_B_START && + traj->state <= RUNNING_XY_B_ANGLE_OK ) { + v2pol_target.r = -v2pol_target.r; + v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI); + } + + /* if we don't need to go forward */ + if (traj->state >= RUNNING_XY_START && + traj->state <= RUNNING_XY_ANGLE_OK ) { + /* If the target is behind the robot, we need to go + * backwards. 0.52 instead of 0.5 because we prefer to + * go forward */ + if ((v2pol_target.theta > 0.52*M_PI) || + (v2pol_target.theta < -0.52*M_PI ) ) { + v2pol_target.r = -v2pol_target.r; + v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI); + } + } + + /* XXX circle */ + + /* 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) // || ABS(v2pol_target.r) < traj->d_win) + set_quadramp_speed(traj, 0, traj->a_speed); + else { + coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad; + set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed); + } + + d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm)); + d_consign += rs_get_distance(traj->robot); + + /* angle consign */ + /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */ + a_consign = (int32_t)(v2pol_target.theta * + (traj->position->phys.distance_imp_per_mm) * + (traj->position->phys.track_mm) / 2.2); + a_consign += rs_get_angle(traj->robot); + + break; + + default: + /* hmmm quite odd, delete the event */ + DEBUG(E_TRAJECTORY, "GNI ???"); + delete_event(traj); + traj->state = READY; + } + + + /* step 2 : update state, or delete event if we reached the + * destination */ + + /* XXX if target is our pos !! */ + + switch (traj->state) { + case RUNNING_XY_START: + case RUNNING_XY_F_START: + case RUNNING_XY_B_START: + /* START -> ANGLE */ + DEBUG(E_TRAJECTORY, "-> ANGLE"); + traj->state ++; + break; + + case RUNNING_XY_ANGLE: + case RUNNING_XY_F_ANGLE: + case RUNNING_XY_B_ANGLE: { + struct quadramp_filter *q_a; + q_a = traj->csm_angle->consign_filter_params; + /* if d_speed is not 0, we are in start_angle_win */ + if (get_quadramp_distance_speed(traj)) { + if(is_robot_in_xy_window(traj, traj->d_win)) { + delete_event(traj); + } + /* ANGLE -> ANGLE_OK */ + traj->state ++; + DEBUG(E_TRAJECTORY, "-> ANGLE_OK"); + } + break; + } + + case RUNNING_XY_ANGLE_OK: + case RUNNING_XY_F_ANGLE_OK: + case RUNNING_XY_B_ANGLE_OK: + /* If we reached the destination */ + if(is_robot_in_xy_window(traj, traj->d_win)) { + delete_event(traj); + } + break; + + default: + break; + } + + /* step 3 : send the processed commands to cs */ + + DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)", + x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta); + + DEBUG(E_TRAJECTORY,"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)); + + cs_set_consign(traj->csm_angle, a_consign); + cs_set_consign(traj->csm_distance, d_consign); +} + +/* trajectory event for circles */ +void trajectory_manager_circle_event(struct trajectory *traj) +{ +#if 0 + double x = position_get_x_double(traj->position); + double y = position_get_y_double(traj->position); + double a = position_get_a_rad_double(traj->position); + int32_t d_consign=0, a_consign=0; +#endif +} + +/* trajectory event */ +void trajectory_manager_event(void * param) +{ + struct trajectory *traj = (struct trajectory *)param; + + switch (traj->state) { + case RUNNING_XY_START: + case RUNNING_XY_ANGLE: + case RUNNING_XY_ANGLE_OK: + case RUNNING_XY_F_START: + case RUNNING_XY_F_ANGLE: + case RUNNING_XY_F_ANGLE_OK: + case RUNNING_XY_B_START: + case RUNNING_XY_B_ANGLE: + case RUNNING_XY_B_ANGLE_OK: + trajectory_manager_xy_event(traj); + break; + + case RUNNING_CIRCLE: + trajectory_manager_circle_event(traj); + break; + + default: + break; + } +} + +/*********** *CIRCLE */ + +/* + * 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) +{ + double a_rad = RAD(a_deg); + return a_rad * radius_mm; +} + +/* + * Start a circle of specified radius around the specified center + * (relative with d,a). The distance is specified in mm. + */ +void trajectory_circle(struct trajectory *traj, + double center_d_mm, double center_a_rad, + double radius_mm, double dist_mm) +{ +/* double */ + +/* DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */ +/* center_a_rad); */ +/* delete_event(traj); */ +/* traj->state = RUNNING_CIRCLE; */ + + +} + +/* + * Start a circle of specified radius around the specified center + * (absolute). The distance is specified in mm. + */ +void trajectory_circle_abs_dist_mm(struct trajectory *traj, + double x_rel_mm, double y_rel_mm, + double radius_mm, double dist_mm) +{ +} + +/* + * Start a circle of specified radius around the specified center + * (absolute). The distance is specified in degrees. + */ +void trajectory_circle_abs_dist_deg(struct trajectory *traj, + double x_rel_mm, double y_rel_mm, + double radius_mm, double dist_degrees) +{ + +} diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.h b/modules/devices/robot/trajectory_manager/trajectory_manager_core.h new file mode 100644 index 0000000..a724f69 --- /dev/null +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.h @@ -0,0 +1,113 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $ + * + */ + +/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */ + +/** + * update angle and/or distance + * this function is not called directly by the user + * traj : pointer to the trajectory structure + * d_mm : distance in mm + * a_rad : angle in radian + * flags : what to update (UPDATE_A, UPDATE_D) + */ +void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm, + double a_rad, uint8_t state, uint8_t flags); + +/** go straight forward (d is in mm) */ +void trajectory_d_rel(struct trajectory *traj, double d_mm); + +/** update distance consign without changing angle consign */ +void trajectory_only_d_rel(struct trajectory *traj, double d_mm); + +/** turn by 'a' degrees */ +void trajectory_a_rel(struct trajectory *traj, double a_deg_rel); + +/** turn by 'a' degrees */ +void trajectory_a_abs(struct trajectory *traj, double a_deg_abs); + +/** turn the robot until the point x,y is in front of us */ +void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm); + + +/** turn the robot until the point x,y is behind us */ +void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm); + + +/** update angle consign without changing distance consign */ +void trajectory_only_a_rel(struct trajectory *traj, double a_deg); + +/** update angle consign without changing distance consign */ +void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs); + + +/** turn by 'a' degrees */ +void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg); + +/** set relative angle and distance consign to 0 */ +void trajectory_stop(struct trajectory *traj); + +/** set relative angle and distance consign to 0, and break any + * deceleration ramp in quadramp filter */ +void trajectory_hardstop(struct trajectory *traj); + + +/************ GOTO XY, USE EVENTS */ + +/** goto a x,y point, using a trajectory event */ +void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y); + +/** go forward to a x,y point, using a trajectory event */ +void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y); + +/** go backward to a x,y point, using a trajectory event */ +void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y); + +/** go forward to a d,a point, using a trajectory event */ +void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a); + +/** go forward to a x,y relative point, using a trajectory event */ +void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm); + +/************ FUNCS FOR GETTING TRAJ STATE */ + +/** return true if the position consign is equal to the filtered + * position consign (after quadramp filter), for angle and + * distance. */ +uint8_t trajectory_finished(struct trajectory *traj); + +/** return true if traj is nearly finished */ +uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad); + +/*********** *TRAJECTORY EVENT FUNC */ + +/** event called for xy trajectories */ +void trajectory_manager_xy_event(struct trajectory *traj); + +/* trajectory event for circles */ +void trajectory_manager_circle_event(struct trajectory *traj); + +/* trajectory event */ +void trajectory_manager_event(void * param); + +/*********** *CIRCLE */ + +/* XXX TODO */ diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c new file mode 100644 index 0000000..0cf01ac --- /dev/null +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c @@ -0,0 +1,145 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $ + * + */ + +/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */ + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include "trajectory_manager_utils.h" +#include "trajectory_manager_core.h" + +/** set speed consign in quadramp filter */ +void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed) +{ + struct quadramp_filter * q_d, * q_a; + q_d = traj->csm_distance->consign_filter_params; + q_a = traj->csm_angle->consign_filter_params; + quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed)); + quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed)); +} + +/** get angle speed consign in quadramp filter */ +uint32_t get_quadramp_angle_speed(struct trajectory *traj) +{ + struct quadramp_filter *q_a; + q_a = traj->csm_angle->consign_filter_params; + return q_a->var_1st_ord_pos; +} + +/** get distance speed consign in quadramp filter */ +uint32_t get_quadramp_distance_speed(struct trajectory *traj) +{ + struct quadramp_filter *q_d; + q_d = traj->csm_distance->consign_filter_params; + return q_d->var_1st_ord_pos; +} + +/** remove event if any */ +void delete_event(struct trajectory *traj) +{ + set_quadramp_speed(traj, traj->d_speed, traj->a_speed); + if ( traj->scheduler_task != -1) { + DEBUG(E_TRAJECTORY, "Delete event"); + scheduler_del_event(traj->scheduler_task); + traj->scheduler_task = -1; + } +} + +/** schedule the trajectory event */ +void schedule_event(struct trajectory *traj) +{ + if ( traj->scheduler_task != -1) { + DEBUG(E_TRAJECTORY, "Schedule event, already scheduled"); + } + else { + traj->scheduler_task = + scheduler_add_periodical_event_priority(&trajectory_manager_event, + (void*)traj, + TRAJ_EVT_PERIOD, 30); + } +} + +/** 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; +} + +/** do a modulo 2.pi -> [-Pi,+Pi] */ +double modulo_2pi(double a) +{ + double res = a - (((int32_t) (a/M_2PI)) * M_2PI); + return simple_modulo_2pi(res); +} + +/** near the target (dist) ? */ +uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win) +{ + double d = traj->target.pol.distance - rs_get_distance(traj->robot); + d = ABS(d); + d = d / traj->position->phys.distance_imp_per_mm; + return (d < d_win); +} + +/** near the target (dist in x,y) ? */ +uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win) +{ + double x1 = traj->target.cart.x; + double y1 = traj->target.cart.y; + double x2 = position_get_x_double(traj->position); + double y2 = position_get_y_double(traj->position); + return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win ); +} + +/** near the angle target in radian ? Only valid if + * traj->target.pol.angle is set (i.e. an angle command, not an xy + * command) */ +uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad) +{ + double a; + + /* convert relative angle from imp to rad */ + a = traj->target.pol.angle - rs_get_angle(traj->robot); + a /= traj->position->phys.distance_imp_per_mm; + a /= traj->position->phys.track_mm; + a *= 2.; + return ABS(a) < a_win_rad; +} diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h new file mode 100644 index 0000000..9ceaf48 --- /dev/null +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.h @@ -0,0 +1,62 @@ +/* + * Copyright Droids Corporation, Microb Technology, Eirbot (2005) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $ + * + */ + +/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */ + +#define M_2PI (2*M_PI) + +#define DEG(x) ((x) * (180.0 / M_PI)) +#define RAD(x) ((x) * (M_PI / 180.0)) + +/* 25 ms */ +#define TRAJ_EVT_PERIOD (25000UL/SCHEDULER_UNIT) + +/** set speed consign in quadramp filter */ +void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed); + +/** get angle speed consign in quadramp filter */ +uint32_t get_quadramp_angle_speed(struct trajectory *traj); + +/** get distance speed consign in quadramp filter */ +uint32_t get_quadramp_distance_speed(struct trajectory *traj); + +/** remove event if any */ +void delete_event(struct trajectory *traj); + +/** schedule the trajectory event */ +void schedule_event(struct trajectory *traj); + +/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */ +double simple_modulo_2pi(double a); + +/** do a modulo 2.pi -> [-Pi,+Pi] */ +double modulo_2pi(double a); + +/** near the target (dist) ? */ +uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win); + +/** near the target (dist in x,y) ? */ +uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win); + +/** near the angle target in radian ? Only valid if + * traj->target.pol.angle is set (i.e. an angle command, not an xy + * command) */ +uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad); -- 2.39.5