Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <math.h>
00022 #include <stdio.h>
00023 #include <strings.h>
00024 #include <unistd.h>
00025
00026 #include <fcntl.h>
00027 #include <sys/time.h>
00028 #include <sys/types.h>
00029 #include <time.h>
00030
00031 #ifdef HAVE_CONFIG_H
00032 #include <config.h>
00033 #endif // HAVE_CONFIG_H
00034
00035
00036 #include <shvel-param.h>
00037
00038
00039 #include <param.h>
00040 #include <control.h>
00041 #include <command.h>
00042 #include <yprintf.h>
00043
00044
00045 #include <ypspur.h>
00046
00047 void joint_torque_com(int id, double *data, SpurUserParamsPtr spur)
00048 {
00049 if (id > YP_PARAM_MAX_MOTOR_NUM)
00050 {
00051 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00052 return;
00053 }
00054 spur->torque[id] = data[0];
00055 spur->wheel_mode[id] = MOTOR_CONTROL_FREE;
00056 }
00057
00058 void joint_vel_com(int id, double *data, SpurUserParamsPtr spur)
00059 {
00060 if (id > YP_PARAM_MAX_MOTOR_NUM)
00061 {
00062 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00063 return;
00064 }
00065 spur->wvelref[id] = data[0];
00066 spur->wheel_mode[id] = MOTOR_CONTROL_VEL;
00067 }
00068
00069 void joint_ang_com(int id, double *data, SpurUserParamsPtr spur)
00070 {
00071 if (id > YP_PARAM_MAX_MOTOR_NUM)
00072 {
00073 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00074 return;
00075 }
00076 spur->wheel_angle[id] = data[0];
00077 spur->wheel_mode[id] = MOTOR_CONTROL_ANGLE;
00078 }
00079
00080 void joint_ang_vel_com(int id, double *data, SpurUserParamsPtr spur)
00081 {
00082 if (id > YP_PARAM_MAX_MOTOR_NUM)
00083 {
00084 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00085 return;
00086 }
00087 spur->wheel_angle[id] = data[0];
00088 spur->wheel_vel_end[id] = data[1];
00089 spur->wheel_mode[id] = MOTOR_CONTROL_ANGLE_VEL;
00090 }
00091
00092 void set_joint_accel_com(int id, double *data, SpurUserParamsPtr spur)
00093 {
00094 if (id > YP_PARAM_MAX_MOTOR_NUM)
00095 {
00096 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00097 return;
00098 }
00099 spur->wheel_accel[id] = data[0];
00100 }
00101
00102 void set_joint_vel_com(int id, double *data, SpurUserParamsPtr spur)
00103 {
00104 if (id > YP_PARAM_MAX_MOTOR_NUM)
00105 {
00106 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00107 return;
00108 }
00109 spur->wheel_vel[id] = data[0];
00110 }
00111
00112 void get_joint_vel_com(int id, double *data, SpurUserParamsPtr spur)
00113 {
00114 OdometryPtr odometry;
00115
00116 if (id > YP_PARAM_MAX_MOTOR_NUM)
00117 {
00118 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00119 return;
00120 }
00121 odometry = get_odometry_ptr();
00122
00123 data[0] = odometry->wvel[id];
00124 data[1] = odometry->time;
00125 }
00126
00127 void get_joint_vref_com(int id, double *data, SpurUserParamsPtr spur)
00128 {
00129 OdometryPtr odometry;
00130
00131 if (id > YP_PARAM_MAX_MOTOR_NUM)
00132 {
00133 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00134 return;
00135 }
00136 odometry = get_odometry_ptr();
00137
00138 data[0] = spur->wvelref[id];
00139 data[1] = odometry->time;
00140 }
00141
00142 void get_joint_ang_com(int id, double *data, SpurUserParamsPtr spur)
00143 {
00144 OdometryPtr odometry;
00145
00146 if (id > YP_PARAM_MAX_MOTOR_NUM)
00147 {
00148 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00149 return;
00150 }
00151 odometry = get_odometry_ptr();
00152
00153 data[0] = odometry->wang[id];
00154 data[1] = odometry->time;
00155 }
00156
00157 void get_joint_torque_com(int id, double *data, SpurUserParamsPtr spur)
00158 {
00159 OdometryPtr odometry;
00160
00161 if (id > YP_PARAM_MAX_MOTOR_NUM)
00162 {
00163 yprintf(OUTPUT_LV_ERROR, "Motor id out of range (%d)\n", id);
00164 return;
00165 }
00166 odometry = get_odometry_ptr();
00167
00168 data[0] = odometry->wtorque[id];
00169 data[1] = odometry->time;
00170 }