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
00033 #include "joint_motion_handler.h"
00034 #include "joint_message.h"
00035 #include "log_wrapper.h"
00036
00037
00038 using namespace industrial::joint_message;
00039 using namespace industrial::joint_data;
00040 using namespace industrial::simple_message;
00041
00042 namespace motoman
00043 {
00044 namespace joint_motion_handler
00045 {
00046
00047 #define JOBNAME "PVARQ20VV"
00048
00049 JointMotionHandler::JointMotionHandler()
00050 {
00051
00052
00053 job_start_data.sTaskNo = 0;
00054 strcpy(job_start_data.cJobName, JOBNAME);
00055
00056
00057 strcpy(job_reset_data.cJobName, JOBNAME);
00058
00059 this->jobStarted = false;
00060 this->motionEnabled = false;
00061 }
00062
00063 JointMotionHandler::~JointMotionHandler()
00064 {
00065 this->disableMotion();
00066 this->stopMotionJob();
00067 }
00068
00069 bool JointMotionHandler::init(industrial::smpl_msg_connection::SmplMsgConnection* connection)
00070 {
00071 return this->init(StandardMsgTypes::JOINT_POSITION, connection);
00072 }
00073
00074 bool JointMotionHandler::internalCB(industrial::simple_message::SimpleMessage & in)
00075 {
00076 bool rtn = false;
00077 JointMessage jMsg;
00078 SimpleMessage reply;
00079
00080 if (jMsg.init(in))
00081 {
00082 this->motionInterface(jMsg);
00083 }
00084 else
00085 {
00086 LOG_ERROR("Failed to initialize joint message");
00087 rtn = false;
00088 }
00089
00090
00091 if (CommTypes::SERVICE_REQUEST == in.getCommType())
00092 if (jMsg.toReply(reply, ReplyTypes::SUCCESS))
00093 {
00094 if(this->getConnection()->sendMsg(reply))
00095 {
00096 LOG_INFO("Joint ack sent");
00097 rtn = true;
00098 }
00099 else
00100 {
00101 LOG_ERROR("Failed to send joint ack");
00102 rtn = false;
00103 }
00104 }
00105 else
00106 {
00107 LOG_ERROR("Failed to generate joint ack message");
00108 rtn = false;
00109 }
00110
00111 return rtn;
00112 }
00113
00114
00115 void JointMotionHandler::motionInterface(JointMessage & jMsg)
00116 {
00117
00118
00119
00120
00121
00122
00123
00124 JointData joints;
00125
00126 switch (jMsg.getSequence())
00127 {
00128 case SpecialSeqValues::END_TRAJECTORY:
00129 LOG_INFO("Received end trajectory command");
00130 while(!pVarQ.bufferEmpty())
00131 {
00132 LOG_DEBUG("Waiting for motion buffer to empty");
00133 mpTaskDelay(this->BUFFER_POLL_TICK_DELAY);
00134 };
00135 this->stopMotionJob();
00136
00137 break;
00138
00139 case SpecialSeqValues::STOP_TRAJECTORY:
00140 LOG_WARN("Received stop command, stopping motion immediately");
00141 this->stopMotionJob();
00142 break;
00143
00144 default:
00145
00146 joints.copyFrom(jMsg.getJoints());
00147 if (!(this->isJobStarted()))
00148 {
00149
00150 pVarQ.init(joints, 0.0);
00151 this->startMotionJob();
00152 }
00153 else
00154 {
00155 pVarQ.addPoint(joints, 0.0);
00156 }
00157 }
00158 }
00159
00160
00161 void JointMotionHandler::enableMotion(void)
00162 {
00163
00164 LOG_INFO("Enabling motion");
00165 this->motionEnabled = false;
00166
00167 servo_power_data.sServoPower = ON;
00168 while(mpSetServoPower(&servo_power_data, &servo_power_error) == ERROR)
00169 {
00170 LOG_ERROR("Failed to turn on servo power, error: %d, retrying...", servo_power_error.err_no);
00171 mpTaskDelay(this->MP_POLL_TICK_DELAY);
00172 };
00173
00174 hold_data.sHold = OFF;
00175 while(mpHold(&hold_data, &hold_error) == ERROR)
00176 {
00177 LOG_ERROR("Failed to turn off hold, error: %d, retrying...", hold_error.err_no);
00178 mpTaskDelay(this->MP_POLL_TICK_DELAY);
00179 };
00180
00181 this->motionEnabled = true;
00182 }
00183
00184
00185 void JointMotionHandler::disableMotion(void)
00186 {
00187 LOG_INFO("Disabling motion");
00188 servo_power_data.sServoPower = OFF;
00189 while(mpSetServoPower(&servo_power_data, &servo_power_error) == ERROR)
00190 {
00191 LOG_ERROR("Failed to turn off servo power, error: %d, retrying...", servo_power_error.err_no);
00192 mpTaskDelay(this->MP_POLL_TICK_DELAY);
00193 };
00194
00195 this->motionEnabled = false;
00196 }
00197
00198 void JointMotionHandler::startMotionJob(void)
00199 {
00200
00201 this->jobStarted = false;
00202
00203 this->enableMotion();
00204
00205 LOG_INFO("Starting motion job");
00206 while(mpStartJob(&job_start_data, &job_error) == ERROR)
00207 {
00208 LOG_ERROR("Failed to start job, error: %d, retrying...", job_error.err_no);
00209 mpTaskDelay(this->MP_POLL_TICK_DELAY);
00210 };
00211
00212 LOG_DEBUG("Waiting for indexes to reset");
00213
00214 while( (pVarQ.getMotionPosIndex() != 0) && ( pVarQ.getBufferPosIndex() != 0))
00215 {
00216 LOG_ERROR("Waiting for indexes to reset, retying...");
00217 mpTaskDelay(this->MP_POLL_TICK_DELAY);
00218 };
00219
00220 LOG_DEBUG("Reset indexes, motion: %d, buffer: %d", pVarQ.getMotionPosIndex(),
00221 pVarQ.getBufferPosIndex());
00222
00223 this->jobStarted = true;
00224 }
00225 void JointMotionHandler::stopMotionJob(void)
00226 {
00227 LOG_INFO("Stopping motion job");
00228 this->disableMotion();
00229
00230
00231 job_reset_data.usJobLine = 1;
00232 while(mpSetCurJob(&job_reset_data, &job_reset_error) == ERROR)
00233 {
00234 LOG_ERROR("Failed to reset job, error: %d, retrying...", job_reset_error.err_no);
00235 mpTaskDelay(this->MP_POLL_TICK_DELAY);
00236 };
00237
00238 this->jobStarted = false;
00239 }
00240
00241 }
00242 }
00243
00244