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 #ifdef ROS
00032 #include "fs100/simple_message/motoman_motion_ctrl.h"
00033 #include "simple_message/shared_types.h"
00034 #include "simple_message/log_wrapper.h"
00035 #endif
00036
00037 #ifdef MOTOPLUS
00038 #include "motoman_motion_ctrl.h"
00039 #include "shared_types.h"
00040 #include "log_wrapper.h"
00041 #endif
00042
00043 using namespace industrial::shared_types;
00044
00045 namespace motoman
00046 {
00047 namespace simple_message
00048 {
00049 namespace motion_ctrl
00050 {
00051
00052 MotionCtrl::MotionCtrl(void)
00053 {
00054 this->init();
00055 }
00056 MotionCtrl::~MotionCtrl(void)
00057 {
00058
00059 }
00060
00061 void MotionCtrl::init()
00062 {
00063 this->init(0, 0, MotionControlCmds::UNDEFINED, 0);
00064 }
00065
00066 void MotionCtrl::init(shared_int robot_id, shared_int sequence,
00067 MotionControlCmd command, shared_real data)
00068 {
00069 this->setRobotID(robot_id);
00070 this->setSequence(sequence);
00071 this->setCommand(command);
00072 this->clearData();
00073 this->setData(0,data);
00074 }
00075
00076 void MotionCtrl::copyFrom(MotionCtrl &src)
00077 {
00078 this->setRobotID(src.getRobotID());
00079 this->setSequence(src.getSequence());
00080 this->setCommand(src.getCommand());
00081 for (size_t i=0; i<MAX_DATA_CNT; ++i)
00082 this->setData(i, src.getData(i));
00083 }
00084
00085 bool MotionCtrl::operator==(MotionCtrl &rhs)
00086 {
00087 bool rslt = this->robot_id_ == rhs.robot_id_ &&
00088 this->sequence_ == rhs.sequence_ &&
00089 this->command_ == rhs.command_;
00090
00091 for (size_t i=0; i<MAX_DATA_CNT; ++i)
00092 rslt &= (this->data_[i] == rhs.data_[i]);
00093
00094 return rslt;
00095 }
00096
00097 bool MotionCtrl::load(industrial::byte_array::ByteArray *buffer)
00098 {
00099 LOG_COMM("Executing MotionCtrl command load");
00100
00101 if (!buffer->load(this->robot_id_))
00102 {
00103 LOG_ERROR("Failed to load MotionCtrl robot_id");
00104 return false;
00105 }
00106
00107 if (!buffer->load(this->sequence_))
00108 {
00109 LOG_ERROR("Failed to load MotionCtrl sequence");
00110 return false;
00111 }
00112
00113 if (!buffer->load(this->command_))
00114 {
00115 LOG_ERROR("Failed to load MotionCtrl command");
00116 return false;
00117 }
00118
00119 for (size_t i=0; i<MAX_DATA_CNT; ++i)
00120 {
00121 shared_real value = this->getData(i);
00122 if (!buffer->load(value))
00123 {
00124 LOG_ERROR("Failed to load MotionCtrl data element %d from data[%d]", (int)i, buffer->getBufferSize());
00125 return false;
00126 }
00127 }
00128
00129 LOG_COMM("MotionCtrl data successfully loaded");
00130 return true;
00131 }
00132
00133 bool MotionCtrl::unload(industrial::byte_array::ByteArray *buffer)
00134 {
00135 LOG_COMM("Executing MotionCtrl command unload");
00136
00137 for (int i=MAX_DATA_CNT-1; i>=0; --i)
00138 {
00139 shared_real value;
00140 if (!buffer->unload(value))
00141 {
00142 LOG_ERROR("Failed to unload message data element: %d from data[%d]", (int)i, buffer->getBufferSize());
00143 return false;
00144 }
00145 this->setData(i, value);
00146 }
00147
00148 if (!buffer->unload(this->command_))
00149 {
00150 LOG_ERROR("Failed to unload MotionCtrl command");
00151 return false;
00152 }
00153
00154 if (!buffer->unload(this->sequence_))
00155 {
00156 LOG_ERROR("Failed to unload MotionCtrl sequence");
00157 return false;
00158 }
00159
00160 if (!buffer->unload(this->robot_id_))
00161 {
00162 LOG_ERROR("Failed to unload MotionCtrl robot_id");
00163 return false;
00164 }
00165
00166 LOG_COMM("MotionCtrl data successfully unloaded");
00167 return true;
00168 }
00169
00170 }
00171 }
00172 }