tabs

movearm/move.py

@@ -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
+#
+
+
+    
+