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
00020
00021
00022 from .robotis_def import *
00023
00024
00025 class GroupSyncRead:
00026 def __init__(self, port, ph, start_address, data_length):
00027 self.port = port
00028 self.ph = ph
00029 self.start_address = start_address
00030 self.data_length = data_length
00031
00032 self.last_result = False
00033 self.is_param_changed = False
00034 self.param = []
00035 self.data_dict = {}
00036
00037 self.clearParam()
00038
00039 def makeParam(self):
00040 if self.ph.getProtocolVersion() == 1.0:
00041 return
00042
00043 if not self.data_dict:
00044 return
00045
00046 self.param = []
00047
00048 for dxl_id in self.data_dict:
00049 self.param.append(dxl_id)
00050
00051 def addParam(self, dxl_id):
00052 if self.ph.getProtocolVersion() == 1.0:
00053 return False
00054
00055 if dxl_id in self.data_dict:
00056 return False
00057
00058 self.data_dict[dxl_id] = []
00059
00060 self.is_param_changed = True
00061 return True
00062
00063 def removeParam(self, dxl_id):
00064 if self.ph.getProtocolVersion() == 1.0:
00065 return
00066
00067 if dxl_id not in self.data_dict:
00068 return
00069
00070 del self.data_dict[dxl_id]
00071
00072 self.is_param_changed = True
00073
00074 def clearParam(self):
00075 if self.ph.getProtocolVersion() == 1.0:
00076 return
00077
00078 self.data_dict.clear()
00079
00080 def txPacket(self):
00081 if self.ph.getProtocolVersion() == 1.0 or len(self.data_dict.keys()) == 0:
00082 return COMM_NOT_AVAILABLE
00083
00084 if self.is_param_changed is True or not self.param:
00085 self.makeParam()
00086
00087 return self.ph.syncReadTx(self.port, self.start_address, self.data_length, self.param,
00088 len(self.data_dict.keys()) * 1)
00089
00090 def rxPacket(self):
00091 self.last_result = False
00092
00093 if self.ph.getProtocolVersion() == 1.0:
00094 return COMM_NOT_AVAILABLE
00095
00096 result = COMM_RX_FAIL
00097
00098 if len(self.data_dict.keys()) == 0:
00099 return COMM_NOT_AVAILABLE
00100
00101 for dxl_id in self.data_dict:
00102 self.data_dict[dxl_id], result, _ = self.ph.readRx(self.port, dxl_id, self.data_length)
00103 if result != COMM_SUCCESS:
00104 return result
00105
00106 if result == COMM_SUCCESS:
00107 self.last_result = True
00108
00109 return result
00110
00111 def txRxPacket(self):
00112 if self.ph.getProtocolVersion() == 1.0:
00113 return COMM_NOT_AVAILABLE
00114
00115 result = self.txPacket()
00116 if result != COMM_SUCCESS:
00117 return result
00118
00119 return self.rxPacket()
00120
00121 def isAvailable(self, dxl_id, address, data_length):
00122 if self.ph.getProtocolVersion() == 1.0 or self.last_result is False or dxl_id not in self.data_dict:
00123 return False
00124
00125 if (address < self.start_address) or (self.start_address + self.data_length - data_length < address):
00126 return False
00127
00128 return True
00129
00130 def getData(self, dxl_id, address, data_length):
00131 if not self.isAvailable(dxl_id, address, data_length):
00132 return 0
00133
00134 if data_length == 1:
00135 return self.data_dict[dxl_id][address - self.start_address]
00136 elif data_length == 2:
00137 return DXL_MAKEWORD(self.data_dict[dxl_id][address - self.start_address],
00138 self.data_dict[dxl_id][address - self.start_address + 1])
00139 elif data_length == 4:
00140 return DXL_MAKEDWORD(DXL_MAKEWORD(self.data_dict[dxl_id][address - self.start_address + 0],
00141 self.data_dict[dxl_id][address - self.start_address + 1]),
00142 DXL_MAKEWORD(self.data_dict[dxl_id][address - self.start_address + 2],
00143 self.data_dict[dxl_id][address - self.start_address + 3]))
00144 else:
00145 return 0