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 #include <algorithm>
00020
00021 #if defined(__linux__)
00022 #include "group_bulk_write.h"
00023 #elif defined(__APPLE__)
00024 #include "group_bulk_write.h"
00025 #elif defined(_WIN32) || defined(_WIN64)
00026 #define WINDLLEXPORT
00027 #include "group_bulk_write.h"
00028 #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__)
00029 #include "../../include/dynamixel_sdk/group_bulk_write.h"
00030 #endif
00031
00032 using namespace dynamixel;
00033
00034 GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph)
00035 : port_(port),
00036 ph_(ph),
00037 is_param_changed_(false),
00038 param_(0),
00039 param_length_(0)
00040 {
00041 clearParam();
00042 }
00043
00044 void GroupBulkWrite::makeParam()
00045 {
00046 if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
00047 return;
00048
00049 if (param_ != 0)
00050 delete[] param_;
00051 param_ = 0;
00052
00053 param_length_ = 0;
00054 for (unsigned int i = 0; i < id_list_.size(); i++)
00055 param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]];
00056
00057 param_ = new uint8_t[param_length_];
00058
00059 int idx = 0;
00060 for (unsigned int i = 0; i < id_list_.size(); i++)
00061 {
00062 uint8_t id = id_list_[i];
00063 if (data_list_[id] == 0)
00064 return;
00065
00066 param_[idx++] = id;
00067 param_[idx++] = DXL_LOBYTE(address_list_[id]);
00068 param_[idx++] = DXL_HIBYTE(address_list_[id]);
00069 param_[idx++] = DXL_LOBYTE(length_list_[id]);
00070 param_[idx++] = DXL_HIBYTE(length_list_[id]);
00071 for (int c = 0; c < length_list_[id]; c++)
00072 param_[idx++] = (data_list_[id])[c];
00073 }
00074 }
00075
00076 bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data)
00077 {
00078 if (ph_->getProtocolVersion() == 1.0)
00079 return false;
00080
00081 if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end())
00082 return false;
00083
00084 id_list_.push_back(id);
00085 address_list_[id] = start_address;
00086 length_list_[id] = data_length;
00087 data_list_[id] = new uint8_t[data_length];
00088 for (int c = 0; c < data_length; c++)
00089 data_list_[id][c] = data[c];
00090
00091 is_param_changed_ = true;
00092 return true;
00093 }
00094 void GroupBulkWrite::removeParam(uint8_t id)
00095 {
00096 if (ph_->getProtocolVersion() == 1.0)
00097 return;
00098
00099 std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
00100 if (it == id_list_.end())
00101 return;
00102
00103 id_list_.erase(it);
00104 address_list_.erase(id);
00105 length_list_.erase(id);
00106 delete[] data_list_[id];
00107 data_list_.erase(id);
00108
00109 is_param_changed_ = true;
00110 }
00111 bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data)
00112 {
00113 if (ph_->getProtocolVersion() == 1.0)
00114 return false;
00115
00116 std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
00117 if (it == id_list_.end())
00118 return false;
00119
00120 address_list_[id] = start_address;
00121 length_list_[id] = data_length;
00122 delete[] data_list_[id];
00123 data_list_[id] = new uint8_t[data_length];
00124 for (int c = 0; c < data_length; c++)
00125 data_list_[id][c] = data[c];
00126
00127 is_param_changed_ = true;
00128 return true;
00129 }
00130 void GroupBulkWrite::clearParam()
00131 {
00132 if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
00133 return;
00134
00135 for (unsigned int i = 0; i < id_list_.size(); i++)
00136 delete[] data_list_[id_list_[i]];
00137
00138 id_list_.clear();
00139 address_list_.clear();
00140 length_list_.clear();
00141 data_list_.clear();
00142 if (param_ != 0)
00143 delete[] param_;
00144 param_ = 0;
00145 }
00146 int GroupBulkWrite::txPacket()
00147 {
00148 if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
00149 return COMM_NOT_AVAILABLE;
00150
00151 if (is_param_changed_ == true || param_ == 0)
00152 makeParam();
00153
00154 return ph_->bulkWriteTxOnly(port_, param_, param_length_);
00155 }