X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Ftrajectory_manager%2Ftrajectory_manager.h;h=7cab78569b2fb2fc6cef8cda196d67e1208b3ec5;hp=648ec1c96a749aa04344b8eda66ca6805d032775;hb=d3a38d6cce69bc7debce828b4ecd3a9c0c866e96;hpb=18cb5bc5e70575b33555423dc68d4b49be672dc2 diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h index 648ec1c..7cab785 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager.h +++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h @@ -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 @@ -25,6 +25,8 @@ #include #include #include +#include +#include enum trajectory_state { READY, @@ -44,9 +46,41 @@ enum trajectory_state { RUNNING_XY_B_START, RUNNING_XY_B_ANGLE, RUNNING_XY_B_ANGLE_OK, + + /* circle */ RUNNING_CIRCLE, + + /* line */ + RUNNING_LINE, + + /* clitoid */ + RUNNING_CLITOID_LINE, + RUNNING_CLITOID_CURVE, }; +struct circle_target { + vect2_cart center; /**< center of the circle */ + double radius; /**< radius of the circle */ + int32_t dest_angle; /**< dst angle in inc */ + +#define TRIGO 1 /* rotation is counterclockwise */ +#define FORWARD 2 /* go forward or backward */ + uint8_t flags; /**< flags for this trajectory */ +}; + +/* for line and clitoid */ +struct line_target { + line_t line; + double angle; + double advance; + + /* only for clitoid */ + point_t turn_pt; + double Aa; + double Va; + double alpha; + double R; +}; struct trajectory { enum trajectory_state state; /*<< describe the type of target, and if we reached the target */ @@ -54,54 +88,78 @@ struct trajectory { union { vect2_cart cart; /**<< target, if it is a x,y vector */ struct rs_polar pol; /**<< target, if it is a d,a vector */ + struct circle_target circle; /**<< target, if it is a circle */ + struct line_target line; /**<< target, if it is a line */ } target; double d_win; /**<< distance window (for END_NEAR) */ 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 */ - - uint16_t d_speed; /**<< distance speed consign */ - uint16_t a_speed; /**<< angle speed consign */ + double circle_coef;/**<< corrective circle coef */ + + double d_speed; /**<< distance speed consign */ + double a_speed; /**<< angle speed consign */ + + double d_acc; /**<< distance acceleration consign */ + double a_acc; /**<< angle acceleration consign */ struct robot_position *position; /**<< associated robot_position */ struct robot_system *robot; /**<< associated robot_system */ struct cs *csm_angle; /**<< associated control system (angle) */ struct cs *csm_distance; /**<< associated control system (distance) */ - + + double cs_hz; + int8_t scheduler_task; /**<< id of current task (-1 if no running task) */ }; /** structure initialization */ -void trajectory_init(struct trajectory *traj); +void trajectory_init(struct trajectory *traj, double cs_hz); /** 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); /** structure initialization */ -void trajectory_set_robot_params(struct trajectory *traj, - struct robot_system *rs, +void trajectory_set_robot_params(struct trajectory *traj, + struct robot_system *rs, struct robot_position *pos) ; /** 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, double d_speed, double a_speed); + +/** set speed consign */ +void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc); -/** - * set windows for trajectory. +/** + * set windows for trajectory. * params: distance window, angle window: we the robot enters this - * position window, we deletes the event and the last consign is + * position window, we deletes the event and the last consign is * used. - * a_start_deg used in xy consigns (start to move in distance when + * a_start_deg used in xy consigns (start to move in distance when * a_target < a_start) */ -void trajectory_set_windows(struct trajectory *traj, double d_win, +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 trajectory state + */ +enum trajectory_state trajectory_get_state(struct trajectory *traj); + /** 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); +uint8_t trajectory_angle_finished(struct trajectory *traj); +uint8_t trajectory_distance_finished(struct trajectory *traj); /** return true if traj is nearly finished depending on specified * parameters */ @@ -128,10 +186,10 @@ void trajectory_a_rel(struct trajectory *traj, double a_deg); /** go to angle 'a' in degrees */ void trajectory_a_abs(struct trajectory *traj, double a_deg); -/** turn the robot until the point x,y is in front of us */ +/** 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 */ +/** 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 */ @@ -160,4 +218,23 @@ 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); +/** make the robot orbiting around (x,y) on a circle whose radius is + * radius_mm, and exit when relative destination angle is reached. The + * flags set if we go forward or backwards, and CW/CCW. */ +void trajectory_circle_rel(struct trajectory *traj, double x, double y, + double radius_mm, double rel_a_deg, uint8_t flags); + +/* + * Compute the fastest distance and angle speeds matching the radius + * from current traj_speed + */ +void circle_get_da_speed_from_radius(struct trajectory *traj, + double radius_mm, + double *speed_d, + double *speed_a); + +/* do a line */ +void trajectory_line_abs(struct trajectory *traj, double x1, double y1, + double x2, double y2, double advance); + #endif //TRAJECTORY_MANAGER