From 06e405c86cfb5022a1285aaece562023dd9b6b37 Mon Sep 17 00:00:00 2001 From: adligeerik Date: Sat, 24 Nov 2018 18:26:53 +0100 Subject: [PATCH] test for chat --- movearm/move.py | 5 +- movearm/moveCmdTester.py | 324 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 327 insertions(+), 2 deletions(-) create mode 100644 movearm/moveCmdTester.py diff --git a/movearm/move.py b/movearm/move.py index 0ecf36d..4bbb4bd 100644 --- a/movearm/move.py +++ b/movearm/move.py @@ -185,6 +185,7 @@ 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 @@ -255,10 +256,10 @@ def dn(): adjustSpeed(2, -5) def lr(): - adjustSpeed(9, -5) + adjustSpeed(4, 10) def ll(): - adjustSpeed(10, -5) + adjustSpeed(4, -10) updateLoop() diff --git a/movearm/moveCmdTester.py b/movearm/moveCmdTester.py new file mode 100644 index 0000000..2dbe9be --- /dev/null +++ b/movearm/moveCmdTester.py @@ -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): + 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 +# + + + +