movearm/move.py

@@ -2,35 +2,8 @@ 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",
@@ -50,7 +23,7 @@ class Candybot:
 
             #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.SetJOGJointParams(self.api, 200, 200, 200, 200, 200, 200, 200, 200, isQueued = 1)
             dType.SetJOGCommonParams(self.api, 100, 100, isQueued = 1)
 
             #linear rail
@@ -127,8 +100,7 @@ class Candybot:
             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()
+        threading.Timer(0.1,self.stop).start()
 
     def gripp(self):
         dType.SetEndEffectorGripper(self.api, 1,  1, isQueued=0)
@@ -143,162 +115,4 @@ class Candybot:
         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" : '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
-#
-
-
-    
-        
+        dType.SetQueuedCmdForceStopExec(self.api)