sending command sequential number (plus copy it back in status report)

ver1/basic.bas

@@ -7,11 +7,12 @@ TIMEOUT con 100
 servoindex var byte
 time var word ; unknown units, but it seems that 16bit TCNT counter is running
 battery var word
+lastCmdId var word;
 
 servopos var word(24)
 servopwr var word(24)
 
-inputbuf var byte(24*2+2+1) ; execute at, servos position, 
+inputbuf var byte(24*2+2+2+1) ; cmdId, execute at, servos position, 
 executeAt var word
 servoCmd var word(24)
 
@@ -27,6 +28,7 @@ sethserial1 h38400
 
 gosub stopAllServos
 
+lastCmdId = 0
 main
 	gosub readServoStatus
 	gosub sendServoStatus
@@ -71,12 +73,13 @@ PACKET_START con 0xAB
 
 sendServoStatus
 	dataLen var byte
-	dataLen = 2+2+24*2*2
+	dataLen = 2+2+2+24*2*2
 	chSum = 0
 	hserout s_out, [PACKET_START, dataLen]
+	hserout s_out, [lastCmdId &0xFF, lastCmdId>>8 ]
 	hserout s_out, [time &0xFF, time>>8 ]
 	hserout s_out, [battery & 0xFF, battery >> 8]
-	chSum = chSum + dataLen + (time & 0xFF) + ((time >> 8)&0xFF) + (battery & 0xFF) + ((battery >> 8)&0xFF)
+	chSum = chSum + dataLen + (lastCmdId & 0xFF) + ((lastCmdId >> 8)&0xFF) + (time & 0xFF) + ((time >> 8)&0xFF) + (battery & 0xFF) + ((battery >> 8)&0xFF)
 	for servoindex = 0 to 23
 		hserout s_out, [servopos( servoindex )&0xFF, servopos( servoindex )>>8]
 		chSum = chSum + (servopos( servoindex ) & 0xFF) + (servopos( servoindex ) >> 8)
@@ -97,17 +100,17 @@ receiveServoCmd
 	hserin s_in, timeoutException, TIMEOUT, [tmp]
 	packetSize = tmp
 	chSum = tmp; checksum is with length included
-	if packetSize = 24*2+2 then
+	if packetSize = 24*2+2+2 then
 		for i = 0 to packetSize; including check sum
 			hserin s_in, timeoutException, TIMEOUT, [tmp]
 			inputbuf(i) = tmp
 			chSum = chSum + tmp
 		next
 		if chSum = 0 then
-			; TODO unpack data
-			executeAt = inputbuf(0) + 256*inputbuf(1)
+			lastCmdId = inputbuf(0) + 256*inputbuf(1)
+			executeAt = inputbuf(2) + 256*inputbuf(3)
 			for i = 0 to 23
-				servoCmd(i) = inputbuf(2+2*i) + 256*inputbuf(3+2*i)
+				servoCmd(i) = inputbuf(4+2*i) + 256*inputbuf(5+2*i)
 			next
 			return 1
 		else

ver1/fireant.py

@@ -49,6 +49,8 @@ def __init__( self, name, com, runInit=True ):
     self.servoPosRaw = None
     self.power = None
     self.lastCmd = None
+    self.cmdId = 0
+    self.receivedCmdId = None
     if runInit:
       self.init()
 
@@ -61,18 +63,20 @@ def readStatus( self ):
     chSum = size;
     buf = self.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]
-    assert size-4 == 2*2*NUM_SERVOS, (size, NUM_SERVOS)
-    raw = struct.unpack_from( "HH"+"hH"*NUM_SERVOS, buf ) # big indian
+    assert size-4-2 == 2*2*NUM_SERVOS, (size, NUM_SERVOS)
+    raw = struct.unpack_from( "HHH"+"hH"*NUM_SERVOS, buf ) # big indian
+    self.receivedCmdId = raw[0]
     if verbose:
       print raw
-      print "TIME\t%d" % raw[0]
-    self.tickTime = raw[0]
+      print "TIME\t%d" % raw[1]
+    self.tickTime = raw[1]
     self.time = self.tickTime/1000.0 # TODO 16bit overflow
-    self.power = raw[1]*5/1024.
-    self.servoPosRaw = [raw[2::2][i]*10/SERVO_DEGREE for i in servoPin]
+    self.power = raw[2]*5/1024.
+    self.servoPosRaw = [raw[3::2][i]*10/SERVO_DEGREE for i in servoPin]
     return raw
 
   def writeCmd( self, cmd ):
+    self.cmdId += 1
     if verbose:
       print "SEND", self.time
     executeAt = (self.tickTime + int(self.servoUpdateTime*1000)) & 0xFFFF
@@ -81,7 +85,7 @@ def writeCmd( self, cmd ):
     for i,v in zip(servoPin, cmd): # reindexing
       if v != None and v != STOP_SERVO:
         cmd2[i] = v*SERVO_DEGREE/10
-    buf = struct.pack( "H"+"h"*NUM_SERVOS, executeAt, *cmd2 )
+    buf = struct.pack( "HH"+"h"*NUM_SERVOS, self.cmdId & 0xFFFF, executeAt, *cmd2 )
     self.com.write( PACKET_START )
     self.com.write( chr(len(buf)) )
     self.com.write( buf )
@@ -238,7 +242,7 @@ def readTest( self, numBytes = 1000 ):
       cmd[1] = 0
       cmd[2] = -300
       robot.update( cmd=cmd )
-      print robot.time, robot.servoPosRaw[:4]
+      print robot.time, robot.cmdId, robot.receivedCmdId, robot.servoPosRaw[:4]
     sys.exit(0)
 
   robot = FireAnt( robotName, com )