Compare commits

...

4 Commits

Author SHA1 Message Date
adligeerik 1db35c90c8 tabs
6 years ago
adligeerik 7d3723eab3 Merge branch 'master' of https://git.elab.kth.se/marek/Twitch-grabs-candy
6 years ago
adligeerik 902acd4cf6 functi
6 years ago
adligeerik b6c8cc2f1b Example
6 years ago

@ -2,8 +2,35 @@ 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",
@ -23,7 +50,7 @@ class Candybot:
#Async Motion Params Setting
dType.SetHOMEParams(self.api, 250, 0, 50, 0, isQueued = 1)
dType.SetJOGJointParams(self.api, 200, 200, 200, 200, 200, 200, 200, 200, 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
@ -100,7 +127,8 @@ class Candybot:
return
#dType.SetWAITCmd(self.api, 1, isQueued=0)
#dType.SetJOGCmd(self.api, 1, 0)
threading.Timer(0.1,self.stop).start()
self.lint = threading.Timer(0.2,self.stop)
self.lint.start()
def gripp(self):
dType.SetEndEffectorGripper(self.api, 1, 1, isQueued=0)
@ -115,4 +143,162 @@ class Candybot:
dType.SetEndEffectorGripper(self.api, 0, 0, isQueued=0)
def panic(self):
dType.SetQueuedCmdForceStopExec(self.api)
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':
dType.SetJOGLParams(self.api, speed, 200, isQueued=0)
else:
speeds = [0,0,0,0]
speeds[lookup[name]] = 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" : 'base' ,"positive":5, "negative":6, 'speedslot':6}
friction = 0.99
movement_time = 5
max_movement_time = 10
min_speed = 1
max_speed = 50
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:
speed += adjustment * (abs(speed)/max_speed)
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(0.2, 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)
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 grab():
# c.gripp()
# c.stopcmp()
#
#def drop():
# c.letgo()
# c.stopcmp()
#
#def stop():
# c.panic()
#
#def getCurrentCmd():
# return c.getCurrentCmd()
#
#def updateLinearSpeed():
# pass
#

Loading…
Cancel
Save