From: zer0 Date: Mon, 5 Apr 2010 12:07:24 +0000 (+0200) Subject: microb_cmd X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=9fa8975e7eff1095c2766a34605048b46b3ae880;p=aversive.git microb_cmd --- diff --git a/projects/microb2010/microb_cmd/microbcmd.py b/projects/microb2010/microb_cmd/microbcmd.py index 4e923c6..555141f 100755 --- a/projects/microb2010/microb_cmd/microbcmd.py +++ b/projects/microb2010/microb_cmd/microbcmd.py @@ -5,10 +5,11 @@ import serial from select import select import cmd #import pylab -from matplotlib import pylab +from matplotlib import pylab from math import * -import numpy +import struct +import numpy import shlex import time import math @@ -25,6 +26,7 @@ log.setLevel(1) MICROB_PATH=os.path.dirname(sys.argv[0]) SPM_PAGE_SIZE = 256 +METADATA_ADDR = 256 def crc_ccitt_update (crc, data): """crc argument is the previous value of 16 bits crc (the initial @@ -34,7 +36,7 @@ def crc_ccitt_update (crc, data): data ^= (crc & 0xff) data ^= (data << 4) data &= 0xff - + ret = (data << 8) & 0xffff ret |= ((crc >> 8) & 0xff) ret ^= ((data >> 4) & 0xff) @@ -53,8 +55,8 @@ def do_crc(buf): def prog_page(ser, addr, buf): """program a page from buf at addr""" - - # switch in program mode + + # switch in program mode ser.flushInput() ser.write('p') @@ -75,7 +77,7 @@ def prog_page(ser, addr, buf): while i < SPM_PAGE_SIZE and i < len(buf): page[i] = buf[i] i += 1 - + # send data i = 0 while i < SPM_PAGE_SIZE: @@ -102,21 +104,62 @@ def prog_page(ser, addr, buf): return -1 return 0 +def prog_metadata(ser, addr, buf): + length = len(buf) + crc = do_crc(buf) + page = struct.pack(""%(filename) + return buf + def read32(ser, addr): """read a 32 bits value at addr""" - - # switch in program mode + + ser.write('\n') + ser.write('\n') + ser.write('\n') + time.sleep(0.2) ser.flushInput() + + # switch in program mode ser.write('d') # send address + time.sleep(0.1) + s = ser.readline() + print repr(s) if not s.endswith("addr?\r\n"): print "failed (don't match addr)" return -1 + print addr ser.write("%x\n"%addr) s = ser.readline() - return int(s) + print repr(s) + return int(s, 16) def check_crc(ser, buf, offset, size): """Process the crc of buf, ask for a crc of the flash, and check @@ -170,77 +213,6 @@ class SerialLogger: self.fout.write(s) self.ser.write(s) - - - - -""" -fig = figure() - -ax = subplot(111) - - -X = 45. -Y = -10. -l1 = 9. -l2 = 21.13 -l3 = 47.14 - -l_mirror = 249. -h_mirror = 13. - - -def ang2_a_mirror(b): - x2 = X+l1*math.cos(b) - y2 = Y+l1*math.sin(b) - - A = (l3**2+x2**2+y2**2-l2**2)/(2*l3) - - DELTA = -(A**2-x2**2-y2**2) - B = +math.sqrt(DELTA) - - D = x2**2+y2**2 - - c_a = (x2*A+y2*B)/D - s_a = -(x2*B-y2*A)/D - - a = math.atan2(s_a, c_a) - return x2, y2, c_a, s_a, a - - -def ang2_H_L(l_telemetre, c_a, s_a, a): - d = h_mirror*c_a/s_a - H = (l_telemetre - l_mirror - d)*math.sin(2*a) - L = l_mirror + d + H/math.tan(2*a) - return H, L - -all_p = [] -for b in xrange(0, 360, 20): - b = b*2*math.pi / 360. - - x2, y2, c_a, s_a, a = ang2_a_mirror(b) - x1 = l3*c_a - y1 = l3*s_a - - px = [0, x1, x2, X] - py = [0, y1, y2, Y] - - all_p+=[px, py] - - print math.sqrt((x2-x1)**2+(y2-y1)**2) - - H, L = ang2_H_L(400., c_a, s_a, a) - print H, L - -ax.plot(*all_p) - -show() - -""" - - - - class Interp(cmd.Cmd): prompt = "Microb> " def __init__(self, tty, baudrate=57600): @@ -262,7 +234,7 @@ class Interp(cmd.Cmd): log logs to /tmp/microb.log or the last used file""" if self.serial_logging: - log.error("Already logging to %s and %s" % (self.ser.filein, + log.error("Already logging to %s and %s" % (self.ser.filein, self.ser.fileout)) else: self.serial_logging = True @@ -279,19 +251,19 @@ class Interp(cmd.Cmd): print "Can't parse arguments" self.ser = SerialLogger(self.ser, *files) - log.info("Starting serial logging to %s and %s" % (self.ser.filein, + log.info("Starting serial logging to %s and %s" % (self.ser.filein, self.ser.fileout)) def do_unlog(self, args): if self.serial_logging: - log.info("Stopping serial logging to %s and %s" % (self.ser.filein, + log.info("Stopping serial logging to %s and %s" % (self.ser.filein, self.ser.fileout)) self.ser = self.ser.ser self.serial_logging = False else: log.error("No log to stop") - + def do_raw(self, args): "Switch to RAW mode" @@ -300,14 +272,14 @@ class Interp(cmd.Cmd): stdin_termios = termios.tcgetattr(stdin) raw_termios = stdin_termios[:] - + try: log.info("Switching to RAW mode") # iflag - raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT | - termios.PARMRK | termios.ISTRIP | - termios.INLCR | termios.IGNCR | + raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT | + termios.PARMRK | termios.ISTRIP | + termios.INLCR | termios.IGNCR | termios.ICRNL | termios.IXON) # oflag raw_termios[1] &= ~termios.OPOST; @@ -315,8 +287,8 @@ class Interp(cmd.Cmd): raw_termios[2] &= ~(termios.CSIZE | termios.PARENB); raw_termios[2] |= termios.CS8; # lflag - raw_termios[3] &= ~(termios.ECHO | termios.ECHONL | - termios.ICANON | termios.ISIG | + raw_termios[3] &= ~(termios.ECHO | termios.ECHONL | + termios.ICANON | termios.ISIG | termios.IEXTEN); termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios) @@ -346,511 +318,9 @@ class Interp(cmd.Cmd): finally: termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios) log.info("Back to normal mode") - - - def do_arm_x(self, args): - fsdf - my_h = 100 - my_r = 220 - my_ang = 90 - - self.ser.write("armxy %d %d %d\n"%(my_h, -my_r, my_ang)) - time.sleep(1) - - for i in xrange(-my_r, my_r, 25): - self.ser.write("armxy %d %d %d\n"%(my_h, i, my_ang)) - self.ser.flushInput() - - time.sleep(0.03) - - def do_arm_y(self, args): - my_x = 80 - my_r = 145 - my_ang = 0 - self.ser.write("armxy %d %d %d\n"%(-my_r, my_x, my_ang)) - time.sleep(1) - - for i in xrange(-my_r, my_r, 25): - self.ser.write("armxy %d %d %d\n"%(i, my_x, my_ang)) - self.ser.flushInput() - - time.sleep(0.03) - - def do_arm_circ(self, args): - add_h = 120 - add_d = 120 - l = 70 - for i in xrange(0, 360, 10): - x = l*math.cos(i*math.pi/180) - y = l*math.sin(i*math.pi/180) - - - self.ser.write("armxy %d %d 90\n"%(x+add_h, y+add_d)) - self.ser.flushInput() - - time.sleep(0.05) - - def do_arm_init(self, args): - self.arm_h = 130 - self.arm_v = 130 - self.mov_max = 20 - - self.ser.write("armxy %d %d\n"%(self.arm_h, self.arm_v)) - - def arm_py_goto(self, h, v, a): - """ - dh, dv = h-self.arm_h, v-self.arm_v - d = math.sqrt(dh**2 + dv**2) - - old_h = self.arm_h - old_v = self.arm_v - - mov_todo = int(d/self.mov_max) - for i in xrange(1, mov_todo): - p_h = dh*i/mov_todo - p_v = dv*i/mov_todo - - new_h = old_h+p_h - new_v = old_v+p_v - - self.ser.write("armxy %d %d %d\n"%(new_h, new_v, a)) - self.ser.flushInput() - self.arm_h = new_h - self.arm_v = new_v - - time.sleep(0.04) - - self.ser.write("armxy %d %d %d\n"%(h, v, a)) - self.ser.flushInput() - """ - - self.ser.write("armxy %d %d %d\n"%(h, v, a)) - self.ser.flushInput() - - time.sleep(0.2) - - - - def do_arm_tt(self, args): - for i in xrange(2): - self.arm_py_goto(80, 80, 200) - self.arm_py_goto(80, 200, 200) - self.arm_py_goto(200, 200, 200) - self.arm_py_goto(200, 80, 200) - - def do_arm_harve(self, args): - angl1 = 1 - angl2 = 100 - my_time = 0.03 - self.arm_py_goto(130,130,angl1) - self.arm_py_goto(-150,60,angl1) - time.sleep(0.1) - - self.ser.write("pwm 1B -3000\n") - self.ser.flushInput() - time.sleep(0.2) - - self.arm_py_goto(-120,60,angl1) - time.sleep(2) - self.arm_py_goto(-120,60,angl2) - time.sleep(2) - self.arm_py_goto(-150,60,angl2) - self.ser.write("pwm 3C -3000\n") - self.ser.flushInput() - time.sleep(0.2) - self.arm_py_goto(-130,60,angl2) - self.arm_py_goto(0,160,angl2) - - #middle point - self.arm_py_goto(-40,200,angl2) - - h = -150 - d = 210 - - self.arm_py_goto(h,d,angl2) - time.sleep(.3) - self.ser.write("pwm 3C 3000\n") - time.sleep(0.1) - self.arm_py_goto(h+60,d,angl2) - time.sleep(0.1) - - self.arm_py_goto(h+60,d,angl1) - time.sleep(0.3) - self.arm_py_goto(h+40,d,angl1) - time.sleep(0.3) - self.arm_py_goto(h+30,d,angl1) - time.sleep(0.3) - self.ser.write("pwm 1B 3000\n") - time.sleep(0.1) - self.arm_py_goto(h+70,d,angl1) - - self.ser.write("pwm 1B 0\n") - self.ser.write("pwm 3C 0\n") - - self.arm_py_goto(130,130,angl2) - - - - def update_graph(self, val): - freq = self.sfreq.val - self.theta_max = freq*math.pi*2.0 - self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r)) - self.theta = self.theta[:len(self.r)] - - self.myplot.set_xdata(self.theta) - draw() - """ - def do_graph(self, args): - self.ser.write("pwm 1A 2000\n") - time.sleep(0.5) - print "sampling..." - self.ser.write("sample start\n") - while True: - l = self.ser.readline() - if "dump end" in l: - break - #time.sleep(2) - self.ser.write("pwm 1A 0\n") - l = self.ser.readline() - l = self.ser.readline() - - print "dumping..." - self.ser.write("sample dump\n") - vals = [] - while True: - l = self.ser.readline() - if l[0] in ['s', 'c', 'a']: - continue - if l[0] in ['e']: - break - tokens = [x for x in shlex.shlex(l)] - v = int(tokens[0]) - #v = min(v, 150) - vals.append(v) - vals.reverse() - print "total vals:", len(vals) - - pylab.subplot(111, polar = True) - self.r = vals - valinit = 5.38 - #theta_max = 4.8*2.3*pi - self.theta_max =valinit*pylab.pi - self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r)) - - self.myplot, = pylab.plot(self.theta, self.r) - - #slide bar - axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03]) - self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit) - self.sfreq.on_changed(self.update_graph) - - pylab.show() - """ - - - def do_dump(self, args): - - t = [x for x in shlex.shlex(args)] - - t.reverse() - do_img = False - - #send speed,debug=off - #self.ser.write("scan_params 500 0\n") - #send algo 1 wrkazone 1 cx 15 cy 15 - self.ser.write("scan_img 1 1 15 15\n") - - print t - while len(t): - x = t.pop() - if x == 'img': - do_img = True - - print "dumping..." - self.ser.write("sample dump 0 0 400 0\n") - - - - while True: - l = self.ser.readline() - - if "start dumping" in l: - tokens = [x for x in shlex.shlex(l)] - num_rows = int(tokens[-1]) - print "num row: ", num_rows - break - print l.strip() - #scan_stop = time.time() - #print "total time:", scan_stop-scan_start - - - vals = [] - while True: - l = self.ser.readline() - - if l[0] in ['s', 'c', 'a']: - continue - if l[0] in ['e']: - break - tokens = [x for x in shlex.shlex(l)] - v = int(tokens[0]) - #v = min(v, 150) - vals.append(v) - - - #vals.reverse() - print "total vals:", len(vals) - valinit = 5 - - #num_rows = int(600/valinit) - #num_cols = int(valinit) - num_rows_orig = num_rows - num_rows *= 1 - num_cols = len(vals)/num_rows - - data = [] - pt_num = 0 - my_min = None - my_max = None - - print "dim", num_rows, num_cols - print "sav img to pgm" - fimg = open("dump.pgm", "wb") - fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols)) - for i in xrange(num_cols): - data.append([]) - #data[-1].append(0.0) - - for j in xrange(num_rows): - if vals[pt_num]>0x10: - p = 0 - else: - p=vals[pt_num] * 0x20 - if (p>0xFF): - p = 0xFF - - fimg.write(chr(p)) - if my_min == None or my_min>p: - my_min = p - if p!=255 and (my_max == None or my_max= 205: - p = 0 - p/=1. - - - - data[-1].append(p) - pt_num+=1 - #data[-1].append(1.) - fimg.close() - print my_min, my_max - #print data - data = numpy.array(data) - - if do_img: - ax = pylab.subplot(111) - ax.imshow(data) - - - #pylab.subplot(111, polar = True) - self.r = vals - #theta_max = 4.8*2.3*pi - self.theta_max =valinit*pylab.pi - self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r)) - - """ - tmp = [] - for x in data: - tmp+=list(x) - self.myplot, = pylab.plot(tmp) - - - """ - if not do_img : - tmpx = [] - tmpy = [] - for x in data: - tmpy+=list(x) - tmpx+=range(len(x)) - self.myplot, = pylab.plot(tmpx, tmpy) - - - #slide bar - #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03]) - #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit) - #self.sfreq.on_changed(self.update_graph) - - pylab.show() - - - def do_scan_params(self, args): - t = [x for x in shlex.shlex(args)] - - if len(t)!=2: - return - t = [int(x) for x in t] - self.ser.write("scan_params %d %d\n"%tuple(t)) - - def do_graph(self, args): - t = [x for x in shlex.shlex(args)] - - t.reverse() - do_img = False - - #send speed,debug=off - #self.ser.write("scan_params 500 0\n") - #send algo 1 wrkazone 1 cx 15 cy 15 - self.ser.write("scan_img 1 1 15 15\n") - - print t - while len(t): - x = t.pop() - if x == 'img': - do_img = True - - - scan_start = time.time() - print "sampling..." - - self.ser.write("scan_do\n") - - flog = open('log.txt', 'w') - - while True: - l = self.ser.readline() - flog.write(l) - - if "dump end" in l: - break - flog.close() - - #time.sleep(2) - #self.ser.write("pwm 1A 0\n") - #l = self.ser.readline() - #l = self.ser.readline() - - print "dumping..." - self.ser.write("sample dump 0 0 400 0\n") - - - - while True: - l = self.ser.readline() - - if "start dumping" in l: - tokens = [x for x in shlex.shlex(l)] - num_rows = int(tokens[-1]) - print "num row: ", num_rows - break - print l.strip() - scan_stop = time.time() - print "total time:", scan_stop-scan_start - - - vals = [] - while True: - l = self.ser.readline() - - if l[0] in ['s', 'c', 'a']: - continue - if l[0] in ['e']: - break - tokens = [x for x in shlex.shlex(l)] - v = int(tokens[0]) - #v = min(v, 150) - vals.append(v) - - - #vals.reverse() - print "total vals:", len(vals) - valinit = 5 - - #num_rows = int(600/valinit) - #num_cols = int(valinit) - num_rows_orig = num_rows - num_rows *= 1 - num_cols = len(vals)/num_rows - - data = [] - pt_num = 0 - my_min = None - my_max = None - - print "dim", num_rows, num_cols - print "sav img to pgm" - fimg = open("dump.pgm", "wb") - fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols)) - for i in xrange(num_cols): - data.append([]) - #data[-1].append(0.0) - - for j in xrange(num_rows): - if vals[pt_num]>0x10: - p = 0 - else: - p=vals[pt_num] * 0x20 - if (p>0xFF): - p = 0xFF - - fimg.write(chr(p)) - if my_min == None or my_min>p: - my_min = p - if p!=255 and (my_max == None or my_max= 205: - p = 0 - p/=1. - - - - data[-1].append(p) - pt_num+=1 - #data[-1].append(1.) - fimg.close() - print my_min, my_max - #print data - data = numpy.array(data) - - if do_img: - ax = pylab.subplot(111) - ax.imshow(data) - - - #pylab.subplot(111, polar = True) - self.r = vals - #theta_max = 4.8*2.3*pi - self.theta_max =valinit*pylab.pi - self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r)) - - """ - tmp = [] - for x in data: - tmp+=list(x) - self.myplot, = pylab.plot(tmp) - - - """ - if not do_img : - tmpx = [] - tmpy = [] - for x in data: - tmpy+=list(x) - tmpx+=range(len(x)) - self.myplot, = pylab.plot(tmpx, tmpy) - - - #slide bar - #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03]) - #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit) - #self.sfreq.on_changed(self.update_graph) - - pylab.show() - - def bootloader(self, filename, boardnum): - self.ser.write("\n") + self.ser.write("") time.sleep(0.4) self.ser.write("bootloader\n") time.sleep(0.4) @@ -860,23 +330,39 @@ class Interp(cmd.Cmd): self.ser.flushInput() f = open(filename) buf = f.read() + f.close() + addr = 0 + + #old_buf = get_same_bin_file(self.ser) + old_buf = None + while addr < len(buf): + if addr > METADATA_ADDR and old_buf != None and \ + old_buf[addr:addr+SPM_PAGE_SIZE] == buf[addr:addr+SPM_PAGE_SIZE]: + sys.stdout.write("-") + sys.stdout.flush() + addr += SPM_PAGE_SIZE + continue time.sleep(0.1) if check_crc(self.ser, buf, addr, SPM_PAGE_SIZE) == 0: sys.stdout.write("*") sys.stdout.flush() - elif prog_page(self.ser, addr, + elif prog_page(self.ser, addr, buf[addr:addr+SPM_PAGE_SIZE]) != 0: return addr += SPM_PAGE_SIZE if check_crc(self.ser, buf, 0, len(buf)): print "crc failed" return + print + if prog_metadata(self.ser, METADATA_ADDR, buf) != 0: + print "metadata failed" + return print "Done." self.ser.write("x") self.do_raw("") - + def do_bootloader(self, args): self.bootloader(args, 0)