#include <string.h>#include <stdlib.h>#include <math.h>#include <aversive.h>#include <aversive/error.h>#include <scheduler.h>#include <vect2.h>#include <position_manager.h>#include <robot_system.h>#include <control_system_manager.h>#include <quadramp.h>#include <trajectory_manager.h>Go to the source code of this file.
Defines | |
| #define | M_2PI (2*M_PI) |
| #define | DEG(x) ((x) * (180.0 / M_PI)) |
| #define | RAD(x) ((x) * (M_PI / 180.0)) |
| #define | UPDATE_D 1 |
| #define | UPDATE_A 2 |
| #define | RESET_D 4 |
| #define | RESET_A 8 |
Functions | |
| void | trajectory_init (struct trajectory *traj) |
| void | trajectory_set_cs (struct trajectory *traj, struct cs *cs_d, struct cs *cs_a) |
| void | trajectory_set_robot_params (struct trajectory *traj, struct robot_system *rs, struct robot_position *pos) |
| void | trajectory_set_speed (struct trajectory *traj, int16_t d_speed, int16_t a_speed) |
| void | trajectory_set_windows (struct trajectory *traj, double d_win, double a_win_deg, double a_start_deg) |
| void | __trajectory_goto_d_a_rel (struct trajectory *traj, double d_cm, double a_rad, uint8_t flags) |
| void | trajectory_d_rel (struct trajectory *traj, double d_cm) |
| void | trajectory_only_d_rel (struct trajectory *traj, double d_cm) |
| void | trajectory_a_rel (struct trajectory *traj, double a_deg_rel) |
| void | trajectory_a_abs (struct trajectory *traj, double a_deg_abs) |
| void | trajectory_turnto_xy (struct trajectory *traj, double x_abs_cm, double y_abs_cm) |
| void | trajectory_turnto_xy_behind (struct trajectory *traj, double x_abs_cm, double y_abs_cm) |
| void | trajectory_only_a_rel (struct trajectory *traj, double a_deg) |
| void | trajectory_d_a_rel (struct trajectory *traj, double d_cm, double a_deg) |
| void | trajectory_stop (struct trajectory *traj) |
| void | trajectory_hardstop (struct trajectory *traj) |
| void | trajectory_goto_xy_abs (struct trajectory *traj, double x, double y) |
| void | trajectory_goto_forward_xy_abs (struct trajectory *traj, double x, double y) |
| void | trajectory_goto_d_a_rel (struct trajectory *traj, double d, double a) |
| void | trajectory_goto_xy_rel (struct trajectory *traj, double x_rel_cm, double y_rel_cm) |
| uint8_t | trajectory_finished (struct trajectory *traj) |
| uint8_t | trajectory_in_window (struct trajectory *traj, double d_win, double a_win_rad) |
| #define DEG | ( | x | ) | ((x) * (180.0 / M_PI)) |
Definition at line 42 of file trajectory_manager.c.
| #define M_2PI (2*M_PI) |
Definition at line 40 of file trajectory_manager.c.
| #define RAD | ( | x | ) | ((x) * (M_PI / 180.0)) |
Definition at line 43 of file trajectory_manager.c.
Referenced by trajectory_a_abs(), trajectory_a_rel(), trajectory_d_a_rel(), trajectory_goto_d_a_rel(), trajectory_only_a_rel(), and trajectory_set_windows().
| #define RESET_A 8 |
Definition at line 220 of file trajectory_manager.c.
Referenced by __trajectory_goto_d_a_rel(), trajectory_d_rel(), trajectory_hardstop(), and trajectory_stop().
| #define RESET_D 4 |
Definition at line 219 of file trajectory_manager.c.
Referenced by __trajectory_goto_d_a_rel(), trajectory_a_abs(), trajectory_a_rel(), trajectory_hardstop(), trajectory_stop(), trajectory_turnto_xy(), and trajectory_turnto_xy_behind().
| #define UPDATE_A 2 |
Definition at line 218 of file trajectory_manager.c.
Referenced by __trajectory_goto_d_a_rel(), trajectory_a_abs(), trajectory_a_rel(), trajectory_d_a_rel(), trajectory_d_rel(), trajectory_hardstop(), trajectory_only_a_rel(), trajectory_stop(), trajectory_turnto_xy(), and trajectory_turnto_xy_behind().
| #define UPDATE_D 1 |
Definition at line 217 of file trajectory_manager.c.
Referenced by __trajectory_goto_d_a_rel(), trajectory_a_abs(), trajectory_a_rel(), trajectory_d_a_rel(), trajectory_d_rel(), trajectory_hardstop(), trajectory_only_d_rel(), trajectory_stop(), trajectory_turnto_xy(), and trajectory_turnto_xy_behind().
| void __trajectory_goto_d_a_rel | ( | struct trajectory * | traj, | |
| double | d_cm, | |||
| double | a_rad, | |||
| uint8_t | flags | |||
| ) |
update angle and/or distance this function is not called directly by the user traj : pointer to the trajectory structure d_cm : distance in cm a_rad : angle in radian flags : what to update (UPDATE_A, UPDATE_D)
Definition at line 230 of file trajectory_manager.c.
References rs_polar::angle, cs_set_consign(), trajectory::csm_angle, trajectory::csm_distance, DEBUG, rs_polar::distance, robot_physical_params::distance_imp_per_cm, E_TRAJECTORY, robot_position::phys, trajectory::pol, trajectory::position, position_get_a_rad_double(), position_get_x_double(), position_get_y_double(), RESET_A, RESET_D, trajectory::robot, rs_get_angle(), rs_get_distance(), trajectory::target, robot_physical_params::track_cm, UPDATE_A, and UPDATE_D.
Referenced by trajectory_a_abs(), trajectory_a_rel(), trajectory_d_a_rel(), trajectory_d_rel(), trajectory_hardstop(), trajectory_only_a_rel(), trajectory_only_d_rel(), trajectory_stop(), trajectory_turnto_xy(), and trajectory_turnto_xy_behind().
| void trajectory_a_abs | ( | struct trajectory * | traj, | |
| double | a_deg_abs | |||
| ) |
turn by 'a' degrees
Definition at line 286 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), trajectory::position, position_get_a_rad_double(), RAD, RESET_D, RUNNING_A, trajectory::state, UPDATE_A, and UPDATE_D.
| void trajectory_a_rel | ( | struct trajectory * | traj, | |
| double | a_deg_rel | |||
| ) |
turn by 'a' degrees
Definition at line 279 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), RAD, RESET_D, RUNNING_A, trajectory::state, UPDATE_A, and UPDATE_D.
| void trajectory_d_a_rel | ( | struct trajectory * | traj, | |
| double | d_cm, | |||
| double | a_deg | |||
| ) |
turn by 'a' degrees
Definition at line 333 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), RAD, RUNNING_AD, trajectory::state, UPDATE_A, and UPDATE_D.
| void trajectory_d_rel | ( | struct trajectory * | traj, | |
| double | d_cm | |||
| ) |
go straight forward (d is in cm)
Definition at line 265 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), RESET_A, RUNNING_D, trajectory::state, UPDATE_A, and UPDATE_D.
| uint8_t trajectory_finished | ( | struct trajectory * | traj | ) |
return true if the position consign is equal to the filtered position consign (after quadramp filter), for angle and distance.
Definition at line 442 of file trajectory_manager.c.
References cs_get_consign(), cs_get_filtered_consign(), trajectory::csm_angle, and trajectory::csm_distance.
| void trajectory_goto_d_a_rel | ( | struct trajectory * | traj, | |
| double | d, | |||
| double | a | |||
| ) |
go forward to a d,a point, using a trajectory event
Definition at line 391 of file trajectory_manager.c.
References trajectory::cart, DEBUG, E_TRAJECTORY, gen_regs::p, trajectory::position, position_get_a_rad_double(), position_get_x_double(), position_get_y_double(), _vect2_pol::r, RAD, RUNNING_XY_START, trajectory::state, trajectory::target, _vect2_pol::theta, vect2_pol2cart(), _vect2_cart::x, and _vect2_cart::y.
| void trajectory_goto_forward_xy_abs | ( | struct trajectory * | traj, | |
| double | x, | |||
| double | y | |||
| ) |
go forward to a x,y point, using a trajectory event
Definition at line 379 of file trajectory_manager.c.
References trajectory::cart, DEBUG, E_TRAJECTORY, RUNNING_XY_F_START, trajectory::state, trajectory::target, _vect2_cart::x, and _vect2_cart::y.
| void trajectory_goto_xy_abs | ( | struct trajectory * | traj, | |
| double | x, | |||
| double | y | |||
| ) |
goto a x,y point, using a trajectory event
Definition at line 367 of file trajectory_manager.c.
References trajectory::cart, DEBUG, E_TRAJECTORY, RUNNING_XY_START, trajectory::state, trajectory::target, _vect2_cart::x, and _vect2_cart::y.
| void trajectory_goto_xy_rel | ( | struct trajectory * | traj, | |
| double | x_rel_cm, | |||
| double | y_rel_cm | |||
| ) |
go forward to a x,y relative point, using a trajectory event
Definition at line 412 of file trajectory_manager.c.
References trajectory::cart, DEBUG, E_TRAJECTORY, gen_regs::p, trajectory::position, position_get_a_rad_double(), position_get_x_double(), position_get_y_double(), RUNNING_XY_START, trajectory::state, trajectory::target, _vect2_pol::theta, vect2_cart2pol(), vect2_pol2cart(), _vect2_cart::x, and _vect2_cart::y.
| void trajectory_hardstop | ( | struct trajectory * | traj | ) |
set relative angle and distance consign to 0, and break any deceleration ramp in quadramp filter
Definition at line 348 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), cs::consign_filter_params, trajectory::csm_angle, trajectory::csm_distance, quadramp_filter::previous_out, quadramp_filter::previous_var, READY, RESET_A, RESET_D, trajectory::robot, rs_get_angle(), rs_get_distance(), trajectory::state, UPDATE_A, and UPDATE_D.
| uint8_t trajectory_in_window | ( | struct trajectory * | traj, | |
| double | d_win, | |||
| double | a_win_rad | |||
| ) |
return true if traj is nearly finished
Definition at line 449 of file trajectory_manager.c.
References RUNNING_A, RUNNING_AD, RUNNING_D, RUNNING_XY_ANGLE, RUNNING_XY_ANGLE_OK, RUNNING_XY_F_ANGLE, RUNNING_XY_F_ANGLE_OK, RUNNING_XY_F_START, RUNNING_XY_START, and trajectory::state.
| void trajectory_init | ( | struct trajectory * | traj | ) |
structure initialization
Definition at line 50 of file trajectory_manager.c.
References IRQ_LOCK, IRQ_UNLOCK, READY, trajectory::scheduler_task, and trajectory::state.
| void trajectory_only_a_rel | ( | struct trajectory * | traj, | |
| double | a_deg | |||
| ) |
update angle consign without changing distance consign
Definition at line 326 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), RAD, RUNNING_A, trajectory::state, and UPDATE_A.
| void trajectory_only_d_rel | ( | struct trajectory * | traj, | |
| double | d_cm | |||
| ) |
update distance consign without changing angle consign
Definition at line 272 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), RUNNING_D, trajectory::state, and UPDATE_D.
| void trajectory_set_cs | ( | struct trajectory * | traj, | |
| struct cs * | cs_d, | |||
| struct cs * | cs_a | |||
| ) |
structure initialization
Definition at line 62 of file trajectory_manager.c.
References trajectory::csm_angle, trajectory::csm_distance, IRQ_LOCK, and IRQ_UNLOCK.
| void trajectory_set_robot_params | ( | struct trajectory * | traj, | |
| struct robot_system * | rs, | |||
| struct robot_position * | pos | |||
| ) |
structure initialization
Definition at line 74 of file trajectory_manager.c.
References IRQ_LOCK, IRQ_UNLOCK, trajectory::position, and trajectory::robot.
| void trajectory_set_speed | ( | struct trajectory * | traj, | |
| int16_t | d_speed, | |||
| int16_t | a_speed | |||
| ) |
set speed consign
Definition at line 86 of file trajectory_manager.c.
References trajectory::a_speed, trajectory::d_speed, IRQ_LOCK, and IRQ_UNLOCK.
| void trajectory_set_windows | ( | struct trajectory * | traj, | |
| double | d_win, | |||
| double | a_win_deg, | |||
| double | a_start_deg | |||
| ) |
set windows for trajectory
Definition at line 96 of file trajectory_manager.c.
References trajectory::a_start_rad, trajectory::a_win_rad, trajectory::d_win, IRQ_LOCK, IRQ_UNLOCK, and RAD.
| void trajectory_stop | ( | struct trajectory * | traj | ) |
set relative angle and distance consign to 0
Definition at line 340 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), READY, RESET_A, RESET_D, trajectory::state, UPDATE_A, and UPDATE_D.
| void trajectory_turnto_xy | ( | struct trajectory * | traj, | |
| double | x_abs_cm, | |||
| double | y_abs_cm | |||
| ) |
turn the robot until the point x,y is in front of us
Definition at line 298 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), DEBUG, E_TRAJECTORY, trajectory::position, position_get_a_rad_double(), position_get_x_double(), position_get_y_double(), RESET_D, RUNNING_A, trajectory::state, UPDATE_A, and UPDATE_D.
| void trajectory_turnto_xy_behind | ( | struct trajectory * | traj, | |
| double | x_abs_cm, | |||
| double | y_abs_cm | |||
| ) |
turn the robot until the point x,y is behind us
Definition at line 312 of file trajectory_manager.c.
References __trajectory_goto_d_a_rel(), DEBUG, E_TRAJECTORY, trajectory::position, position_get_a_rad_double(), position_get_x_double(), position_get_y_double(), RESET_D, RUNNING_A, trajectory::state, UPDATE_A, and UPDATE_D.
1.5.6