functi

movearm/move.py

@@ -4,7 +4,7 @@ import DobotDllType as dType
 
 
 
-'''
+"""
 Six actuators:
     linear rail
     base rotation 
@@ -23,9 +23,16 @@ 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",
@@ -45,7 +52,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
@@ -122,7 +129,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)
@@ -137,4 +145,82 @@ 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)
+
+
+
+
+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
+
+
+
+    
+