group_bulk_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 <stdio.h>
20 #include <algorithm>
21 
22 #if defined(__linux__)
23 #include "group_bulk_read.h"
24 #elif defined(__APPLE__)
25 #include "group_bulk_read.h"
26 #elif defined(_WIN32) || defined(_WIN64)
27 #define WINDLLEXPORT
28 #include "group_bulk_read.h"
29 #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__)
30 #include "../../include/dynamixel_sdk/group_bulk_read.h"
31 #endif
32 
33 using namespace dynamixel;
34 
36  : port_(port),
37  ph_(ph),
38  last_result_(false),
39  is_param_changed_(false),
40  param_(0)
41 {
42  clearParam();
43 }
44 
46 {
47  if (id_list_.size() == 0)
48  return;
49 
50  if (param_ != 0)
51  delete[] param_;
52  param_ = 0;
53 
54  if (ph_->getProtocolVersion() == 1.0)
55  {
56  param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1)
57  }
58  else // 2.0
59  {
60  param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2)
61  }
62 
63  int idx = 0;
64  for (unsigned int i = 0; i < id_list_.size(); i++)
65  {
66  uint8_t id = id_list_[i];
67  if (ph_->getProtocolVersion() == 1.0)
68  {
69  param_[idx++] = (uint8_t)length_list_[id]; // LEN
70  param_[idx++] = id; // ID
71  param_[idx++] = (uint8_t)address_list_[id]; // ADDR
72  }
73  else // 2.0
74  {
75  param_[idx++] = id; // ID
76  param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L
77  param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H
78  param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L
79  param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H
80  }
81  }
82 }
83 
84 bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_length)
85 {
86  if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist
87  return false;
88 
89  id_list_.push_back(id);
90  length_list_[id] = data_length;
91  address_list_[id] = start_address;
92  data_list_[id] = new uint8_t[data_length];
93  error_list_[id] = new uint8_t[1];
94 
95  is_param_changed_ = true;
96  return true;
97 }
98 
100 {
101  std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
102  if (it == id_list_.end()) // NOT exist
103  return;
104 
105  id_list_.erase(it);
106  address_list_.erase(id);
107  length_list_.erase(id);
108  delete[] data_list_[id];
109  delete[] error_list_[id];
110  data_list_.erase(id);
111  error_list_.erase(id);
112 
113  is_param_changed_ = true;
114 }
115 
117 {
118  if (id_list_.size() == 0)
119  return;
120 
121  for (unsigned int i = 0; i < id_list_.size(); i++)
122  {
123  delete[] data_list_[id_list_[i]];
124  delete[] error_list_[id_list_[i]];
125  }
126 
127  id_list_.clear();
128  address_list_.clear();
129  length_list_.clear();
130  data_list_.clear();
131  error_list_.clear();
132  if (param_ != 0)
133  delete[] param_;
134  param_ = 0;
135 }
136 
138 {
139  if (id_list_.size() == 0)
140  return COMM_NOT_AVAILABLE;
141 
142  if (is_param_changed_ == true || param_ == 0)
143  makeParam();
144 
145  if (ph_->getProtocolVersion() == 1.0)
146  {
147  return ph_->bulkReadTx(port_, param_, id_list_.size() * 3);
148  }
149  else // 2.0
150  {
151  return ph_->bulkReadTx(port_, param_, id_list_.size() * 5);
152  }
153 }
154 
156 {
157  int cnt = id_list_.size();
158  int result = COMM_RX_FAIL;
159 
160  last_result_ = false;
161 
162  if (cnt == 0)
163  return COMM_NOT_AVAILABLE;
164 
165  for (int i = 0; i < cnt; i++)
166  {
167  uint8_t id = id_list_[i];
168 
169  result = ph_->readRx(port_, id, length_list_[id], data_list_[id], error_list_[id]);
170  if (result != COMM_SUCCESS)
171  return result;
172  }
173 
174  if (result == COMM_SUCCESS)
175  last_result_ = true;
176 
177  return result;
178 }
179 
181 {
182  int result = COMM_TX_FAIL;
183 
184  result = txPacket();
185  if (result != COMM_SUCCESS)
186  return result;
187 
188  return rxPacket();
189 }
190 
191 bool GroupBulkRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length)
192 {
193  uint16_t start_addr;
194 
195  if (last_result_ == false || data_list_.find(id) == data_list_.end())
196  return false;
197 
198  start_addr = address_list_[id];
199 
200  if (address < start_addr || start_addr + length_list_[id] - data_length < address)
201  return false;
202 
203  return true;
204 }
205 
206 uint32_t GroupBulkRead::getData(uint8_t id, uint16_t address, uint16_t data_length)
207 {
208  if (isAvailable(id, address, data_length) == false)
209  return 0;
210 
211  uint16_t start_addr = address_list_[id];
212 
213  switch(data_length)
214  {
215  case 1:
216  return data_list_[id][address - start_addr];
217 
218  case 2:
219  return DXL_MAKEWORD(data_list_[id][address - start_addr], data_list_[id][address - start_addr + 1]);
220 
221  case 4:
222  return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_addr + 0], data_list_[id][address - start_addr + 1]),
223  DXL_MAKEWORD(data_list_[id][address - start_addr + 2], data_list_[id][address - start_addr + 3]));
224 
225  default:
226  return 0;
227  }
228 }
229 
230 bool GroupBulkRead::getError(uint8_t id, uint8_t* error)
231 {
232  // TODO : check protocol version, last_result_, data_list
233  // if (last_result_ == false || error_list_.find(id) == error_list_.end())
234 
235  return error[0] = error_list_[id][0];
236 }
#define COMM_RX_FAIL
#define DXL_MAKEDWORD(a, b)
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 GroupBulkRead::r...
std::map< uint8_t, uint16_t > length_list_
int txPacket()
The function that transmits the Bulk Read instruction packet which might be constructed by GroupBulkR...
#define COMM_SUCCESS
bool addParam(uint8_t id, uint16_t start_address, uint16_t data_length)
The function that adds id, start_address, data_length to the Bulk Read list.
#define DXL_MAKEWORD(a, b)
int rxPacket()
The function that receives the packet which might be come from the Dynamixel.
void clearParam()
The function that clears the Bulk Read list.
void removeParam(uint8_t id)
The function that removes id from the Bulk Read list.
#define COMM_TX_FAIL
virtual int bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length)=0
The function that transmits INST_BULK_READ instruction packet The function makes an instruction pack...
The class that inherits Protocol1PacketHandler class or Protocol2PacketHandler class.
#define DXL_LOBYTE(w)
std::map< uint8_t, uint16_t > address_list_
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 DXL_HIBYTE(w)
uint32_t getData(uint8_t id, uint16_t address, uint16_t data_length)
The function that gets the data which might be received by GroupBulkRead::rxPacket or GroupBulkRead::...
std::vector< uint8_t > id_list_
std::map< uint8_t, uint8_t * > error_list_
#define COMM_NOT_AVAILABLE
GroupBulkRead(PortHandler *port, PacketHandler *ph)
The function that Initializes instance for Bulk Read.
std::map< uint8_t, uint8_t * > data_list_
int txRxPacket()
The function that transmits and receives the packet which might be come from the Dynamixel.
bool getError(uint8_t id, uint8_t *error)
The function that gets the error which might be received by GroupBulkRead::rxPacket or GroupBulkRead:...


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