tabs

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)
 
-
+    def setJOGcmd(self, cmd):
-
+        dType.SetJOGCmd(self.api,1,cmd)
-
+
-c = Candybot()
+candy = Candybot()
-
+
-
+
-def slide_left():
+startTime = time.time()
-    c.linear(-1)
+nowTime = 0
-
+
-def slide_right():
+currentMotor = 0
-    c.linear(1)
+motorcfg = [0] * 10
-    
+motorcfg[1] = {"name" : 'base' , "positive":1, "negative":2, 'speedslot':2}
-def rotate_left():
+motorcfg[2] = {"name" : 'reararm' ,"positive":3, "negative":4, 'speedslot':4}
-    c.base(-1)
+motorcfg[3] = {"name" : 'base' ,"positive":5, "negative":6, 'speedslot':6}
-
+
-def rotate_right():
+friction = 0.99
-    c.base(1)
+movement_time = 5
-
+max_movement_time = 10
-def forward():
+min_speed = 1
-    c.forearm(1)
+max_speed = 50
-
+
-def back():
+speed = 0
-    c.forearm(-1)
+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)
+	adjustSpeed(2, 5)
-
+
-def down():
+def dn():
-    c.rearArm(-1)
+	adjustSpeed(2, -5)
-
+
-def grab():
+updateLoop()
-    c.gripp()
+
-    c.stopcmp()
+#
-
+#c = Candybot()
-def drop():
+#
-    c.letgo()
+#
-    c.stopcmp()
+#def slide_left():
-
+#    c.linear(-1)
-def stop():
+#
-    c.panic()
+#def slide_right():
-
+#    c.linear(1)
-def getCurrentCmd():
+#    
-    return c.getCurrentCmd()
+#def rotate_left():
-
+#    c.base(-1)
-def updateLinearSpeed():    
+#
-    pass
+#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
+#