def thread_proc(stop_event):
# def thread_proc(stop_event, m_bcapclient):
m_bcapclient = bcapclient.BCAPClient(HOST, PORT, TIMEOUT)
print("Main:Open Connection")
# start b_cap Service
m_bcapclient.service_start("")
print("Main:Send SERVICE_START packet")
Name = "sub_process"
Provider = "CaoProv.DENSO.VRC"
Machine = "localhost"
Option = ""
try:
# Connect to RC8 (RC8(VRC)provider)
hCtrl = m_bcapclient.controller_connect(Name, Provider, Machine,
Option)
print("Sub :Connect " + Provider)
hRobot = m_bcapclient.controller_getrobot(hCtrl, "Arm", "")
while not stop_event.wait(0.01):
ret = m_bcapclient.robot_execute(hRobot, "CurPos")
print('Sub :' + str(ret))
except Exception as e:
print(e)
finally:
if (hRobot != 0):
m_bcapclient.robot_release(hRobot)
if (hCtrl != 0):
m_bcapclient.controller_disconnect(hCtrl)
print("Sub :Release Controller")
def connect(self, host='192.168.0.1', port=5007):
res = ''
try:
if self.conect_flg == 0:
# Connection processing of tcp communication
print("Open Connection")
self.mbcap = bcapclient.BCAPClient(host, port, self.timeout)
# start b_cap Service
print("Send SERVICE_START packet")
self.mbcap.service_start("")
# Connect to RC8 (RC8(VRC)provider)
self.h_ctrl = self.mbcap.controller_connect(
self.Name, self.Provider, self.Machine, self.Option)
print("Connect RC8")
# get Robot Object Handl
self.h_robot = self.mbcap.controller_getrobot(
self.h_ctrl, "arm", self.ifnotm)
self.h_var = self.mbcap.robot_getvariable(
self.h_robot, "@Current_Position", self.ifnotm)
res = 'connected'
self.conect_flg = 1
except Exception as e:
print('=== ERROR Description ===')
print(str(e))
res = 'Error : ' + str(e)
finally:
return res
def connect(host, port, timeout, provider="CaoProv.DENSO.VRC"):
client = bcapclient.BCAPClient(host, port, timeout)
client.service_start("")
Name = ""
Provider = provider
Machine = ("localhost")
Option = ("")
hCtrl = client.controller_connect(Name, Provider, Machine, Option)
hRobot = client.controller_getrobot(hCtrl, "Arm0", "")
client.robot_execute(hRobot, "TakeArm", [0, 0])
client.robot_execute(hRobot, "Motor", [1, 0])
return (client, hCtrl, hRobot)
def __init__(self,host,port):
### Connection processing of tcp communication
print("Open Connection")
self.m_bcap = bcapclient.BCAPClient(host,port,self.timeout)
### start b_cap Service
print("Send SERVICE_START packet")
self.m_bcap.service_start("")
### Connect to RC8 (RC8(VRC)provider)
self.H_ctrl = self.m_bcap.controller_connect(self.Name,self.Provider,self.Machine,self.Option)
print("Connect RC8")
### get Robot Object Handl
self.H_robot = self.m_bcap.controller_getrobot(self.H_ctrl,"","")
def __init__(self, host='192.168.0.1', port=5007):
# Connection processing of tcp communication
print("Open Connection")
self.m_bcap = bcapclient.BCAPClient(host, port, self.timeout)
# start b_cap Service
print("Send SERVICE_START packet")
self.m_bcap.service_start("")
# Connect to RC8 (RC8(VRC)provider)
self.H_ctrl = self.m_bcap.controller_connect(self.Name, self.Provider,
self.Machine, self.Option)
print("Connect RC8")
# get Robot Object Handl
self.H_robot = self.m_bcap.controller_getrobot(self.H_ctrl, "", "")
self.H_Var = self.m_bcap.robot_getvariable(self.H_robot,
"@Current_Position")
def connect(self):
self.port = 5007
self.timeout = 2000
try:
bcapclient = bcap.BCAPClient(self.strIP, self.port, self.timeout)
self.mbcapclient = bcapclient
print("TCP Connect")
self.mbcapclient.service_start("")
# set Parameter
Name = ""
Provider = "CaoProv.DENSO.VRC"
Machine = ("localhost")
Option = ("")
self.hctrl = self.mbcapclient.controller_connect(Name, Provider, Machine, Option)
self.hrobot = self.mbcapclient.controller_getrobot(self.hctrl, "")
print("Connect RC8")
return True
except Exception as e:
print("Error Connect RC8")
print(e)
return False
return (bool(ctypes.windll.user32.GetAsyncKeyState(key) & 0x8000))
# End def
ESC = 0x1B # Virtual key code of [ESC] key
key_1 = 0X31 # Virtual key code of [1] key
key_2 = 0X32 # Virtual key code of [2] key
### set IP Address , Port number and Timeout of connected RC8
host = "192.168.0.1"
port = 5007
timeout = 2000
### Connection processing of tcp communication
m_bcapclient = bcapclient.BCAPClient(host, port, timeout)
print("Open Connection")
### start b_cap Service
m_bcapclient.service_start("")
print("Send SERVICE_START packet")
### set Parameter
Name = ""
Provider = "CaoProv.DENSO.VRC"
Machine = "localhost"
Option = ""
try:
### Connect to RC8 (RC8(VRC)provider) , Get Controller Handle
hCtrl = m_bcapclient.controller_connect(Name, Provider, Machine, Option)
def main():
flg = True
ESC = 0x1B # [ESC] virtual key code
### set IP Address , Port number and Timeout of connected RC8
host = "192.168.0.2"
port = 5007
timeout = 2000
### Handls
hctrl = 0
hrob = 0
sumstate = np.zeros(7)
target_pos = np.zeros(7)
org_pos = np.zeros(7)
success = spacenavigator.open()
if success:
bcap = bcapclient.BCAPClient(host, port, timeout)
bcap.service_start("")
print("Start b-cap service")
Name = ""
Provider = "CaoProv.DENSO.VRC"
Machine = ("localhost")
Option = ("")
hctrl = bcap.controller_connect(Name, Provider, Machine, Option)
print("Connect Ctrl")
#Connect To arm
hrob = bcap.controller_getrobot(hctrl, "Arm", "")
print("Connect Robot")
#Take Arm
Command = "TakeArm"
Param = [0, 0]
bcap.robot_execute(hrob, Command, Param)
print("Take Arm")
#Motor On
Command = "Motor"
Param = [1, 0]
bcap.robot_execute(hrob, Command, Param)
print("Motor On")
#Get CurPos
Command = "CurPos"
Param = ""
tmp_cur_pos = bcap.robot_execute(hrob, Command, Param)
org_pos = np.array(tmp_cur_pos[0:7])
print("Origin Pos")
print(org_pos)
org_list = org_pos.tolist()
origin_pos = org_pos
print(origin_pos)
while flg:
state = spacenavigator.read()
arr_state = np.array([
state.x, state.y, state.z, state.pitch, -1 * state.roll,
-1 * state.yaw, 0
])
#print(type(state.buttons))
#print(state.buttons)
sumstate = sumstate + arr_state
target_pos = sumstate + origin_pos
# FIX Rx,Ry,Fig
target_pos[3:5] = origin_pos[3:5] #Rx,Ry
target_pos[6] = origin_pos[6] #Fig
tlist = target_pos.tolist()
print("tlist")
print(tlist)
POSEDATA = [tlist, "P", "@P"]
bcap.robot_move(hrob, 1, POSEDATA, "")
if getkey(ESC): # If push the ESC key,program finish
flg = False
#End if
#End while
if hrob != 0:
bcap.robot_release(hrob)
hrob = 0
print("Release Robot")
#End if
if hctrl != 0:
bcap.controller_disconnect(hctrl)
hctrl = 0
print("Dissconnect Controller")
bcap.service_stop()
print("b-cap service Stop")
#End if
else:
print("Failed spacenavigator.Open() ")
def main_proc():
stop_event = threading.Event()
# set Parameter
# If you want to connect to RC9, please select "VRC9" as the provider name.
# If you want to connect to RC8, RC8A, or COBOTTA, select "VRC" as the provider name.
Name = "mainprocess"
Provider = "CaoProv.DENSO.VRC"
Machine = "localhost"
Option = ""
# Connection processing of tcp communication
m_bcapclient = bcapclient.BCAPClient(HOST, PORT, TIMEOUT)
print("Main:Open Connection")
# start b_cap Service
m_bcapclient.service_start("")
print("Main:Send SERVICE_START packet")
try:
# Connect to RC8 (RC8(VRC)provider)
hCtrl = m_bcapclient.controller_connect(Name, Provider, Machine,
Option)
thread = threading.Thread(
# target=thread_proc, args=(stop_event, m_bcapclient,))
target=thread_proc,
args=(stop_event, ))
thread.start()
# get Robot Object Handl
HRobot = m_bcapclient.controller_getrobot(hCtrl, "Arm", "")
print("Main:AddRobot")
# TakeArm
Command = "TakeArm"
Param = [0, 0]
m_bcapclient.robot_execute(HRobot, Command, Param)
print("Main:TakeArm")
# Interpolation
Comp = 1
# PoseData (string)
Pose = "@P P1"
m_bcapclient.robot_move(HRobot, Comp, Pose, "")
print("Main:Complete Move P,@P P[1]")
Pose = "@P P2"
m_bcapclient.robot_move(HRobot, Comp, Pose, "")
print("Main:Complete Move P,@P P[2]")
Pose = "@P P3"
m_bcapclient.robot_move(HRobot, Comp, Pose, "")
print("Main:Complete Move P,@P P[3]")
# Give Arm
Command = "GiveArm"
Param = None
m_bcapclient.robot_execute(HRobot, Command, Param)
print("Main:GiveArm")
except Exception as e:
print(e)
finally:
if not (thread is None):
stop_event.set()
thread.join()
# Disconnect
if (HRobot != 0):
m_bcapclient.robot_release(HRobot)
print("Main:Release Robot Object")
# End If
if (hCtrl != 0):
m_bcapclient.controller_disconnect(hCtrl)
print("Main:Release Controller")
# End If
m_bcapclient.service_stop()
print("Main:B-CAP service Stop")
def printlist():
print("[ESC] : Close Application")
print("[1] : Start Synchro Mode")
print("[2] : End Synchro Mode")
# End def
esc = 0x1B # Virtual key code of [ESC] key
key_1 = 0X31 # Virtual key code of [1] key
key_2 = 0X32 # Virtual key code of [2] key
# Connection processing of tcp communication
m_bcapclient = bcap.BCAPClient(master_ip, port, timeout)
s_bcapclient = bcap.BCAPClient(slave_ip, port, timeout)
print("Open Connection")
# start b_cap Service
m_bcapclient.service_start("")
s_bcapclient.service_start("")
print("Send SERVICE_START packet")
# set Parameter
Name = ""
Provider = "CaoProv.DENSO.VRC"
Machine = ("localhost")
Option = ("")
# Connect To RC8
def vacuum_load_check(sngPower, interval, loadtime):
filename = str(sngPower) + "%_interval" + str(interval) + \
"ms_LoadTime" + str(loadtime) + "ms.csv"
f = open(os.path.join("data", filename), 'w')
writer = csv.writer(f, lineterminator='\n')
header = ["Time(s)", "HandCurPressure[kPa]", "HandCurLoad[%]"]
writer.writerow(header)
# f = open(os.path.join("data", filename), 'w')
# writer = csv.writer(f, lineterminator='\n')
# header = ["Time(ms)", "HandCurPressure[kPa]", "HandCurLoad[%]"]
# Connection processing of tcp communication
m_bcapclient = bcapclient.BCAPClient(host, port, timeout)
print("Open Connection")
# start b_cap Service
m_bcapclient.service_start("")
print("Send SERVICE_START packet")
# set Parameter
Name = ""
Provider = "CaoProv.DENSO.VRC"
Machine = "localhost"
Option = ""
try:
# Connect to RC8 (RC8(VRC)provider) , Get Controller Handle
hCtrl = m_bcapclient.controller_connect(Name, Provider, Machine,
Option)
print("Connect RC8")
# Get Robot Handle
hRobot = m_bcapclient.controller_getrobot(hCtrl, "Arm", "")
print("Get Arm Handle")
param = [sngPower, True]
m_bcapclient.controller_execute(hCtrl, "HandMoveVH", param)
strat_time = time.time()
Interval_Stime = time.time()
flg_load = True
# for i in range(20):
while True:
if interval > 0:
interval_time = time.time() - Interval_Stime
if flg_load is True:
if loadtime < interval_time:
m_bcapclient.controller_execute(hCtrl, "HandStop")
Interval_Stime = time.time()
flg_load = False
else:
pass
# End if
else:
if interval < interval_time:
m_bcapclient.controller_execute(
hCtrl, "HandMoveVH", param)
Interval_Stime = time.time()
flg_load = True
# End if
# End if
# End if
time.sleep(0.3)
retPressure = m_bcapclient.controller_execute(
hCtrl, "HandCurPressure")
retLoad = m_bcapclient.controller_execute(hCtrl, "HandCurLoad")
elapsed_time = time.time() - strat_time
row = [elapsed_time, retPressure, retLoad]
print(row)
# ret.append(row)
writer.writerow(row)
if retLoad > 100:
break
# End if
# if elapsed_time > 600:
# break
# End if
errorcnt = m_bcapclient.controller_execute(hCtrl,
"GetCurErrorCount")
if errorcnt > 0:
break
# End if
# End while
m_bcapclient.controller_execute(hCtrl, "HandStop")
print("HandStop")
strat_time = time.time()
while True:
time.sleep(0.5)
retPressure = m_bcapclient.controller_execute(
hCtrl, "HandCurPressure")
retLoad = m_bcapclient.controller_execute(hCtrl, "HandCurLoad")
elapsed_time = time.time() - strat_time
row = [elapsed_time, retPressure, retLoad]
print(row)
# ret.append(row)
writer.writerow(row)
if elapsed_time > 600:
break
# End if
# End while
except Exception as e:
print('=== ERROR Description ===')
if str(type(
e)) == "<class 'pybcapclient.orinexception.ORiNException'>":
print(e)
errorcode_int = int(str(e))
if errorcode_int < 0:
errorcode_hex = format(errorcode_int & 0xffffffff, 'x')
else:
errorcode_hex = hex(errorcode_int)
print("Error Code : 0x" + str(errorcode_hex))
error_description = m_bcapclient.controller_execute(
hCtrl, "GetErrorDescription", errorcode_int)
print("Error Description : " + error_description)
else:
print(e)
finally:
while True:
time.sleep(0.5)
retPressure = m_bcapclient.controller_execute(
hCtrl, "HandCurPressure")
retLoad = m_bcapclient.controller_execute(hCtrl, "HandCurLoad")
elapsed_time = time.time() - strat_time
row = [elapsed_time, retPressure, retLoad]
print(row)
# ret.append(row)
writer.writerow(row)
if retLoad < 40:
break
# End if
# End while
m_bcapclient.controller_execute(hCtrl, "ClearError")
# DisConnect
if (hRobot != 0):
m_bcapclient.robot_release(hRobot)
print("Release Robot Handle")
# End If
if (hCtrl != 0):
m_bcapclient.controller_disconnect(hCtrl)
print("Release Controller")
# End If
m_bcapclient.service_stop()
print("B-CAP service Stop")
# f = open(os.path.join("data", filename), 'w')
# writer = csv.writer(f, lineterminator='\n')
# header = ["Time(s)", "HandCurPressure[kPa]", "HandCurLoad[%]"]
# writer.writerow(header)
# writer.writerows(ret)
f.close()
# b-cap Lib URL
# https://github.com/DENSORobot/orin_bcap
import pybcapclient.bcapclient as bcapclient
# set IP Address , Port number and Timeout of connected RC8
R1_ip_address = "192.168.0.1"
R1_port = 5007
R1_timeout = 2000
R2_ip_address = "192.168.0.2"
R2_port = 5007
R2_timeout = 2000
# Connection processing of tcp communication
R1_bcapclient = bcapclient.BCAPClient(R1_ip_address, R1_port, R1_timeout)
print("R1:Open Connection")
R2_bcapclient = bcapclient.BCAPClient(R2_ip_address, R2_port, R2_timeout)
print("R2:Open Connection")
# start b_cap Service
R1_bcapclient.service_start("")
print("R1:Send SERVICE_START packet")
R2_bcapclient.service_start("")
print("R2:Send SERVICE_START packet")
# set Parameter
Name = ""
Provider = "CaoProv.DENSO.VRC"
Machine = "localhost"
Option = ""
def getkey(key):
return(bool(ctypes.windll.user32.GetAsyncKeyState(key)&0x8000))
# End def
def printlist():
print("[ESC] : Close Application")
print("[1] : Start Synchro Mode")
print("[2] : End Synchro Mode")
# End def
ESC = 0x1B # Virtual key code of [ESC] key
key_1 = 0X31 # Virtual key code of [1] key
key_2 = 0X32 # Virtual key code of [2] key
### Connection processing of tcp communication
m_bcapclient = bcap.BCAPClient(MasterIpStr,port,timeout)
s_bcapclient = bcap.BCAPClient(SlaveIpStr,port,timeout)
print("Open Connection")
### start b_cap Service
m_bcapclient.service_start("")
s_bcapclient.service_start("")
print("Send SERVICE_START packet")
### set Parameter
Name = ""
Provider="CaoProv.DENSO.VRC"
Machine = ("localhost")
Option = ("")
# Connect To RC8
def main():
flg = True
ESC = 0x1B # [ESC] virtual key code
xp = 0x41 # a : x+
xm = 0x53 # s : x-
yp = 0x44 # d : y+
ym = 0x46 # f : y-
zp = 0x47 # g : z+
zm = 0x48 # h : z-
Rxp = 0x5A # z : Rx+
Rxm = 0x58 # x : Rx-
Ryp = 0x43 # c : Ry+
Rym = 0x56 # v : Ry-
Rzp = 0x42 # b : Rz+
Rzm = 0x4E # n :Rz-
### set IP Address , Port number and Timeout of connected RC8
host = "192.168.0.2"
port = 5007
timeout = 2000
### Handls
hctrl = 0
hrob = 0
sumstate = np.zeros(7)
target_pos = np.zeros(7)
org_pos = np.zeros(7)
bcap = bcapclient.BCAPClient(host, port, timeout)
bcap.service_start("")
print("Start b-cap service")
Name = ""
Provider = "CaoProv.DENSO.VRC"
Machine = ("localhost")
Option = ("")
hctrl = bcap.controller_connect(Name, Provider, Machine, Option)
print("Connect Ctrl")
#Connect To arm
hrob = bcap.controller_getrobot(hctrl, "Arm", "")
print("Connect Robot")
#Take Arm
Command = "TakeArm"
Param = [0, 0]
bcap.robot_execute(hrob, Command, Param)
print("Take Arm")
#Motor On
Command = "Motor"
Param = [1, 0]
bcap.robot_execute(hrob, Command, Param)
print("Motor On")
#Get CurPos
Command = "CurPos"
Param = ""
tmp_cur_pos = bcap.robot_execute(hrob, Command, Param)
print(type(tmp_cur_pos))
print(tmp_cur_pos)
org_pos = np.array(tmp_cur_pos[0:7])
'''print("Origin Pos")
print(org_pos)
org_list = org_pos.tolist()
origin_pos = org_pos
print(origin_pos)
while flg:
#print(type(state.buttons))
#print(state.buttons)
sumstate = sumstate + arr_state
target_pos = sumstate + origin_pos
# FIX Rx,Ry,Fig
target_pos[3:5] = origin_pos[3:5] #Rx,Ry
target_pos[6] = origin_pos[6] #Fig
tlist = target_pos.tolist()
print("tlist")
print(tlist)
POSEDATA = [tlist,"P","@P"]
bcap.robot_move(hrob,1,POSEDATA,"")
if getkey(ESC): # If push the ESC key,program finish
flg=False
#End if
#End while'''
if hrob != 0:
bcap.robot_release(hrob)
hrob = 0
print("Release Robot")
#End if
if hctrl != 0:
bcap.controller_disconnect(hctrl)
hctrl = 0
print("Dissconnect Controller")
bcap.service_stop()
print("b-cap service Stop")
def main():
# set IP Address , Port number and Timeout of connected RC8
host = '192.168.0.1'
port = 5007
timeout = 2000
strBtnName = ['Func', '+', '-']
# Connection processing of tcp communication
m_bcapclient = bcapclient.BCAPClient(host, port, timeout)
print("Open Connection")
# start b_cap Service
m_bcapclient.service_start("")
print("Send SERVICE_START packet")
# set Parameter
Name = ""
Provider = "CaoProv.DENSO.VRC"
Machine = "localhost"
Option = ""
try:
# Connect to RC8 (RC8(VRC)provider) , Get Controller Handle
hCtrl = m_bcapclient.controller_connect(Name, Provider, Machine,
Option)
print("Connect RC8")
# Get Robot Handle
hRobot = m_bcapclient.controller_getrobot(hCtrl, "Arm", "")
hCtrlVer = m_bcapclient.controller_getvariable(hCtrl, '@VERSION')
# Check Robot type And Software version
# The 'GetMechaButtonState', 'ClearMechaButtonState' command can be used with COBOTTA version 1.13 or later.
ret = m_bcapclient.robot_execute(hRobot, 'GetRobotTypeName')
if 'CVR038' in ret:
print('Robot Type Name' + ret)
else:
print('Connect to not COBOTTA')
return
ret = m_bcapclient.variable_getvalue(hCtrlVer)
version_sep = ret.split('.')
if (int(version_sep[0]) >= 2) and (int(version_sep[1]) >= 13):
print('version : ' + ret)
else:
print('Connected to a version before than 2.13')
return
for iBtnType in range(0, 3):
ret = m_bcapclient.robot_execute(hRobot, "GetMechaButtonState",
iBtnType)
print(strBtnName[iBtnType], ret)
# End for
"""
for iBtnType in range(0, 3):
ret = m_bcapclient.robot_execute(
hRobot, 'ClearMechaButtonState', iBtnType)
print(strBtnName[iBtnType] + ' Button Clear State')
"""
except Exception as e:
print('=== ERROR Description ===')
if str(type(
e)) == "<class 'pybcapclient.orinexception.ORiNException'>":
print(e)
errorcode_int = int(str(e))
if errorcode_int < 0:
errorcode_hex = format(errorcode_int & 0xffffffff, 'x')
else:
errorcode_hex = hex(errorcode_int)
print("Error Code : 0x" + str(errorcode_hex))
error_description = m_bcapclient.controller_execute(
hCtrl, "GetErrorDescription", errorcode_int)
print("Error Description : " + error_description)
else:
print(e)
finally:
# DisConnect
if (hRobot != 0):
m_bcapclient.robot_release(hRobot)
print("Release Robot Handle")
# End If
if (hCtrl != 0):
m_bcapclient.controller_disconnect(hCtrl)
print("Release Controller")
# End If
m_bcapclient.service_stop()
print("B-CAP service Stop")
def main_proc():
stop_event = threading.Event()
# set Parameter
# If you want to connect to RC9, please select "VRC9" as the provider name.
# If you want to connect to RC8, RC8A, or COBOTTA, select "VRC" as the provider name.
Name = "mainprocess"
Provider = "CaoProv.DENSO.VRC"
Machine = "localhost"
Option = ""
# Connection processing of tcp communication
m_bcapclient = bcapclient.BCAPClient(HOST, PORT, TIMEOUT)
print("Main:Open Connection")
# start b_cap Service
m_bcapclient.service_start("")
print("Main:Send SERVICE_START packet")
try:
# Connect to RC8 (RC8(VRC)provider)
hCtrl = m_bcapclient.controller_connect(Name, Provider, Machine,
Option)
thread = threading.Thread(target=thread_proc, args=(stop_event, ))
thread.start()
# get Robot Object Handl
HRobot = m_bcapclient.controller_getrobot(hCtrl, "Arm", "")
print("Main:AddRobot")
m_bcapclient.controller_execute(hCtrl, 'ClearError')
# TakeArm
Command = "TakeArm"
Param = [0, 0]
m_bcapclient.robot_execute(HRobot, Command, Param)
print("Main:TakeArm")
Command = "Motor"
Param = [1, 0]
m_bcapclient.robot_execute(HRobot, Command, Param)
print("Motor on")
# Move Initialize Position
Comp = 1
Pos_value = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0]
Pose = [Pos_value, "J", "@E"]
m_bcapclient.robot_move(HRobot, Comp, Pose, "")
print("Complete Move P,@E J(0.0, 0.0, 90.0, 0.0, 90.0, 0.0)")
# Slave move: Change Send format
Command = "slvSendFormat"
# Change the format to position and Hand IO(0x0020), Mini IO(0x0100)
Param = 0x0000
m_bcapclient.robot_execute(HRobot, Command, Param)
print("slvMove Format Change" + Command + ":" + str(Param))
# Slave move: Change return format
Command = "slvRecvFormat"
# Param = 0x0001 # Change the format to position
Param = 0x0014 # hex(10): timestamp, hex(4): [pose, joint]
m_bcapclient.robot_execute(HRobot, Command, Param)
print("slvMove Format Change" + Command + ":" + str(Param))
# Slave move: Change mode
Command = "slvChangeMode"
# Param = 0x001 # Type P, mode 0 (buffer the destination)
# Param = 0x201 # Type P, mode 2 (overwrite the destination)
Param = 0x202 # Type J, mode 2 (overwrite the joint)
m_bcapclient.robot_execute(HRobot, Command, Param)
print("slvMove Format Change" + Command + ":" + str(Param))
# Send POS slvMove
Command = "slvMove"
LoopNum = 300
# oldtime = 0
for num in range(LoopNum):
Pos_value = [0.0 + num * 0.1, 0.0, 90.0, 0.0, 90.0, 0.0, 0,
0] # Joint Type
print(Pos_value)
ret = m_bcapclient.robot_execute(HRobot, Command, Pos_value)
for num in range(LoopNum):
Pos_value = [
0.0 + (LoopNum - num) * 0.1, 0.0, 90.0, 0.0, 90.0, 0.0, 0, 0
] # Joint Type
ret = m_bcapclient.robot_execute(HRobot, Command, Pos_value)
print("time:" + str(ret[0]))
print("pos P,J:" + str(ret[1]))
# Slave move: Change mode
Command = "slvChangeMode"
Param = 0x000 # finish Slave Move
m_bcapclient.robot_execute(HRobot, Command, Param)
print("slvMove Format Change" + Command + ":" + str(Param))
# Give Arm
Command = "GiveArm"
Param = None
m_bcapclient.robot_execute(HRobot, Command, Param)
print("Main:GiveArm")
except Exception as e:
print(e)
finally:
if not (thread is None):
stop_event.set()
thread.join()
# Disconnect
if (HRobot != 0):
m_bcapclient.robot_release(HRobot)
print("Main:Release Robot Object")
# End If
if (hCtrl != 0):
m_bcapclient.controller_disconnect(hCtrl)
print("Main:Release Controller")
# End If
m_bcapclient.service_stop()
print("Main:B-CAP service Stop")
