commit
071236bc0e
@ -0,0 +1,48 @@
|
|||||||
|
import threading
|
||||||
|
import DobotDllType as dType
|
||||||
|
|
||||||
|
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 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
|
||||||
|
dType.SetHOMECmd(api, temp = 0, isQueued = 1)
|
||||||
|
|
||||||
|
#Async PTP Motion
|
||||||
|
for i in range(0, 5):
|
||||||
|
if i % 2 == 0:
|
||||||
|
offset = 50
|
||||||
|
else:
|
||||||
|
offset = -50
|
||||||
|
lastIndex = dType.SetPTPCmd(api, dType.PTPMode.PTPMOVLXYZMode, 200 + offset, offset, offset, offset, 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)
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,12 @@
|
|||||||
|
#ifndef DOBOTDLL_GLOBAL_H
|
||||||
|
#define DOBOTDLL_GLOBAL_H
|
||||||
|
|
||||||
|
#include <QtCore/qglobal.h>
|
||||||
|
|
||||||
|
#if defined(DOBOTDLL_LIBRARY)
|
||||||
|
# define DOBOTDLLSHARED_EXPORT Q_DECL_EXPORT
|
||||||
|
#else
|
||||||
|
# define DOBOTDLLSHARED_EXPORT Q_DECL_IMPORT
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif // DOBOTDLL_GLOBAL_H
|
||||||
@ -0,0 +1,78 @@
|
|||||||
|
import threading
|
||||||
|
import time
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
startTime = time.time()
|
||||||
|
nowTime = 0;
|
||||||
|
|
||||||
|
currentMotor = 0
|
||||||
|
motorcfg = [0] * 10
|
||||||
|
motorcfg[1] = {"positive":1, "negative":2, 'speedslot':2}
|
||||||
|
motorcfg[2] = {"positive":3, "negative":4, 'speedslot':4}
|
||||||
|
motorcfg[3] = {"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):
|
||||||
|
print("JOG command {}".format(motorcfg[currentMotor]['positive']))
|
||||||
|
else:
|
||||||
|
print("JOG command {}".format(motorcfg[currentMotor]['negative']))
|
||||||
|
|
||||||
|
|
||||||
|
def stopMotorFunctions():
|
||||||
|
global currentMotor, speed
|
||||||
|
currentMotor = 0
|
||||||
|
speed = 0
|
||||||
|
print ("JOB command stop all motors")
|
||||||
|
|
||||||
|
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
|
||||||
|
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();
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -0,0 +1,324 @@
|
|||||||
|
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):
|
||||||
|
dType.SetEndEffectorGripper(self.api, 1, 1, isQueued=0)
|
||||||
|
|
||||||
|
def letgo(self):
|
||||||
|
dType.SetEndEffectorGripper(self.api, 1, 0, isQueued=0)
|
||||||
|
|
||||||
|
def stop(self):
|
||||||
|
dType.SetJOGCmd(self.api, 1, 0)
|
||||||
|
|
||||||
|
def stopcmp(self):
|
||||||
|
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':
|
||||||
|
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" : '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 = 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:
|
||||||
|
if (adjustment>0 and speed>0) or (adjustment<0 and speed<0):=
|
||||||
|
speed += adjustment * 1-(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 fw():
|
||||||
|
adjustSpeed(2, 5)
|
||||||
|
|
||||||
|
def bc():
|
||||||
|
adjustSpeed(2, -5)
|
||||||
|
|
||||||
|
def up():
|
||||||
|
adjustSpeed(3, 5)
|
||||||
|
|
||||||
|
def dn():
|
||||||
|
adjustSpeed(3, -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
|
||||||
|
#
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -0,0 +1,294 @@
|
|||||||
|
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
|
||||||
|
return
|
||||||
|
|
||||||
|
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 " + str(speed) +"\n" )
|
||||||
|
#dType.SetJOGLParams(self.api, speed, 200, isQueued=0)
|
||||||
|
|
||||||
|
else:
|
||||||
|
speeds = [0,0,0,0]
|
||||||
|
speeds[lookup[name]] = speed
|
||||||
|
print("change speed " + name + str(speed))
|
||||||
|
#dType.SetJOGJointParams(self.api, speeds[0], 200, speeds[1], 200, speeds[2], 200, speeds[3], 200, isQueued = 1)
|
||||||
|
|
||||||
|
def setJOGcmd(self, cmd):
|
||||||
|
print("Joint "+ str(cmd) + "is moving \n")
|
||||||
|
#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 fw():
|
||||||
|
adjustSpeed(2, 5)
|
||||||
|
|
||||||
|
def bc():
|
||||||
|
adjustSpeed(2, -5)
|
||||||
|
|
||||||
|
def up():
|
||||||
|
adjustSpeed(3, 5)
|
||||||
|
|
||||||
|
def dn():
|
||||||
|
adjustSpeed(3, -5)
|
||||||
|
|
||||||
|
def lr():
|
||||||
|
adjustSpeed(4, 10)
|
||||||
|
|
||||||
|
def ll():
|
||||||
|
adjustSpeed(4, -10)
|
||||||
|
updateLoop()
|
||||||
|
|
||||||
|
|
||||||
|
def stopCmp():
|
||||||
|
candy.stopcmp()
|
||||||
|
|
||||||
|
def grab():
|
||||||
|
candy.gripp()
|
||||||
|
|
||||||
|
def drop():
|
||||||
|
candy.letgo()
|
||||||
|
|
||||||
|
def stop():
|
||||||
|
candy.panic()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Binary file not shown.
@ -0,0 +1,62 @@
|
|||||||
|
#include <HX711.h>
|
||||||
|
|
||||||
|
|
||||||
|
HX711 scale;
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
Serial.begin(38400);
|
||||||
|
Serial.println("HX711 Demo");
|
||||||
|
|
||||||
|
Serial.println("Initializing the scale");
|
||||||
|
// parameter "gain" is ommited; the default value 128 is used by the library
|
||||||
|
// HX711.DOUT - pin #A1
|
||||||
|
// HX711.PD_SCK - pin #A0
|
||||||
|
scale.begin(2, 3);
|
||||||
|
|
||||||
|
Serial.println("Before setting up the scale:");
|
||||||
|
Serial.print("read: \t\t");
|
||||||
|
Serial.println(scale.read()); // print a raw reading from the ADC
|
||||||
|
|
||||||
|
Serial.print("read average: \t\t");
|
||||||
|
Serial.println(scale.read_average(20)); // print the average of 20 readings from the ADC
|
||||||
|
|
||||||
|
Serial.print("get value: \t\t");
|
||||||
|
Serial.println(scale.get_value(5)); // print the average of 5 readings from the ADC minus the tare weight (not set yet)
|
||||||
|
|
||||||
|
Serial.print("get units: \t\t");
|
||||||
|
Serial.println(scale.get_units(5), 1); // print the average of 5 readings from the ADC minus tare weight (not set) divided
|
||||||
|
// by the SCALE parameter (not set yet)
|
||||||
|
|
||||||
|
scale.set_scale(2280.f); // this value is obtained by calibrating the scale with known weights; see the README for details
|
||||||
|
scale.tare(); // reset the scale to 0
|
||||||
|
|
||||||
|
Serial.println("After setting up the scale:");
|
||||||
|
|
||||||
|
Serial.print("read: \t\t");
|
||||||
|
Serial.println(scale.read()); // print a raw reading from the ADC
|
||||||
|
|
||||||
|
Serial.print("read average: \t\t");
|
||||||
|
Serial.println(scale.read_average(20)); // print the average of 20 readings from the ADC
|
||||||
|
|
||||||
|
Serial.print("get value: \t\t");
|
||||||
|
Serial.println(scale.get_value(5)); // print the average of 5 readings from the ADC minus the tare weight, set with tare()
|
||||||
|
|
||||||
|
Serial.print("get units: \t\t");
|
||||||
|
Serial.println(scale.get_units(5), 1); // print the average of 5 readings from the ADC minus tare weight, divided
|
||||||
|
// by the SCALE parameter set with set_scale
|
||||||
|
|
||||||
|
Serial.println("Readings:");
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
//Serial.print("one reading:\t");
|
||||||
|
//Serial.print(scale.get_units(), 1);
|
||||||
|
//Serial.print("\t| average:\t");
|
||||||
|
if (Serial.available() > 0) {
|
||||||
|
Serial.println(scale.get_units(10), 1);
|
||||||
|
|
||||||
|
scale.power_down(); // put the ADC in sleep mode
|
||||||
|
delay(100);
|
||||||
|
scale.power_up();
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
Reference in new issue