group_sync_read.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2017 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /* Author: zerom, Ryu Woon Jung (Leon) */
18 
19 #include <algorithm>
20 
21 #if defined(__linux__)
22 #include "group_sync_read.h"
23 #elif defined(__APPLE__)
24 #include "group_sync_read.h"
25 #elif defined(_WIN32) || defined(_WIN64)
26 #define WINDLLEXPORT
27 #include "group_sync_read.h"
28 #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__)
29 #include "../../include/dynamixel_sdk/group_sync_read.h"
30 #endif
31 
32 using namespace dynamixel;
33 
34 GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length)
35  : port_(port),
36  ph_(ph),
37  last_result_(false),
38  is_param_changed_(false),
39  param_(0),
40  start_address_(start_address),
41  data_length_(data_length)
42 {
43  clearParam();
44 }
45 
47 {
48  if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
49  return;
50 
51  if (param_ != 0)
52  delete[] param_;
53  param_ = 0;
54 
55  param_ = new uint8_t[id_list_.size() * 1]; // ID(1)
56 
57  int idx = 0;
58  for (unsigned int i = 0; i < id_list_.size(); i++)
59  param_[idx++] = id_list_[i];
60 }
61 
62 bool GroupSyncRead::addParam(uint8_t id)
63 {
64  if (ph_->getProtocolVersion() == 1.0)
65  return false;
66 
67  if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist
68  return false;
69 
70  id_list_.push_back(id);
71  data_list_[id] = new uint8_t[data_length_];
72  error_list_[id] = new uint8_t[1];
73 
74  is_param_changed_ = true;
75  return true;
76 }
78 {
79  if (ph_->getProtocolVersion() == 1.0)
80  return;
81 
82  std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
83  if (it == id_list_.end()) // NOT exist
84  return;
85 
86  id_list_.erase(it);
87  delete[] data_list_[id];
88  delete[] error_list_[id];
89  data_list_.erase(id);
90  error_list_.erase(id);
91 
92  is_param_changed_ = true;
93 }
95 {
96  if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
97  return;
98 
99  for (unsigned int i = 0; i < id_list_.size(); i++)
100  {
101  delete[] data_list_[id_list_[i]];
102  delete[] error_list_[id_list_[i]];
103  }
104 
105  id_list_.clear();
106  data_list_.clear();
107  error_list_.clear();
108  if (param_ != 0)
109  delete[] param_;
110  param_ = 0;
111 }
112 
114 {
115  if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
116  return COMM_NOT_AVAILABLE;
117 
118  if (is_param_changed_ == true || param_ == 0)
119  makeParam();
120 
121  return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1);
122 }
123 
125 {
126  last_result_ = false;
127 
128  if (ph_->getProtocolVersion() == 1.0)
129  return COMM_NOT_AVAILABLE;
130 
131  int cnt = id_list_.size();
132  int result = COMM_RX_FAIL;
133 
134  if (cnt == 0)
135  return COMM_NOT_AVAILABLE;
136 
137  for (int i = 0; i < cnt; i++)
138  {
139  uint8_t id = id_list_[i];
140 
141  result = ph_->readRx(port_, id, data_length_, data_list_[id], error_list_[id]);
142  if (result != COMM_SUCCESS)
143  return result;
144  }
145 
146  if (result == COMM_SUCCESS)
147  last_result_ = true;
148 
149  return result;
150 }
151 
153 {
154  if (ph_->getProtocolVersion() == 1.0)
155  return COMM_NOT_AVAILABLE;
156 
157  int result = COMM_TX_FAIL;
158 
159  result = txPacket();
160  if (result != COMM_SUCCESS)
161  return result;
162 
163  return rxPacket();
164 }
165 
166 bool GroupSyncRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length)
167 {
168  if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end())
169  return false;
170 
171  if (address < start_address_ || start_address_ + data_length_ - data_length < address)
172  return false;
173 
174  return true;
175 }
176 
177 uint32_t GroupSyncRead::getData(uint8_t id, uint16_t address, uint16_t data_length)
178 {
179  if (isAvailable(id, address, data_length) == false)
180  return 0;
181 
182  switch(data_length)
183  {
184  case 1:
185  return data_list_[id][address - start_address_];
186 
187  case 2:
188  return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]);
189 
190  case 4:
191  return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]),
192  DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3]));
193 
194  default:
195  return 0;
196  }
197 }
198 
199 bool GroupSyncRead::getError(uint8_t id, uint8_t* error)
200 {
201  // TODO : check protocol version, last_result_, data_list
202  // if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || error_list_.find(id) == error_list_.end())
203 
204  return error[0] = error_list_[id][0];
205 }
#define COMM_RX_FAIL
#define DXL_MAKEDWORD(a, b)
void clearParam()
The function that clears the Sync Read list.
int txRxPacket()
The function that transmits and receives the packet which might be come from the Dynamixel.
std::map< uint8_t, uint8_t * > data_list_
bool isAvailable(uint8_t id, uint16_t address, uint16_t data_length)
The function that checks whether there are available data which might be received by GroupSyncRead::r...
int rxPacket()
The function that receives the packet which might be come from the Dynamixel.
#define COMM_SUCCESS
#define DXL_MAKEWORD(a, b)
bool addParam(uint8_t id)
The function that adds id, start_address, data_length to the Sync Read list.
void removeParam(uint8_t id)
The function that removes id from the Sync Read list.
uint32_t getData(uint8_t id, uint16_t address, uint16_t data_length)
The function that gets the data which might be received by GroupSyncRead::rxPacket or GroupSyncRead::...
#define COMM_TX_FAIL
int txPacket()
The function that transmits the Sync Read instruction packet which might be constructed by GroupSyncR...
The class that inherits Protocol1PacketHandler class or Protocol2PacketHandler class.
bool getError(uint8_t id, uint8_t *error)
The function that gets the error which might be received by GroupSyncRead::rxPacket or GroupSyncRead:...
The class for port control that inherits PortHandlerLinux, PortHandlerWindows, PortHandlerMac, or PortHandlerArduino.
Definition: port_handler.h:56
virtual float getProtocolVersion()=0
The function that returns Protocol version.
virtual int readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error=0)=0
The function that receives the packet and reads the data in the packet The function receives the pac...
#define COMM_NOT_AVAILABLE
std::vector< uint8_t > id_list_
std::map< uint8_t, uint8_t * > error_list_
virtual int syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)=0
The function that transmits INST_SYNC_READ instruction packet The function makes an instruction pack...
GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length)
The function that Initializes instance for Sync Read.


dynamixel_sdk
Author(s): Gilbert , Zerom , Darby Lim , Leon
autogenerated on Fri Apr 16 2021 02:25:55