group_sync_write.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 ################################################################################
5 # Copyright 2017 ROBOTIS CO., LTD.
6 #
7 # Licensed under the Apache License, Version 2.0 (the "License");
8 # you may not use this file except in compliance with the License.
9 # You may obtain a copy of the License at
10 #
11 # http://www.apache.org/licenses/LICENSE-2.0
12 #
13 # Unless required by applicable law or agreed to in writing, software
14 # distributed under the License is distributed on an "AS IS" BASIS,
15 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 # See the License for the specific language governing permissions and
17 # limitations under the License.
18 ################################################################################
19 
20 # Author: Ryu Woon Jung (Leon)
21 
22 from .robotis_def import *
23 
24 
26  def __init__(self, port, ph, start_address, data_length):
27  self.port = port
28  self.ph = ph
29  self.start_address = start_address
30  self.data_length = data_length
31 
32  self.is_param_changed = False
33  self.param = []
34  self.data_dict = {}
35 
36  self.clearParam()
37 
38  def makeParam(self):
39  if not self.data_dict:
40  return
41 
42  self.param = []
43 
44  for dxl_id in self.data_dict:
45  if not self.data_dict[dxl_id]:
46  return
47 
48  self.param.append(dxl_id)
49  self.param.extend(self.data_dict[dxl_id])
50 
51  def addParam(self, dxl_id, data):
52  if dxl_id in self.data_dict: # dxl_id already exist
53  return False
54 
55  if len(data) > self.data_length: # input data is longer than set
56  return False
57 
58  self.data_dict[dxl_id] = data
59 
60  self.is_param_changed = True
61  return True
62 
63  def removeParam(self, dxl_id):
64  if dxl_id not in self.data_dict: # NOT exist
65  return
66 
67  del self.data_dict[dxl_id]
68 
69  self.is_param_changed = True
70 
71  def changeParam(self, dxl_id, data):
72  if dxl_id not in self.data_dict: # NOT exist
73  return False
74 
75  if len(data) > self.data_length: # input data is longer than set
76  return False
77 
78  self.data_dict[dxl_id] = data
79 
80  self.is_param_changed = True
81  return True
82 
83  def clearParam(self):
84  self.data_dict.clear()
85 
86  def txPacket(self):
87  if len(self.data_dict.keys()) == 0:
88  return COMM_NOT_AVAILABLE
89 
90  if self.is_param_changed is True or not self.param:
91  self.makeParam()
92 
93  return self.ph.syncWriteTxOnly(self.port, self.start_address, self.data_length, self.param,
94  len(self.data_dict.keys()) * (1 + self.data_length))
def __init__(self, port, ph, start_address, data_length)


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