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 <serial.h>
00040 #include <param.h>
00041 #include <command.h>
00042 #include <odometry.h>
00043 #include <control.h>
00044 #include <utility.h>
00045 #include <yprintf.h>
00046
00047
00048 #include <ypspur.h>
00049 #include <cartesian2d.h>
00050
00051 void param_set_com(int cs, double *data, SpurUserParamsPtr spur)
00052 {
00053 if (cs >= 0 && cs < YP_PARAM_NUM)
00054 {
00055 *pp(cs, 0) = data[0];
00056 *pp(cs, 1) = data[1];
00057 }
00058 }
00059
00060 int param_get_com(int cs, double *resdata, SpurUserParamsPtr spur)
00061 {
00062 if (cs >= 0 && cs < YP_PARAM_NUM)
00063 {
00064 resdata[0] = *pp(cs, 0);
00065 resdata[1] = *pp(cs, 1);
00066 return cs;
00067 }
00068 else
00069 {
00070 return 0;
00071 }
00072 }
00073
00074 void param_set_motor_com(int cs, double *data, SpurUserParamsPtr spur)
00075 {
00076 if (cs >= 0 && cs < YP_PARAM_NUM)
00077 *pp(cs, data[0]) = data[1];
00078 }
00079
00080 int param_get_motor_com(int cs, double *resdata, SpurUserParamsPtr spur)
00081 {
00082 if (cs >= 0 && cs < YP_PARAM_NUM)
00083 {
00084 resdata[0] = *pp(cs, resdata[0]);
00085 return cs;
00086 }
00087 else
00088 {
00089 return 0;
00090 }
00091 }
00092
00093 void param_state_com(int cs, double *data, SpurUserParamsPtr spur)
00094 {
00095 OdometryPtr odometry;
00096
00097 odometry = get_odometry_ptr();
00098
00099 if (cs >= 0 && cs < YP_STATE_NUM)
00100 {
00101 if (data[0])
00102 {
00103 enable_state(cs);
00104 }
00105 else
00106 {
00107 disable_state(cs);
00108 }
00109 }
00110
00111 if (cs == YP_STATE_MOTOR && data[0] == ENABLE)
00112 {
00113 set_param_motor();
00114 yprintf(OUTPUT_LV_DEBUG, "COMMAND: Motor Control: enabled\n");
00115 }
00116 if (cs == YP_STATE_VELOCITY && data[0] == ENABLE)
00117 {
00118 set_param_velocity();
00119 yprintf(OUTPUT_LV_DEBUG, "COMMAND: Velocity Control: enabled\n");
00120 }
00121 if (cs == YP_STATE_BODY && data[0] == ENABLE)
00122 {
00123 spur->vref_smooth = 0;
00124 spur->wref_smooth = 0;
00125 robot_speed(spur);
00126 odometry->x = 0;
00127 odometry->y = 0;
00128 odometry->theta = 0;
00129 yprintf(OUTPUT_LV_DEBUG, "COMMAND: Body Control: enabled\n");
00130 }
00131 if (cs == YP_STATE_TRACKING && data[0] == ENABLE)
00132 {
00133 yprintf(OUTPUT_LV_DEBUG, "COMMAND: Trajectory Control: enabled\n");
00134 }
00135 if (cs == YP_STATE_GRAVITY && data[0] == ENABLE)
00136 {
00137 yprintf(OUTPUT_LV_DEBUG, "COMMAND: Gravity Compensation: enabled\n");
00138 }
00139 }