support for 9 servos (instead of 3)

robotika · Jan 13, 2014 · 5886f86 · 5886f86
1 parent 1e0b191
commit 5886f86
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 6 deletions.
diff --git a/force_test/force_test.ino b/force_test/force_test.ino
@@ -6,11 +6,17 @@
 #include <Orion.h>
 #include <SPI.h>
 
+#define LFC 21
+#define LFF 22 
+#define LFT 23
 #define LMC 20
 #define LMF 19
-#define LMT 18 
+#define LMT 18
+#define LRC 17
+#define LRF 16
+#define LRT 0 
 
-const unsigned char servoPins[] = { LMC, LMF, LMT };
+const unsigned char servoPins[] = { LFC, LFF, LFT,   LMC, LMF, LMT,   LRC, LRF, LRT   };
 const unsigned int NUM_SERVOS = sizeof(servoPins)/sizeof(unsigned char);
 
 const uint8_t ECHO_CHAR = 'D'; // in memory of Daisy robot

diff --git a/force_test/force_test.py b/force_test/force_test.py
@@ -17,6 +17,8 @@
 from serial_servo import LogIt, ReplyLog
 from serial_servo import pos2angles
 
+NUM_SERVOS = 9
+
 SERIAL_BAUD = 38400
 
 PACKET_START = chr(0xAB)
@@ -33,7 +35,8 @@ def readRobotStatus( com ):
   chSum = size;
   buf = com.read( size + 1 ) # read data + checksum
   assert (size+sum([ord(x) for x in buf])) % 256 == 0, [hex(ord(x)) for x in buf]
-  raw = struct.unpack_from( "HHhhhhhh", buf ) # big indian
+  assert size-4 == 4*NUM_SERVOS, (size, NUM_SERVOS)
+  raw = struct.unpack_from( "HH"+"hh"*NUM_SERVOS, buf ) # big indian
   if verbose:
     print raw
     print "TIME\t%d" % raw[0]
@@ -46,7 +49,7 @@ def writeRobotCmd( com, cmd, servoTime = 100 ):
   g_time += servoTime
   if verbose:
     print "SEND", g_time
-  buf = struct.pack( "HHHhhh", g_time, 0xFFFF, servoTime, *cmd )
+  buf = struct.pack( "HHH"+"h"*NUM_SERVOS, g_time, 0xFFFF, servoTime, *cmd )
   com.write( PACKET_START )
   com.write( chr(len(buf)) )
   com.write( buf )
@@ -123,7 +126,7 @@ def record( com, filename, loopLen ):
   for i in xrange( loopLen ):
     status = readRobotStatus( com )
     f.write( str(status) + '\n' )
-    writeRobotCmd( com, cmd=[STOP_SERVO, STOP_SERVO, STOP_SERVO] )
+    writeRobotCmd( com, cmd=[STOP_SERVO]*NUM_SERVOS )
   f.close()
   print "END"
 
@@ -136,7 +139,7 @@ def replay( com, filename ):
     cmd = oldStatus[2::2]
     print cmd
     writeRobotCmd( com, cmd=cmd )
-  writeRobotCmd( com, cmd=[STOP_SERVO, STOP_SERVO, STOP_SERVO] )
+  writeRobotCmd( com, cmd=[STOP_SERVO]*NUM_SERVOS )
   print "END"