You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
325 lines
8.0 KiB
325 lines
8.0 KiB
6 years ago
|
import time
|
||
|
|
import threading
|
||
|
|
import DobotDllType as dType
|
||
|
|
|
||
|
|
"""
|
||
|
|
Six actuators:
|
||
|
|
linear rail
|
||
|
|
base rotation
|
||
|
|
rear arm
|
||
|
|
forarm
|
||
|
|
gripperrotation
|
||
|
|
gripper
|
||
|
|
|
||
|
|
First five takes argument 1 or -1 (direction)
|
||
|
|
Example:
|
||
|
|
#initialisation
|
||
|
|
c = Candybot()
|
||
|
|
c.linear(1) # move linear in positive direction
|
||
|
|
c.gripp() # gripp candy
|
||
|
|
c.letgo() # let go of candy
|
||
|
|
c.stopcmp() # stoop compressor
|
||
|
|
|
||
|
|
c.panic() # stops everything
|
||
|
|
"""
|
||
|
|
|
||
|
|
class Candybot:
|
||
|
|
def __init__(self):
|
||
|
|
self.jointspeed = 100
|
||
|
|
self.lastjointspeed = 100
|
||
|
|
|
||
|
|
self.linearspeed = 100
|
||
|
|
self.lastleinearspeed = 100
|
||
|
|
|
||
|
|
CON_STR = {
|
||
|
|
dType.DobotConnect.DobotConnect_NoError: "DobotConnect_NoError",
|
||
|
|
dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound",
|
||
|
|
dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied"}
|
||
|
|
|
||
|
|
#Load Dll
|
||
|
|
self.api = dType.load()
|
||
|
|
|
||
|
|
#Connect Dobot
|
||
|
|
state = dType.ConnectDobot(self.api, "", 115200)[0]
|
||
|
|
print("Connect status:",CON_STR[state])
|
||
|
|
|
||
|
|
if (state == dType.DobotConnect.DobotConnect_NoError):
|
||
|
|
|
||
|
|
#Clean Command Queued
|
||
|
|
dType.SetQueuedCmdClear(self.api)
|
||
|
|
|
||
|
|
#Async Motion Params Setting
|
||
|
|
dType.SetHOMEParams(self.api, 250, 0, 50, 0, isQueued = 1)
|
||
|
|
dType.SetJOGJointParams(self.api, 2, 200, 2, 200, 2, 200, 2, 200, isQueued = 1)
|
||
|
|
dType.SetJOGCommonParams(self.api, 100, 100, isQueued = 1)
|
||
|
|
|
||
|
|
#linear rail
|
||
|
|
dType.SetDeviceWithL(self.api, 1)
|
||
|
|
dType.SetJOGLParams(self.api, 200, 200, isQueued=0)
|
||
|
|
|
||
|
|
#Async Home
|
||
|
|
dType.SetHOMECmd(self.api, temp = 0, isQueued = 1)
|
||
|
|
|
||
|
|
def disconnect(self):
|
||
|
|
#Disconnect Dobot
|
||
|
|
dType.DisconnectDobot(self.api)
|
||
|
|
|
||
|
|
def base(self, direction):
|
||
|
|
if direction == 1:
|
||
|
|
dType.SetJOGCmd(self.api,1,1)
|
||
|
|
|
||
|
|
elif direction == -1:
|
||
|
|
dType.SetJOGCmd(self.api,1,2)
|
||
|
|
|
||
|
|
else :
|
||
|
|
return
|
||
|
|
threading.Timer(0.1,self.stop).start()
|
||
|
|
#dType.SetWAITCmd(self.api, 1, isQueued=0)
|
||
|
|
#dType.SetJOGCmd(self.api, 1, 0)
|
||
|
|
|
||
|
|
def rearArm(self, direction):
|
||
|
|
if direction == 1:
|
||
|
|
dType.SetJOGCmd(self.api,1,3)
|
||
|
|
|
||
|
|
elif direction == -1:
|
||
|
|
dType.SetJOGCmd(self.api,1,4)
|
||
|
|
|
||
|
|
else :
|
||
|
|
return
|
||
|
|
#dType.SetWAITCmd(self.api, 1, isQueued=0)
|
||
|
|
#dType.SetJOGCmd(self.api, 1, 0)
|
||
|
|
threading.Timer(0.1,self.stop).start()
|
||
|
|
|
||
|
|
def forearm(self, direction):
|
||
|
|
if direction == 1:
|
||
|
|
dType.SetJOGCmd(self.api,1,5)
|
||
|
|
|
||
|
|
elif direction == -1:
|
||
|
|
dType.SetJOGCmd(self.api,1,6)
|
||
|
|
|
||
|
|
else :
|
||
|
|
return
|
||
|
|
#dType.SetWAITCmd(self.api, 1, isQueued=0)
|
||
|
|
#dType.SetJOGCmd(self.api, 1, 0)
|
||
|
|
threading.Timer(0.1,self.stop).start()
|
||
|
|
|
||
|
|
def gripperRotation(self, direction):
|
||
|
|
if direction == 1:
|
||
|
|
dType.SetJOGCmd(self.api,1,7)
|
||
|
|
|
||
|
|
elif direction == -1:
|
||
|
|
dType.SetJOGCmd(self.api,1,8)
|
||
|
|
|
||
|
|
else :
|
||
|
|
return
|
||
|
|
#dType.SetWAITCmd(self.api, 1, isQueued=0)
|
||
|
|
#dType.SetJOGCmd(self.api, 1, 0)
|
||
|
|
threading.Timer(0.1,self.stop).start()
|
||
|
|
|
||
|
|
def linear(self, direction):
|
||
|
|
if direction == 1:
|
||
|
|
dType.SetJOGCmd(self.api,1,9)
|
||
|
|
|
||
|
|
elif direction == -1:
|
||
|
|
dType.SetJOGCmd(self.api,1,10)
|
||
|
|
|
||
|
|
else :
|
||
|
|
return
|
||
|
|
#dType.SetWAITCmd(self.api, 1, isQueued=0)
|
||
|
|
#dType.SetJOGCmd(self.api, 1, 0)
|
||
|
|
self.lint = threading.Timer(0.2,self.stop)
|
||
|
|
self.lint.start()
|
||
|
|
|
||
|
|
def gripp(self):
|
||
|
|
print("gripping\n")
|
||
|
|
#dType.SetEndEffectorGripper(self.api, 1, 1, isQueued=0)
|
||
|
|
|
||
|
|
def letgo(self):
|
||
|
|
print("drops\n")
|
||
|
|
dType.SetEndEffectorGripper(self.api, 1, 0, isQueued=0)
|
||
|
|
|
||
|
|
def stop(self):
|
||
|
|
print("stops\n")
|
||
|
|
#dType.SetJOGCmd(self.api, 1, 0)
|
||
|
|
|
||
|
|
def stopcmp(self):
|
||
|
|
print("stops compressor\n")
|
||
|
|
dType.SetEndEffectorGripper(self.api, 0, 0, isQueued=0)
|
||
|
|
|
||
|
|
def panic(self):
|
||
|
|
dType.SetQueuedCmdForceStopExec(self.api)
|
||
|
|
|
||
|
|
def getCurrentCmd(self):
|
||
|
|
return dType.GetQueuedCmdCurrentIndex(self.api)
|
||
|
|
|
||
|
|
def JOGtest(self):
|
||
|
|
dType.SetJOGCmd(self.api,1,1)
|
||
|
|
dType.SetJOGCmd(self.api,1,9)
|
||
|
|
|
||
|
|
def JOGtest2(self):
|
||
|
|
dType.SetJOGLParams(self.api, 100, 200, isQueued=0)
|
||
|
|
dType.SetJOGCmd(self.api,1,9)
|
||
|
|
time.sleep(1)
|
||
|
|
for speed in range(100,10,-10):
|
||
|
|
time.sleep(0.3)
|
||
|
|
dType.SetJOGLParams(self.api, speed, 200, isQueued=0)
|
||
|
|
|
||
|
|
|
||
|
|
def setparams(self, name, speed):
|
||
|
|
lookup = {'base' : 0, 'reararm' : 1, 'forearm' : 2, 'gripperroation' :3}
|
||
|
|
if name == 'rail':
|
||
|
|
print("Change speed rail %d\n", speed)
|
||
|
|
#dType.SetJOGLParams(self.api, speed, 200, isQueued=0)
|
||
|
|
|
||
|
|
else:
|
||
|
|
speeds = [0,0,0,0]
|
||
|
|
speeds[lookup[name]] = speed
|
||
|
|
print("change speed" + name + "%d\n",speed)
|
||
|
|
#dType.SetJOGJointParams(self.api, speeds[0], 200, speeds[1], 200, speeds[2], 200, speeds[3], 200, isQueued = 1)
|
||
|
|
|
||
|
|
def setJOGcmd(self, cmd):
|
||
|
|
dType.SetJOGCmd(self.api,1,cmd)
|
||
|
|
|
||
|
|
candy = Candybot()
|
||
|
|
|
||
|
|
|
||
|
|
startTime = time.time()
|
||
|
|
nowTime = 0
|
||
|
|
|
||
|
|
currentMotor = 0
|
||
|
|
motorcfg = [0] * 10
|
||
|
|
motorcfg[1] = {"name" : 'base' , "positive":1, "negative":2, 'speedslot':2}
|
||
|
|
motorcfg[2] = {"name" : 'reararm' ,"positive":3, "negative":4, 'speedslot':4}
|
||
|
|
motorcfg[3] = {"name" : 'forearm' ,"positive":5, "negative":6, 'speedslot':6}
|
||
|
|
motorcfg[4] = {"name" : 'rail' ,"positive":9, "negative":10, 'speedslot':1}
|
||
|
|
|
||
|
|
friction = 0.95
|
||
|
|
movement_time = 3
|
||
|
|
max_movement_time = 5
|
||
|
|
min_speed = 1
|
||
|
|
max_speed = 100
|
||
|
|
|
||
|
|
speed = 0
|
||
|
|
stopTime = 0
|
||
|
|
isRunning = False
|
||
|
|
|
||
|
|
|
||
|
|
def adjustSpeed(motor, adjustment):
|
||
|
|
global nowTime, stopTime, currentMotor, speed, motorcfg, movement_time, max_speed, max_movement_time
|
||
|
|
|
||
|
|
if currentMotor==motor:
|
||
|
|
if (adjustment>0 and speed>0) or (adjustment<0 and speed<0):
|
||
|
|
speed += adjustment * (abs(speed)/max_speed)
|
||
|
|
else:
|
||
|
|
stopMotorFunctions()
|
||
|
|
stopTime += movement_time/2 * max_movement_time/(stopTime - nowTime)
|
||
|
|
else:
|
||
|
|
currentMotor = motor
|
||
|
|
speed = adjustment
|
||
|
|
stopTime = nowTime + movement_time
|
||
|
|
if(speed>0):
|
||
|
|
candy.setJOGcmd(motorcfg[currentMotor]['positive'])
|
||
|
|
#print("JOG command {}".format(motorcfg[currentMotor]['positive']))
|
||
|
|
else:
|
||
|
|
candy.setJOGcmd(motorcfg[currentMotor]['negative'])
|
||
|
|
# print("JOG command {}".format(motorcfg[currentMotor]['negative']))
|
||
|
|
|
||
|
|
|
||
|
|
def stopMotorFunctions():
|
||
|
|
global currentMotor, speed
|
||
|
|
currentMotor = 0
|
||
|
|
speed = 0
|
||
|
|
candy.stop()
|
||
|
|
|
||
|
|
def updateLoop():
|
||
|
|
global nowTime, stopTime, currentMotor, speed, friction, min_speed, motorcfg
|
||
|
|
nowTime = (time.time() - startTime)
|
||
|
|
threading.Timer(1, updateLoop).start()
|
||
|
|
|
||
|
|
if currentMotor != 0:
|
||
|
|
if nowTime > stopTime:
|
||
|
|
stopMotorFunctions()
|
||
|
|
return
|
||
|
|
else:
|
||
|
|
speed *= friction
|
||
|
|
if abs(speed) < min_speed:
|
||
|
|
stopMotorFunctions()
|
||
|
|
return
|
||
|
|
candy.setparams(motorcfg[currentMotor]['name'], abs(speed))
|
||
|
|
#print("JOG parameter {} to {}".format( motorcfg[currentMotor]['speedslot'], abs(speed)) )
|
||
|
|
|
||
|
|
|
||
|
|
def rt():
|
||
|
|
adjustSpeed(1, 5)
|
||
|
|
|
||
|
|
def lt():
|
||
|
|
adjustSpeed(1, -5)
|
||
|
|
|
||
|
|
def up():
|
||
|
|
adjustSpeed(2, 5)
|
||
|
|
|
||
|
|
def dn():
|
||
|
|
adjustSpeed(2, -5)
|
||
|
|
|
||
|
|
def lr():
|
||
|
|
adjustSpeed(4, 10)
|
||
|
|
|
||
|
|
def ll():
|
||
|
|
adjustSpeed(4, -10)
|
||
|
|
|
||
|
|
updateLoop()
|
||
|
|
|
||
|
|
#
|
||
|
|
#c = Candybot()
|
||
|
|
#
|
||
|
|
#
|
||
|
|
#def slide_left():
|
||
|
|
# c.linear(-1)
|
||
|
|
#
|
||
|
|
#def slide_right():
|
||
|
|
# c.linear(1)
|
||
|
|
#
|
||
|
|
#def rotate_left():
|
||
|
|
# c.base(-1)
|
||
|
|
#
|
||
|
|
#def rotate_right():
|
||
|
|
# c.base(1)
|
||
|
|
#
|
||
|
|
#def forward():
|
||
|
|
# c.forearm(1)
|
||
|
|
#
|
||
|
|
#def back():
|
||
|
|
# c.forearm(-1)
|
||
|
|
#
|
||
|
|
#def up():
|
||
|
|
# c.rearArm(1)
|
||
|
|
#
|
||
|
|
#def down():
|
||
|
|
# c.rearArm(-1)
|
||
|
|
#
|
||
|
|
def stopCmp():
|
||
|
|
candy.stopcmp()
|
||
|
|
|
||
|
|
def grab():
|
||
|
|
candy.gripp()
|
||
|
|
# c.stopcmp()
|
||
|
|
#
|
||
|
|
def drop():
|
||
|
|
candy.letgo()
|
||
|
|
# c.stopcmp()
|
||
|
|
#
|
||
|
|
|
||
|
|
def stop():
|
||
|
|
candy.panic()
|
||
|
|
#
|
||
|
|
#def getCurrentCmd():
|
||
|
|
# return c.getCurrentCmd()
|
||
|
|
#
|
||
|
|
#def updateLinearSpeed():
|
||
|
|
# pass
|
||
|
|
#
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|