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