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
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "p_var_q.h"
00033 #include "controller.h"
00034 #include "joint_data.h"
00035 #include "joint_motion_handler.h"
00036 #include "ros_conversion.h"
00037 #include "log_wrapper.h"
00038
00039 using motoman::controller::Controller;
00040 using motoman::joint_motion_handler;
00041 using motoman::ros_conversion;
00042 using industrial::joint_data;
00043
00044
00045 namespace motoman
00046 {
00047 namespace p_var_q
00048 {
00049
00050
00051 PVarQ::PVarQ()
00052 {
00053
00054
00055
00056
00057 STATIC_ASSERT(QSIZE_ > PT_LOOK_AHEAD_, QSIZE_NOT_GREATER_THAN_LOOK_AHEAD);
00058 STATIC_ASSERT(MOTION_POINTER_ > QSIZE_, MOTION_PONTER_NOT_GREATER_THAN_QSIZE);
00059 STATIC_ASSERT(BUFFER_POINTER_ > QSIZE_, BUFFER_PONTER_NOT_GREATER_THAN_QSIZE);
00060 STATIC_ASSERT(MIN_BUF_START_POINTER_ > QSIZE_, MIN_BUFF_START_PONTER_NOT_GREATER_THAN_QSIZE);
00061
00062 }
00063
00064 PVarQ::~PVarQ(void)
00065 {
00066 }
00067
00068
00069 void PVarQ::init(industrial::joint_data::JointData & point, double velocity_percent)
00070 {
00071
00072
00073 industrial::joint_data::JointData empty;
00074 for(int i = 0; i < this->posVarQueueSize(); i++)
00075 {
00076 this->setPosition(i, empty, 0);
00077 }
00078
00079
00080
00081 setPosition(0, point, this->TEMP_getVelocityPercent());
00082
00083
00084 Controller::setInteger(MIN_BUF_START_POINTER_, PT_LOOK_AHEAD_);
00085 }
00086
00087 void PVarQ::addPoint(industrial::joint_data::JointData & joints, double velocity_percent)
00088 {
00089
00090
00091
00092 LOG_DEBUG("Checking for buffer full");
00093 while(this->bufferFull()) {
00094 mpTaskDelay(this->BUFFER_POLL_TICK_DELAY_);
00095 };
00096 LOG_DEBUG("Buffer ready to accept new points");
00097
00098 setNextPosition(joints, this->TEMP_getVelocityPercent());
00099 incBufferIndex();
00100
00101 }
00102
00103 int PVarQ::bufferSize()
00104 {
00105 int motionIdx = this->getMotionIndex();
00106 int bufferIdx = this->getBufferIndex();
00107 int rtn = 0;
00108
00109 if (motionIdx > bufferIdx)
00110 {
00111 LOG_ERROR("Motion index: %d is greater than Buffer index: %d, returning empty buffer size", motionIdx, bufferIdx);
00112 rtn = 0;
00113 }
00114 else
00115 {
00116 rtn = bufferIdx - motionIdx;
00117 }
00118
00119 return rtn;
00120 }
00121
00122
00123
00124 int PVarQ::getMotionPosIndex()
00125 {
00126 return (this->getMotionIndex() % this->posVarQueueSize());
00127 }
00128
00129 int PVarQ::getBufferPosIndex()
00130 {
00131 return (this->getBufferIndex() % this->posVarQueueSize());
00132 }
00133
00134 int PVarQ::getNextBufferPosIndex()
00135 {
00136 return ((this->getBufferIndex() + 1) % this->posVarQueueSize());
00137 }
00138
00139 bool PVarQ::bufferFull()
00140 {
00141 bool rtn = false;
00142 int bufferSize = this->bufferSize();
00143 int maxBufferSize = this->maxBufferSize();
00144 if (bufferSize >= maxBufferSize)
00145 {
00146 rtn = true;
00147 }
00148 return rtn;
00149 }
00150
00151 bool PVarQ::bufferEmpty()
00152 {
00153 bool rtn = false;
00154 if (this->bufferSize() <= 0)
00155 {
00156 LOG_DEBUG("Buffer is empty");
00157 rtn = true;
00158 }
00159 return rtn;
00160 }
00161
00162 void PVarQ::incBufferIndex()
00163 {
00164 int bufferIdx = this->getBufferIndex();
00165
00166 LOG_DEBUG("Incrementing buffer index from %d to %d", bufferIdx, bufferIdx + 1);
00167 Controller::setInteger(BUFFER_POINTER_, bufferIdx + 1);
00168 }
00169
00170
00171 void PVarQ::setNextPosition(industrial::joint_data::JointData & point, double velocity_percent)
00172 {
00173 setPosition(this->getNextBufferPosIndex(), point, velocity_percent);
00174 }
00175
00176
00177 void PVarQ::setPosition(int index, industrial::joint_data::JointData & point,
00178 double velocity_percent)
00179 {
00180 const double VELOCITY_CONVERSION = 100.0;
00181 int convertedVelocity = 0;
00182
00183 LOG_DEBUG("Setting joint position, index: %d", index);
00184
00185 while (!Controller::setJointPositionVar(index, point)) {
00186 LOG_ERROR("Failed set position variable, index: %d, retrying...", index);
00187 mpTaskDelay(this->VAR_POLL_TICK_DELAY_);
00188 };
00189
00190 convertedVelocity = (int)(velocity_percent * VELOCITY_CONVERSION);
00191 LOG_DEBUG("Converting percent velocity: %g to motoman integer value: %d",
00192 velocity_percent, convertedVelocity);
00193 LOG_DEBUG("Setting velocity, index: %d, value: %d", index, convertedVelocity);
00194 Controller::setInteger(index, convertedVelocity);
00195 }
00196
00197
00198
00199 }
00200 }