timeout fixed on ax12 library, now a more fluid movement experince

thiagohersan · Jul 28, 2014 · 81eee19 · 81eee19
1 parent f2ab87b
commit 81eee19
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 22 deletions.
diff --git a/Python/ax12/ax12.py b/Python/ax12/ax12.py
@@ -129,15 +129,15 @@ class Ax12:
     RPI_DIRECTION_PIN = 18
     RPI_DIRECTION_TX = GPIO.HIGH
     RPI_DIRECTION_RX = GPIO.LOW
-    RPI_DIRECTION_SWITCH_DELAY = 0.0005
+    RPI_DIRECTION_SWITCH_DELAY = 0.00005
 
     # static variables
     port = None
     gpioSet = False
 
     def __init__(self):
         if(Ax12.port == None):
-            Ax12.port = Serial("/dev/ttyAMA0", baudrate=1000000, timeout=0.5)
+            Ax12.port = Serial("/dev/ttyAMA0", baudrate=1000000, timeout=0.001)
         if(not Ax12.gpioSet):
             GPIO.setwarnings(False)
             GPIO.setmode(GPIO.BCM)
@@ -168,7 +168,6 @@ def direction(self,d):
         sleep(Ax12.RPI_DIRECTION_SWITCH_DELAY)
 
     def readData(self,id):
-        sleep(Ax12.TX_DELAY_TIME)
         self.direction(Ax12.RPI_DIRECTION_RX)
         try:
             h1 = Ax12.port.read()
@@ -398,7 +397,7 @@ def action(self):
         outData += chr(Ax12.AX_ACTION)
         outData += chr(Ax12.AX_ACTION_CHECKSUM)
         Ax12.port.write(outData)
-        sleep(Ax12.TX_DELAY_TIME)
+        #sleep(Ax12.TX_DELAY_TIME)
 
     def setTorqueStatus(self, id, status):
         self.direction(Ax12.RPI_DIRECTION_TX)

diff --git a/Python/tests/testOSC2.py b/Python/tests/testOSC2.py
@@ -1,6 +1,6 @@
 from liblo import *
 import sys
-from time import sleep
+from time import sleep,time
 from random import random
 import math
 sys.path.append("..")
@@ -12,11 +12,12 @@ def __init__(self):
 
     @make_method('/angles', 'ffffff')
     def foo_callback(self, path, args):
-        print args
+        #print args
+        #print map(lambda x:math.degrees(x),args)
         anglesData.append(args)
 
 def moveMotors():
-    print "wrinting on the motors..."
+    #print "wrinting on the motors..."
     angles = anglesData[0]
 
     del anglesData[0]
@@ -33,10 +34,13 @@ def moveMotors():
         try:
             servos.moveSpeedRW(i+1,angPos,1023)
         except:
-            pass
+            try:
-        sleep(0.03)
+                servos.moveSpeedRW(i+1,angPos,1023)
+            except:
+                print "3rd time c.p."
+                pass
+
     servos.action()
-    sleep(0.02)
 
 
 def initMotors():
@@ -47,7 +51,7 @@ def initMotors():
     for i in range(2):
         for mt in range(1,6+1):
             pp = p if mt%2==0 else 1024-p
-            servos.moveSpeedRW(mt,pp,50)
+            servos.moveSpeedRW(mt,pp,450)
             sleep(0.01)
         servos.action()
         p = 1320 - p
@@ -76,15 +80,24 @@ def setup():
 
 if __name__=="__main__":
     setup()
-    while(1):
+    timeAverage = 0
-        try :
+    count = 0
-           while 1:
+    try :
-            if(anglesData):
+       while 1:
-                moveMotors()
+        lastTime = time()
-            sleep(0.1)
+        if(anglesData):
-
+            moveMotors()
-        except KeyboardInterrupt :
+        if(time()-lastTime > 0.001):
-            print  "\nStoping OSCServer."
+            print "%f" % (time()-lastTime)
-            server.stop()
+        timeAverage += (time()-lastTime)
-            sys.exit()
+        count +=1
+        if count >= 100:
+           # print "%f" % (timeAverage/100)
+            timeAverage = 0
+            count = 0
+
+    except KeyboardInterrupt :
+        print  "\nStoping OSCServer."
+        server.stop()
+        sys.exit()