def start_com(dobotId):
print("Start dobot motion:dobotId =", dobotId)
dType.SetHOMEParams(api, dobotId, 250, 0, 50, 0, isQueued=0) # 홈 파라미터 설정
dType.SetPTPJointParams(api,
dobotId,
200,
200,
200,
200,
200,
200,
200,
200,
isQueued=1) # joint별 속도, 가속도 설정
dType.SetPTPCommonParams(api, dobotId, 100, 100,
isQueued=1) # 속도비 및 가속도비 설정
dType.SetInfraredSensor(api, dobotId, 1, 2)
while (True):
print(dType.GetInfraredSensor(api, dobotId, 2))
# 물체가 감지되었을때 1, 아니면 0
if dType.GetInfraredSensor(api, dobotId, 2)[0] == 1:
dType.SetEndEffectorSuctionCup(api, dobotId, 1, 1, isQueued=0)
dType.SetEndEffectorSuctionCup(api, dobotId, 1, 0, isQueued=0)
dType.SetQueuedCmdClear(api, dobotId)
dType.SetQueuedCmdStopExec(api, dobotId)
def initDobot(api):
# Clean Command Queued
dType.SetQueuedCmdClear(api)
# デバイスのシリアルナンバーを取得する
dSN = dType.GetDeviceSN(api)
print(dSN)
# デバイス名を取得する
dName = dType.GetDeviceName(api)
print(dName)
# デバイスのバージョンを取得する
majorV, minorV, revision = dType.GetDeviceVersion(api)
print(majorV, minorV, revision)
# JOGパラメータの設定
dType.SetJOGJointParams(api, 200, 200, 200, 200,
200, 200, 200, 200, isQueued=1) # 関節座標系での各モータの速度および加速度の設定
dType.SetJOGCoordinateParams(api, 200, 200, 200, 200,
200, 200, 200, 200, isQueued=1) # デカルト座標系での各方向への速度および加速度の設定
# JOG動作の速度、加速度の比率を設定
dType.SetJOGCommonParams(api, 100, 100, isQueued=1)
# PTPパラメータの設定
dType.SetPTPJointParams(api, 200, 200, 200, 200,
200, 200, 200, 200, isQueued=1) # 関節座標系の各モータの速度および加速度を設定
# デカルト座標系での各方向への速度および加速度の設定
dType.SetPTPCoordinateParams(api, 200, 200, 200, 200, isQueued=1)
# PTP動作の速度、加速度の比率を設定
dType.SetPTPCommonParams(api, 100, 100, isQueued=1)
def draw01():
dType.SetPTPCmd(api,
dType.PTPMode.PTPJUMPXYZMode,
xArr[1] - xOS,
y2DArr[1][0] - yOS[1],
zCoor,
0,
isQueued=1)
dType.SetPTPCmd(api,
dType.PTPMode.PTPMOVLXYZMode,
xArr[2] + xOS,
y2DArr[2][1] + yOS[1],
zCoor,
0,
isQueued=1)
dType.SetPTPCmd(api,
dType.PTPMode.PTPJUMPXYZMode,
xArr[1] - xOS,
y2DArr[1][1] + yOS[1],
zCoor,
0,
isQueued=1)
dType.SetPTPCmd(api,
dType.PTPMode.PTPMOVLXYZMode,
xArr[2] + xOS,
y2DArr[2][0] - yOS[1],
zCoor,
0,
isQueued=1)
goHome()
def draw20():
dType.SetPTPCmd(api,
dType.PTPMode.PTPJUMPXYZMode,
xArr[2] - xOS,
y2DArr[2][2] - yOS[2],
zCoor,
0,
isQueued=1)
dType.SetPTPCmd(api,
dType.PTPMode.PTPMOVLXYZMode,
xArr[3] + xOS,
y2DArr[3][3] + yOS[2],
zCoor,
0,
isQueued=1)
dType.SetPTPCmd(api,
dType.PTPMode.PTPJUMPXYZMode,
xArr[2] - xOS,
y2DArr[2][3] + yOS[2],
zCoor,
0,
isQueued=1)
dType.SetPTPCmd(api,
dType.PTPMode.PTPMOVLXYZMode,
xArr[3] + xOS,
y2DArr[3][2] - yOS[2],
zCoor,
0,
isQueued=1)
goHome()
def getAll_In_draw():
print("in")
#GetIODO(api, dobotId, addr)
for i in range(0, 10):
print(dType.GetIODO(api, id_m1, i))
for i in range(0, 5):
print(dType.GetIOADC(api, id_m1, i))
def connectDobots():
global id_m1
global id_magL
global id_magR
print("connecting all Dobots")
# print("Start search dobot, max:", maxDobotConnectCount)
# for i in range(0, maxDobotConnectCount):
# result = dType.ConnectDobot(api, "",115200)
id_m1 = connectCOM("COM3")
window.checkBox_M1.setChecked(id_m1 != -1)
window.label_M1_id.setText(str(id_m1)) #f'{10}'
if id_m1 > -1:
dType.SetQueuedCmdStartExec(api, id_m1)
id_magL = connectCOM("COM4")
window.checkBox_MagL.setChecked(id_magL != -1)
window.label_MagL_id.setText(str(id_magL))
if id_magL > -1:
dType.SetQueuedCmdStartExec(api, id_magL)
id_magR = connectCOM("COM5")
window.checkBox_MagR.setChecked(id_magR != -1)
window.label_MagR_id.setText(str(id_magR))
if id_magR > -1:
dType.SetQueuedCmdStartExec(api, id_magR)
def start(self):
dType.SetQueuedCmdClear(self.api)
#Async Motion Params Setting
#dType.SetHOMEParams(self.api, 200, 0, 0, 0, isQueued = 1)
#dType.SetPTPCoordinateParamsEx(self.api,30,50,30,50,1)
dType.SetPTPJointParams(self.api,
200,
200,
200,
200,
200,
200,
200,
200,
isQueued=1)
dType.SetPTPCommonParams(self.api, 100, 100, isQueued=1)
dType.SetJOGJointParams(self.api,
150,
150,
150,
150,
150,
150,
150,
150,
isQueued=1)
def __init__(self, id_, dobotSt):
self.dobotSt = dobotSt
dType.SetPTPLParamsEx(api, id_, 50, 40, 1) ###!! reail speed, accel
print('rail id_:', id_, id_nms_default[id_])
if (not bDebug and self.dobotSt.bOn):
print(' rail L:', dType.GetPoseL(api, id_))
self.id_ = id_
def start_com(dobotId):
pos = [250, 0, 30, 0]
flag = 1
flagb = 1
flagc = 1
print("Start dobot motion:dobotId =", dobotId)
dType.SetQueuedCmdStartExec(api, dobotId)
while (True):
if flag > 0:
pos[1] = 20
else:
pos[1] = -20
if flagb > 0:
pos[2] = 20
else:
pos[2] = -20
dType.SetPTPCmd(api, dobotId, 1, pos[0], pos[1], pos[2], pos[3])
flag = flag * -1
flagc = flagc + 1
if flagc % 3 == 0:
flagb = flagb * -1
dType.dSleep(100)
print("11111111111:", dobotId)
def __init__(self, api, screen_width, screen_height, fenetre):
self.fenetre = fenetre
"""to initialize our screen and its properties"""
self.fenetre.setInstruction(
"placez le robot dans le coin haut-gauche de l'écran (puis ENTREZ) :\n"
)
fenetre.desactive()
while fenetre.entrer == 0:
{}
self.topleft = dType.GetPose(api)[0:2]
self.fenetre.setInstruction(
"placez le robot dans le coin haut-droit de l'écran (puis ENTREZ) :\n"
)
fenetre.desactive()
while fenetre.entrer == 0:
{}
self.topright = dType.GetPose(api)[0:2]
self.fenetre.setInstruction(
"placez le robot dans le coin bas-gauche de l'écran (puis ENTREZ) :\n"
)
fenetre.desactive()
while fenetre.entrer == 0:
{}
self.bottomleft = dType.GetPose(api)[0:2]
self.pixelheight = screen_height
self.pixelwidth = screen_width
# on calcul la taille de l'ecran dans la base du robot
self.Hori_distance = math.sqrt((self.topleft[0] -
self.topright[0])**2 +
(self.topleft[1] - self.topright[1])**2)
self.Vert_distance = math.sqrt(
(self.topleft[0] - self.bottomleft[0])**2 +
(self.topleft[1] - self.bottomleft[1])**2)
def dohome1():
CON_STR = {
dType.DobotConnect.DobotConnect_NoError: "DobotConnect_NoError",
dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound",
dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied"}
#Load Dll
api = dType.load()
#Connect Dobot
state = dType.ConnectDobot(api, "", 115200)[0]
print("Connect status:",CON_STR[state])
if (state == dType.DobotConnect.DobotConnect_NoError):
#Clean Command Queued
dType.SetQueuedCmdClear(api)
#Async Home
#lastIndex = dType.SetHOMECmd(api, temp = 0, isQueued = 1)[0]
lastIndex = dType.SetPTPCmd(api, dType.PTPMode.PTPMOVLXYZMode, 250, 0, 50, 0, isQueued = 1)[0]
#Start to Execute Command Queued
dType.SetQueuedCmdStartExec(api)
#Wait for Executing Last Command
while lastIndex > dType.GetQueuedCmdCurrentIndex(api)[0]:
dType.dSleep(100)
#Stop to Execute Command Queued
dType.SetQueuedCmdStopExec(api)
#Disconnect Dobot
dType.DisconnectDobot(api)
def OneAction(api,
x=None,
y=None,
z=None,
r=None,
mode=dType.PTPMode.PTPMOVLXYZMode):
"""One step operation"""
print('OneAction')
if mode is 1 or mode is 2:
if (x is None or y is None or z is None or r is None):
pose = dType.GetPose(api)
if x is None:
x = pose[0]
if y is None:
y = pose[1]
if z is None:
z = pose[2]
if r is None:
r = pose[3]
lastIndex = dType.SetPTPCmd(api, mode, x, y, z, r, isQueued=1)[0]
elif mode is 4 or mode is 5:
if (x is None or y is None or z is None or r is None):
pose = dType.GetPose(api)
if x is None:
x = pose[4]
if y is None:
y = pose[5]
if z is None:
z = pose[6]
if r is None:
r = pose[7]
lastIndex = dType.SetPTPCmd(api, mode, x, y, z, r, isQueued=1)[0]
_Act(api, lastIndex)
def Operation(self, file_name, axis, volume=1, initPOS=None):
"""
A function that sends a motion command in any direction
Parameters
----------
api : CDLL
axis : str
移動方向
volume : int
移動量
"""
axis_list = ['x', 'y', 'z', 'r']
if (initPOS != None):
pose = initPOS
else:
pose = dType.GetPose(self.api)
if (axis in axis_list):
if (axis == 'x'):
_OneAction(self.api, dType.PTPMode.PTPMOVLXYZMode,
pose[0] + volume, pose[1], pose[2], pose[3])
elif (axis == 'y'):
_OneAction(self.api, dType.PTPMode.PTPMOVLXYZMode, pose[0],
pose[1] + volume, pose[2], pose[3])
elif (axis == 'z'):
_OneAction(self.api, dType.PTPMode.PTPMOVLXYZMode, pose[0],
pose[1], pose[2] + volume, pose[3])
else:
print('rは実装されていません。')
else:
print('移動軸に問題があります！')
# 座標をファイルへ書き込む
csv_write(file_name, dType.GetPose(self.api))
def switch12V(self, port, state):
#port(16,17) -> int #state-> str
if state == "on":
self.lastIndex = dType.SetIODO(self.api, port, 1, 1)[0]
#print(self.lastIndex)
else:
self.lastIndex = dType.SetIODO(self.api, port, 0, 1)[0]
def home_dobot():
#Clean Command Queued
dType.SetQueuedCmdClear(api)
#Async Motion Params Setting
dType.SetHOMEParams(api, 250, 0, 50, 0, isQueued=1)
dType.SetPTPJointParams(api,
200,
200,
200,
200,
200,
200,
200,
200,
isQueued=1)
dType.SetPTPCommonParams(api, 100, 100, isQueued=1)
#Async Home
lastIndex = dType.SetHOMECmd(api, temp=0, isQueued=1)[0]
lastIndex = lastIndex
#Start to Execute Command Queued
dType.SetQueuedCmdStartExec(api)
#Wait for Executing Last Command
while lastIndex > dType.GetQueuedCmdCurrentIndex(api)[0]:
dType.dSleep(100)
#Stop to Execute Command Queued
dType.SetQueuedCmdStopExec(api)
return 'Homing'
def __init__(self, nm, id_, c, bOn, bRail=False):
self.color = c
self.nm = nm
self.id_ = id_
self.bOn = bOn
if (bDebug or not bOn):
print("debug", nm, id_)
self.pos = [id_ * 30, 0, 0, 0]
self.posCursor = [0, id_ * 30, 0, 0]
else:
self.pos = dType.GetPose(api, id_)
self.posCursor = self.pos # setPosCursorXYZR(self.pos)
#print(dType.GetDeviceWithL(api, id_)) #?!TODO F^ always True at start
#bRail=dType.GetDeviceWithL(api, id_)[0]
self.bRail = bRail
if (bRail):
print("dobot id: ", id_, 'checking for rail...')
bRail_now = dType.GetDeviceWithL(api, id_)[0]
if (bRail_now):
print(" already connected Rail")
else:
dType.SetDeviceWithL(api, id_, True)
time.sleep(0.1) #!del
print("connecting rail...", id_nms_default[id_],
dType.GetDeviceWithL(api, id_))
global dobotRailState
dobotRailState = DobotRailState(id_, self)
self.dobotRailState = dobotRailState
def btn_alarm_check_clear_f(id_, btn):
print(dType.GetAlarmsState(api, id_))
#TODO 1st: check AlarmsState, mark color btn, 2nd press if marked red - clear
#btn.setStyleSheet("background-color: #00ff00")
dType.ClearAllAlarmsState(api, id_)
print("after clear", dType.GetAlarmsState(api, id_))
def connectDobot(id_, id_nm, c, checkbox, label, bRail=False):
checkbox.setChecked(id_ != -1) # TODO mark red if fail connect COM
label.setText(str(id_)) # f'{10}'
onNow = False
if id_ > -1:
onNow = True
dType.SetQueuedCmdClear(api, id_)
dType.SetQueuedCmdStartExec(api, id_)
dType.ClearAllAlarmsState(api, id_)
dType.SetPTPCommonParams(
api, id_, 40, 30, 1) #??Ex #speed, acceleration max is 100, 1000
if (id_ == -1):
id_ = id_default(
id_nm) #initial id, for debug or create functions w.o robots
dobotStates[id_] = DobotState(id_nm, id_, c, onNow, bRail)
'''
dType.SetQueuedCmdClear(api) #Clean Command Queued
#Async Motion Params Setting
dType.SetHOMEParams(api, 200, 200, 200, 200, isQueued = 1)
dType.SetPTPJointParams(api, 200, 200, 200, 200, 200, 200, 200, 200, isQueued = 1)
dType.SetPTPCommonParams(api, 100, 100, isQueued = 1)
dType.SetHOMECmd(api, temp = 0, isQueued = 1)
dType.DisconnectDobot(api)
'''
#print(dType.GetDeviceName(api, id_))
return dobotStates[id_], id_
def ACT_JOGMode_click(J1_Angle, J2_Angle, J3_Angle, J4_Angle):
#Operation(api, 'z', J1_Angle, mode=dType.PTPMode.PTPMOVLANGLEMode)
if J1_Angle is '':
J1_Angle = dType.GetPose(api)[4]
if J2_Angle is '':
J2_Angle = dType.GetPose(api)[5]
if J3_Angle is '':
J3_Angle = dType.GetPose(api)[6]
if J4_Angle is '':
J4_Angle = dType.GetPose(api)[7]
J1_Angle = float(J1_Angle)
J2_Angle = float(J2_Angle)
J3_Angle = float(J3_Angle)
J4_Angle = float(J4_Angle)
OneAction(api,
J1_Angle,
J2_Angle,
J3_Angle,
J4_Angle,
mode=dType.PTPMode.PTPMOVLANGLEMode)
print(J1_Angle)
print(J2_Angle)
print(J3_Angle)
print(J4_Angle)
def raise_up(self, height=20):
"""
rasie up the machine
:return:
"""
current_pose = dType.GetPose(self.api)
dType.SetPTPCmd(self.api, dType.PTPMode.PTPMOVJXYZMode,
current_pose[0], current_pose[1], current_pose[2] + height, rHead=0, isQueued=1)
def dobot_init():
#Load Dll
api = dType.load()
#Connect Dobot
state = dType.ConnectDobot(api, "", 115200)[0]
print("Connect status:",CON_STR[state])
if (state == dType.DobotConnect.DobotConnect_NoError):
return api
def Operation(api,
axis,
volume=1,
file_name=None,
initPOS=None,
mode=dType.PTPMode.PTPMOVLXYZMode):
"""
A function that sends a motion command in any direction
Parameters
----------
api : CDLL
axis : str
移動方向
volume : int
移動量
initPos : dict
動作前の位置
mode : int
int | mode
0 | PTPJUMPXYZMode,
1 | PTPMOVJXYZMode,
2 | PTPMOVLXYZMode,
3 | PTPJUMPANGLEMode,
4 | PTPMOVJANGLEMode,
5 | PTPMOVLANGLEMode,
6 | PTPMOVJANGLEINCMode,
7 | PTPMOVLXYZINCMode,
8 | PTPMOVJXYZINCMode,
9 | PTPJUMPMOVLXYZMode,
"""
axis_list = ['x', 'y', 'z', 'r']
axis_list = ['x', 'y', 'z', 'r']
if (initPOS != None):
pose = initPOS
else:
pose = dType.GetPose(api)
if mode is 1 or mode is 2:
if (axis == axis_list[0]):
OneAction(api, pose[0] + volume, pose[1], pose[2], pose[3])
elif (axis == axis_list[1]):
OneAction(api, pose[0], pose[1] + volume, pose[2], pose[3])
elif (axis == axis_list[2]):
OneAction(api, pose[0], pose[1], pose[2] + volume, pose[3])
else:
print('rは実装されていません。')
elif mode is 4 or mode is 5:
OneAction(api, pose[4], pose[5], pose[6], pose[7], mode)
else:
print('選択したmodeに問題があります！')
if file_name is None:
return
else:
# 座標をファイルへ書き込む
csv_write(file_name, dType.GetPose(api))
def mov_to_cursor(
self, l
): #non-blocking so can use from UI without new thread #TODO mov_to_cursor all together xyzrL
id_ = self.id_
current_pose = dType.GetPose(api, id_)
#dType.SetPTPLParamsEx(api, id_,50,30,1)
dType.SetPTPWithLCmdEx(api, id_, 1, current_pose[0], current_pose[1],
current_pose[2], current_pose[3],
self.dobotSt.posCursorL, 1)
def toggleSuction(self):
lastIndex = 0
if(self.suction):
lastIndex = dType.SetEndEffectorSuctionCup( self.api, True, False, isQueued = 0)[0]
self.suction = False
else:
lastIndex = dType.SetEndEffectorSuctionCup(self.api, True, True, isQueued = 0)[0]
self.suction = True
self.commandDelay(lastIndex)
def btnStop_f(id_):
tskMarks_clear_all(True)
queue_clear(thread_m1_queue)
queue_clear(thread_magL_queue)
queue_clear(thread_magR_queue)
dType.SetQueuedCmdForceStopExec(api, id_)
dType.ClearAllAlarmsState(api, id_)
def mov_relative(
self,
l): #TODO now PosCursorL can't be negative in incrPosCursorLRail
#TODO
id_ = self.id_
current_pose = dType.GetPose(api, id_)
dType.SetPTPWithLCmdEx(api, id_, 1, current_pose[0], current_pose[1],
current_pose[2], current_pose[3],
self.getL(False) + l, 1)
def dobot_zero(api):
# Clean Command Queued
dType.SetQueuedCmdClear(api)
print ("relocate zero point......")
#Start to Execute Command Queued
dType.SetQueuedCmdStartExec(api)
dType.SetHOMEParams(api, home_x, home_y, home_z, home_r, isQueued = 1)
dType.SetPTPCommonParamsEx(api, 100, 100, isQueued = 1)
dType.SetHOMECmdEx(api, temp = 0, isQueued = 1)
def pickToggle(self, itemHeight):
lastIndex = 0
positions = dType.GetPose(self.api)
if(self.picking):
lastIndex = dType.SetPTPCmd(self.api, dType.PTPMode.PTPMOVLXYZMode, positions[0], positions[1], self.homeZ, 0)[0]
self.picking = False
else:
lastIndex = dType.SetPTPCmd(self.api, dType.PTPMode.PTPMOVLXYZMode, positions[0], positions[1], itemHeight, 0)[0]
self.picking = True
self.commandDelay(lastIndex)
def disconnectDobots():
global id_m1
global id_magL
global id_magR
print("DISconnecting all Dobots")
dType.DisconnectDobot(api, id_m1) # dType.DisconnectDobot(self.DobotAPI)
dType.DisconnectDobot(api, id_magL)
dType.DisconnectDobot(api, id_magR)
print("DISconnecting probably not working") #? bug. ports left occupied
def moveAway(self):
dType.SetWAITCmd(self.api, 0.1, 1)
dType.SetPTPCmd(self.api,
dType.PTPMode.PTPMOVLXYZMode,
50,
-200,
50,
0,
isQueued=1)
