00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef TRAJECTORY_MANAGER
00023 #define TRAJECTORY_MANAGER
00024
00025 #include <aversive.h>
00026 #include <vect2.h>
00027 #include <robot_system.h>
00028
00029 enum trajectory_state {
00030 READY,
00031
00032
00033 RUNNING_A,
00034 RUNNING_D,
00035 RUNNING_AD,
00036
00037
00038 RUNNING_XY_START,
00039 RUNNING_XY_ANGLE,
00040 RUNNING_XY_ANGLE_OK,
00041 RUNNING_XY_F_START,
00042 RUNNING_XY_F_ANGLE,
00043 RUNNING_XY_F_ANGLE_OK,
00044 };
00045
00046
00047 struct trajectory {
00048 enum trajectory_state state;
00049
00050 union {
00051 vect2_cart cart;
00052 struct rs_polar pol;
00053 } target;
00054
00055 double d_win;
00056 double a_win_rad;
00057 double a_start_rad;
00060 uint16_t d_speed;
00061 uint16_t a_speed;
00063 struct robot_position * position;
00064 struct robot_system * robot;
00065 struct cs * csm_angle;
00066 struct cs * csm_distance;
00068 int8_t scheduler_task;
00069 };
00070
00072 void trajectory_init(struct trajectory * traj);
00073
00075 void trajectory_set_cs(struct trajectory * traj, struct cs * cs_d,
00076 struct cs * cs_a);
00077
00079 void trajectory_set_robot_params(struct trajectory * traj,
00080 struct robot_system * rs,
00081 struct robot_position * pos) ;
00082
00084 void trajectory_set_speed(struct trajectory * traj, int16_t d_speed, int16_t a_speed);
00085
00094 void trajectory_set_windows(struct trajectory * traj, double d_win,
00095 double a_win_deg, double a_start_deg);
00096
00100 uint8_t trajectory_finished(struct trajectory* traj);
00101
00104 uint8_t trajectory_in_window(struct trajectory* traj, double d_win, double a_win_rad);
00105
00106
00107
00109 void trajectory_stop(struct trajectory * traj);
00110
00113 void trajectory_hardstop(struct trajectory * traj);
00114
00116 void trajectory_d_rel(struct trajectory * traj, double d_cm);
00117
00119 void trajectory_only_d_rel(struct trajectory * traj, double d_cm);
00120
00122 void trajectory_a_rel(struct trajectory * traj, double a_deg);
00123
00125 void trajectory_a_abs(struct trajectory * traj, double a_deg);
00126
00128 void trajectory_turnto_xy(struct trajectory* traj, double x_abs_cm, double y_abs_cm);
00129
00131 void trajectory_turnto_xy_behind(struct trajectory* traj, double x_abs_cm, double y_abs_cm);
00132
00134 void trajectory_only_a_rel(struct trajectory * traj, double a_deg);
00135
00137 void trajectory_d_a_rel(struct trajectory * traj, double d_cm, double a_deg);
00138
00139
00140
00142 void trajectory_goto_xy_abs(struct trajectory * traj, double x_abs_cm, double y_abs_cm);
00143
00145 void trajectory_goto_forward_xy_abs(struct trajectory * traj, double x_abs_cm, double y_abs_cm);
00146
00148 void trajectory_goto_d_a_rel(struct trajectory* traj, double d, double a);
00149
00151 void trajectory_goto_xy_rel(struct trajectory* traj, double x_rel_cm, double y_rel_cm);
00152
00153 #endif //TRAJECTORY_MANAGER