centrifugal scripted command in microb_cmd

[aversive.git] / projects / microb2010 / microb_cmd / microbcmd.py

diff --git a/projects/microb2010/microb_cmd/microbcmd.py b/projects/microb2010/microb_cmd/microbcmd.py
index 555141f..2a559cb 100755 (executable)
--- a/projects/microb2010/microb_cmd/microbcmd.py
+++ b/projects/microb2010/microb_cmd/microbcmd.py
@@ -107,7 +107,7 @@ def prog_page(ser, addr, buf):
 def prog_metadata(ser, addr, buf):
     length = len(buf)
     crc = do_crc(buf)
-    page = struct.pack("<LL", length, crc)
+    page = struct.pack(">LL", length, crc)
     filename = os.path.join(MICROB_PATH, "binaries/%x_%x.bin"%(length, crc))
     print "saving in %s"%(filename)
     f = open(filename, "w")
@@ -116,13 +116,8 @@ def prog_metadata(ser, addr, buf):
     return prog_page(ser, addr, page)
 
 def get_same_bin_file(ser):
-    # hack because dump32 does not work
-    l1 = read32(ser, METADATA_ADDR) & 0xFFFF
-    l2 = read32(ser, METADATA_ADDR + 2) & 0xFFFF
-    c1 = read32(ser, METADATA_ADDR + 4) & 0xFFFF
-    c2 = read32(ser, METADATA_ADDR + 6) & 0xFFFF
-    l = l1 + l2 << 16
-    c = c1 + c2 << 16
+    l = read32(ser, METADATA_ADDR)
+    c = read32(ser, METADATA_ADDR + 4)
     filename = os.path.join(MICROB_PATH,
                             "binaries/%x_%x.bin"%(l, c))
     print filename
@@ -334,8 +329,8 @@ class Interp(cmd.Cmd):
 
         addr = 0
 
-        #old_buf = get_same_bin_file(self.ser)
-        old_buf = None
+        old_buf = get_same_bin_file(self.ser)
+        #old_buf = None
 
         while addr < len(buf):
             if addr > METADATA_ADDR and old_buf != None and \
@@ -378,12 +373,36 @@ class Interp(cmd.Cmd):
         filename = os.path.join(MICROB_PATH, "../ballboard/main.bin")
         self.bootloader(filename, 3)
 
+    def do_centrifugal(self, args):
+        try:
+            sa, sd, aa, ad = [int(x) for x in shlex.shlex(args)]
+        except:
+            print "args: speed_a, speed_d, acc_a, acc_d"
+            return
+        print sa, sd, aa, ad
+        time.sleep(10)
+        self.ser.write("traj_speed angle %d\n"%(sa))
+        time.sleep(0.1)
+        self.ser.write("traj_speed distance %d\n"%(sd))
+        time.sleep(0.1)
+        self.ser.write("traj_acc angle %d\n"%(aa))
+        time.sleep(0.1)
+        self.ser.write("traj_acc distance %d\n"%(ad))
+        time.sleep(0.1)
+        self.ser.write("goto da_rel 800 180\n")
+        time.sleep(3)
+        self.ser.flushInput()
+        self.ser.write("position show\n")
+        time.sleep(1)
+        print self.ser.read()
+
     def do_toto(self, args):
-        for i in range(10):
-            time.sleep(1)
-            self.ser.write("pwm s3(3C) 200\n")
-            time.sleep(1)
-            self.ser.write("pwm s3(3C) 250\n")
+        print args
+        time.sleep(1)
+        self.ser.write("position set 0 0 0\n")
+        time.sleep(1)
+        self.ser.write("pwm s3(3C) 250\n")
+
 
 if __name__ == "__main__":
     try: