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