00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef COMMAND_H
00022 #define COMMAND_H
00023
00024 #include <ipcommunication.h>
00025 #include <ypparam.h>
00026 #include <ypspur.h>
00027
00028 typedef enum
00029 {
00030 RUN_OPENFREE = -1,
00031 RUN_FREE = 0,
00032 RUN_STOP,
00033 RUN_LINEFOLLOW,
00034 RUN_STOP_LINE,
00035 RUN_CIRCLEFOLLOW,
00036 RUN_SPIN,
00037 RUN_ORIENT,
00038 RUN_VEL,
00039 RUN_WHEEL_VEL,
00040 RUN_WHEEL_TORQUE,
00041 RUN_WHEEL_ANGLE
00042 } SpurRunMode;
00043
00044 typedef enum
00045 {
00046 MOTOR_CONTROL_OPENFREE,
00047 MOTOR_CONTROL_FREE,
00048 MOTOR_CONTROL_VEHICLE,
00049 MOTOR_CONTROL_VEL,
00050 MOTOR_CONTROL_ANGLE,
00051 MOTOR_CONTROL_ANGLE_VEL
00052 } MotorControlMode;
00053
00054 typedef struct _spur_user_params *SpurUserParamsPtr;
00055 typedef struct _spur_user_params
00056 {
00057 double dv;
00058 double dw;
00059 double v;
00060 double w;
00061 double vref;
00062 double wref;
00063 double vref_smooth;
00064 double wref_smooth;
00065 double wvelref[YP_PARAM_MAX_MOTOR_NUM];
00066 double x;
00067 double y;
00068 double torque[YP_PARAM_MAX_MOTOR_NUM];
00069 double torque_prev[YP_PARAM_MAX_MOTOR_NUM];
00070 double grav_torque[YP_PARAM_MAX_MOTOR_NUM];
00071 double wheel_accel[YP_PARAM_MAX_MOTOR_NUM];
00072 double wheel_vel[YP_PARAM_MAX_MOTOR_NUM];
00073 double wheel_vel_smooth[YP_PARAM_MAX_MOTOR_NUM];
00074 double wheel_angle[YP_PARAM_MAX_MOTOR_NUM];
00075 double wheel_vel_end[YP_PARAM_MAX_MOTOR_NUM];
00076 MotorControlMode wheel_mode[YP_PARAM_MAX_MOTOR_NUM];
00077 MotorControlMode wheel_mode_prev[YP_PARAM_MAX_MOTOR_NUM];
00078 double theta;
00079 double radius;
00080 double tilt;
00081 double dir;
00082 double control_dt;
00083 int freeze;
00084 int before_freeze;
00085 SpurRunMode run_mode;
00086 int run_mode_cnt;
00087 SpurRunMode before_run_mode;
00088 pthread_mutex_t mutex;
00089 } SpurUserParams;
00090
00091 void init_spur_command(void);
00092 void command(void);
00093 void message_return(struct ipcmd_t *ipcmd, long retpid, YPSpur_msg *res_msg);
00094
00095 void init_command_thread(pthread_t *thread);
00096 SpurUserParamsPtr get_spur_user_param_ptr();
00097
00098
00099 void line_com(int cs, double *data, SpurUserParamsPtr spur);
00100 void stop_line_com(int cs, double *data, SpurUserParamsPtr spur);
00101 void circle_com(int cs, double *data, SpurUserParamsPtr spur);
00102 void spin_com(int cs, double *data, SpurUserParamsPtr spur);
00103 void orient_com(int cs, double *data, SpurUserParamsPtr spur);
00104 void stop_com(double *data, SpurUserParamsPtr spur);
00105 void free_com(double *data, SpurUserParamsPtr spur);
00106 void openfree_com(double *data, SpurUserParamsPtr spur);
00107 void vel_com(double *data, SpurUserParamsPtr spur);
00108 void wheel_vel_com(double *data, SpurUserParamsPtr spur);
00109 void wheel_angle_com(double *data, SpurUserParamsPtr spur);
00110
00111
00112 void set_pos_com(int cs, double *data, SpurUserParamsPtr spur);
00113 void set_GL_on_GL_com(double *data, SpurUserParamsPtr spur);
00114 void set_adjust_com(int cs, double *data, SpurUserParamsPtr spur);
00115 void set_vel_com(double *data, SpurUserParamsPtr spur);
00116 void set_ang_accel_com(double *data, SpurUserParamsPtr spur);
00117 void set_accel_com(double *data, SpurUserParamsPtr spur);
00118 void set_ang_vel_com(double *data, SpurUserParamsPtr spur);
00119 void set_tilt_com(int cs, double *data, SpurUserParamsPtr spur);
00120 void set_torque_com(double *data, SpurUserParamsPtr spur);
00121 void set_wheel_vel_com(double *data, SpurUserParamsPtr spur);
00122 void set_wheel_accel_com(double *data, SpurUserParamsPtr spur);
00123
00124
00125 void get_pos_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur);
00126 void get_vel_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur);
00127 void get_vref_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur);
00128 void get_wheel_vref_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur);
00129 int near_pos_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur);
00130 int near_ang_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur);
00131 int over_line_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur);
00132 void get_wheel_vel_com(double *data, double *resdata, SpurUserParamsPtr spur);
00133 void get_wheel_ang_com(double *data, double *resdata, SpurUserParamsPtr spur);
00134 void get_force_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur);
00135 void get_wheel_torque_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur);
00136
00137
00138 void param_set_com(int cs, double *data, SpurUserParamsPtr spur);
00139 int param_get_com(int cs, double *resdata, SpurUserParamsPtr spur);
00140 void param_state_com(int cs, double *data, SpurUserParamsPtr spur);
00141
00142
00143 void get_ad_com(double *data, double *resdata);
00144 void set_io_dir_com(double *data, double *resdata);
00145 void set_io_data_com(double *data, double *resdata);
00146 void get_error_state_com(double *data, double *resdata);
00147
00148 void joint_torque_com(int id, double *data, SpurUserParamsPtr spur);
00149 void joint_vel_com(int id, double *data, SpurUserParamsPtr spur);
00150 void joint_ang_com(int id, double *data, SpurUserParamsPtr spur);
00151 void joint_ang_vel_com(int id, double *data, SpurUserParamsPtr spur);
00152 void set_joint_accel_com(int id, double *data, SpurUserParamsPtr spur);
00153 void set_joint_vel_com(int id, double *data, SpurUserParamsPtr spur);
00154 void get_joint_vel_com(int id, double *data, SpurUserParamsPtr spur);
00155 void get_joint_vref_com(int id, double *data, SpurUserParamsPtr spur);
00156 void get_joint_ang_com(int id, double *data, SpurUserParamsPtr spur);
00157 void get_joint_torque_com(int id, double *data, SpurUserParamsPtr spur);
00158
00159 #endif // COMMAND_H