From 1db35c90c84701225ab6a3cbcec492ae348ffea5 Mon Sep 17 00:00:00 2001 From: adligeerik Date: Sat, 24 Nov 2018 16:04:13 +0100 Subject: [PATCH] tabs --- movearm/move.py | 172 +++++++++++++++++++++++++++++++++++------------- 1 file changed, 125 insertions(+), 47 deletions(-) diff --git a/movearm/move.py b/movearm/move.py index 5ab0123..6cefe18 100644 --- a/movearm/move.py +++ b/movearm/move.py @@ -2,8 +2,6 @@ import time import threading import DobotDllType as dType - - """ Six actuators: linear rail @@ -173,53 +171,133 @@ class Candybot: speeds[lookup[name]] = speed dType.SetJOGJointParams(self.api, speeds[0], 200, speeds[1], 200, speeds[2], 200, speeds[3], 200, isQueued = 1) - - - -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 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(): - 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 - + 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 +#