3 import os,sys,termios,atexit
5 from select import select
8 from matplotlib import pylab
16 warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__)
19 log = logging.getLogger("MicrobShell")
20 _handler = logging.StreamHandler()
21 _handler.setFormatter(logging.Formatter("%(levelname)s: %(message)s"))
22 log.addHandler(_handler)
25 MICROB_PATH=os.path.dirname(sys.argv[0])
29 def crc_ccitt_update (crc, data):
30 """crc argument is the previous value of 16 bits crc (the initial
31 value is 0xffff). 'data' is the 8 bits value added to crc. The
32 function returns the new crc value."""
38 ret = (data << 8) & 0xffff
39 ret |= ((crc >> 8) & 0xff)
40 ret ^= ((data >> 4) & 0xff)
41 ret ^= ((data << 3) & 0xffff)
49 crc = crc_ccitt_update(crc, ord(buf[i]))
52 return (crc << 16) + (sum & 0xffff)
54 def prog_page(ser, addr, buf):
55 """program a page from buf at addr"""
57 # switch in program mode
63 if not s.endswith("addr?\r\n"):
64 print "failed (don't match addr)"
66 ser.write("%x\n"%addr)
68 if not s.startswith("ok"):
72 # fill page with buf data
73 page = [ '\xff' ] * SPM_PAGE_SIZE
75 while i < SPM_PAGE_SIZE and i < len(buf):
81 while i < SPM_PAGE_SIZE:
90 avr_crc = int(ser.readline()[0:8], 16)
94 print "failed: bad crc %x %x"%(crc, avr_crc)
100 if not s.startswith("OK"):
105 def read32(ser, addr):
106 """read a 32 bits value at addr"""
108 # switch in program mode
114 if not s.endswith("addr?\r\n"):
115 print "failed (don't match addr)"
117 ser.write("%x\n"%addr)
121 def check_crc(ser, buf, offset, size):
122 """Process the crc of buf, ask for a crc of the flash, and check
123 that value is correct"""
133 if not s.endswith("addr?\r\n"):
134 print "failed <%s>"%s
136 ser.write("%x\n"%offset)
140 if not s.startswith("size?"):
143 ser.write("%x\n"%size)
146 crc = do_crc(buf[offset:offset+size])
147 avr_crc = int(ser.readline()[0:8], 16)
153 def __init__(self, ser, filein, fileout=None):
156 self.fin = open(filein, "a", 0)
158 self.fileout = fileout
159 self.fout = open(fileout, "a", 0)
161 self.fileout = filein
164 return self.ser.fileno()
165 def read(self, *args):
166 res = self.ser.read(*args)
193 def ang2_a_mirror(b):
194 x2 = X+l1*math.cos(b)
195 y2 = Y+l1*math.sin(b)
197 A = (l3**2+x2**2+y2**2-l2**2)/(2*l3)
199 DELTA = -(A**2-x2**2-y2**2)
200 B = +math.sqrt(DELTA)
207 a = math.atan2(s_a, c_a)
208 return x2, y2, c_a, s_a, a
211 def ang2_H_L(l_telemetre, c_a, s_a, a):
213 H = (l_telemetre - l_mirror - d)*math.sin(2*a)
214 L = l_mirror + d + H/math.tan(2*a)
218 for b in xrange(0, 360, 20):
219 b = b*2*math.pi / 360.
221 x2, y2, c_a, s_a, a = ang2_a_mirror(b)
230 print math.sqrt((x2-x1)**2+(y2-y1)**2)
232 H, L = ang2_H_L(400., c_a, s_a, a)
244 class Interp(cmd.Cmd):
246 def __init__(self, tty, baudrate=57600):
247 cmd.Cmd.__init__(self)
248 self.ser = serial.Serial(tty,baudrate=baudrate)
249 self.escape = "\x01" # C-a
250 self.quitraw = "\x02" # C-b
251 self.serial_logging = False
252 self.default_in_log_file = "/tmp/microb.in.log"
253 self.default_out_log_file = "/tmp/microb.out.log"
255 def do_quit(self, args):
258 def do_log(self, args):
259 """Activate serial logs.
260 log <filename> logs input and output to <filename>
261 log <filein> <fileout> logs input to <filein> and output to <fileout>
262 log logs to /tmp/microb.log or the last used file"""
264 if self.serial_logging:
265 log.error("Already logging to %s and %s" % (self.ser.filein,
268 self.serial_logging = True
269 files = [os.path.expanduser(x) for x in args.split()]
271 files = [self.default_in_log_file, self.default_out_log_file]
272 elif len(files) == 1:
273 self.default_in_log_file = files[0]
274 self.default_out_log_file = None
275 elif len(files) == 2:
276 self.default_in_log_file = files[0]
277 self.default_out_log_file = files[1]
279 print "Can't parse arguments"
281 self.ser = SerialLogger(self.ser, *files)
282 log.info("Starting serial logging to %s and %s" % (self.ser.filein,
286 def do_unlog(self, args):
287 if self.serial_logging:
288 log.info("Stopping serial logging to %s and %s" % (self.ser.filein,
290 self.ser = self.ser.ser
291 self.serial_logging = False
293 log.error("No log to stop")
296 def do_raw(self, args):
298 stdin = os.open("/dev/stdin",os.O_RDONLY)
299 stdout = os.open("/dev/stdout",os.O_WRONLY)
301 stdin_termios = termios.tcgetattr(stdin)
302 raw_termios = stdin_termios[:]
305 log.info("Switching to RAW mode")
308 raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT |
309 termios.PARMRK | termios.ISTRIP |
310 termios.INLCR | termios.IGNCR |
311 termios.ICRNL | termios.IXON)
313 raw_termios[1] &= ~termios.OPOST;
315 raw_termios[2] &= ~(termios.CSIZE | termios.PARENB);
316 raw_termios[2] |= termios.CS8;
318 raw_termios[3] &= ~(termios.ECHO | termios.ECHONL |
319 termios.ICANON | termios.ISIG |
322 termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios)
326 ins,outs,errs=select([stdin,self.ser],[],[])
333 self.ser.write(self.escape)
334 elif c == self.quitraw:
337 self.ser.write(self.escape)
345 os.write(stdout,self.ser.read())
347 termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios)
348 log.info("Back to normal mode")
351 def do_arm_x(self, args):
357 self.ser.write("armxy %d %d %d\n"%(my_h, -my_r, my_ang))
360 for i in xrange(-my_r, my_r, 25):
361 self.ser.write("armxy %d %d %d\n"%(my_h, i, my_ang))
362 self.ser.flushInput()
366 def do_arm_y(self, args):
370 self.ser.write("armxy %d %d %d\n"%(-my_r, my_x, my_ang))
373 for i in xrange(-my_r, my_r, 25):
374 self.ser.write("armxy %d %d %d\n"%(i, my_x, my_ang))
375 self.ser.flushInput()
379 def do_arm_circ(self, args):
383 for i in xrange(0, 360, 10):
384 x = l*math.cos(i*math.pi/180)
385 y = l*math.sin(i*math.pi/180)
388 self.ser.write("armxy %d %d 90\n"%(x+add_h, y+add_d))
389 self.ser.flushInput()
393 def do_arm_init(self, args):
398 self.ser.write("armxy %d %d\n"%(self.arm_h, self.arm_v))
400 def arm_py_goto(self, h, v, a):
402 dh, dv = h-self.arm_h, v-self.arm_v
403 d = math.sqrt(dh**2 + dv**2)
408 mov_todo = int(d/self.mov_max)
409 for i in xrange(1, mov_todo):
416 self.ser.write("armxy %d %d %d\n"%(new_h, new_v, a))
417 self.ser.flushInput()
423 self.ser.write("armxy %d %d %d\n"%(h, v, a))
424 self.ser.flushInput()
427 self.ser.write("armxy %d %d %d\n"%(h, v, a))
428 self.ser.flushInput()
434 def do_arm_tt(self, args):
436 self.arm_py_goto(80, 80, 200)
437 self.arm_py_goto(80, 200, 200)
438 self.arm_py_goto(200, 200, 200)
439 self.arm_py_goto(200, 80, 200)
441 def do_arm_harve(self, args):
445 self.arm_py_goto(130,130,angl1)
446 self.arm_py_goto(-150,60,angl1)
449 self.ser.write("pwm 1B -3000\n")
450 self.ser.flushInput()
453 self.arm_py_goto(-120,60,angl1)
455 self.arm_py_goto(-120,60,angl2)
457 self.arm_py_goto(-150,60,angl2)
458 self.ser.write("pwm 3C -3000\n")
459 self.ser.flushInput()
461 self.arm_py_goto(-130,60,angl2)
462 self.arm_py_goto(0,160,angl2)
465 self.arm_py_goto(-40,200,angl2)
470 self.arm_py_goto(h,d,angl2)
472 self.ser.write("pwm 3C 3000\n")
474 self.arm_py_goto(h+60,d,angl2)
477 self.arm_py_goto(h+60,d,angl1)
479 self.arm_py_goto(h+40,d,angl1)
481 self.arm_py_goto(h+30,d,angl1)
483 self.ser.write("pwm 1B 3000\n")
485 self.arm_py_goto(h+70,d,angl1)
487 self.ser.write("pwm 1B 0\n")
488 self.ser.write("pwm 3C 0\n")
490 self.arm_py_goto(130,130,angl2)
494 def update_graph(self, val):
495 freq = self.sfreq.val
496 self.theta_max = freq*math.pi*2.0
497 self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
498 self.theta = self.theta[:len(self.r)]
500 self.myplot.set_xdata(self.theta)
503 def do_graph(self, args):
504 self.ser.write("pwm 1A 2000\n")
507 self.ser.write("sample start\n")
509 l = self.ser.readline()
513 self.ser.write("pwm 1A 0\n")
514 l = self.ser.readline()
515 l = self.ser.readline()
518 self.ser.write("sample dump\n")
521 l = self.ser.readline()
522 if l[0] in ['s', 'c', 'a']:
526 tokens = [x for x in shlex.shlex(l)]
531 print "total vals:", len(vals)
533 pylab.subplot(111, polar = True)
536 #theta_max = 4.8*2.3*pi
537 self.theta_max =valinit*pylab.pi
538 self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
540 self.myplot, = pylab.plot(self.theta, self.r)
543 axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
544 self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
545 self.sfreq.on_changed(self.update_graph)
551 def do_dump(self, args):
553 t = [x for x in shlex.shlex(args)]
558 #send speed,debug=off
559 #self.ser.write("scan_params 500 0\n")
560 #send algo 1 wrkazone 1 cx 15 cy 15
561 self.ser.write("scan_img 1 1 15 15\n")
570 self.ser.write("sample dump 0 0 400 0\n")
575 l = self.ser.readline()
577 if "start dumping" in l:
578 tokens = [x for x in shlex.shlex(l)]
579 num_rows = int(tokens[-1])
580 print "num row: ", num_rows
583 #scan_stop = time.time()
584 #print "total time:", scan_stop-scan_start
589 l = self.ser.readline()
591 if l[0] in ['s', 'c', 'a']:
595 tokens = [x for x in shlex.shlex(l)]
602 print "total vals:", len(vals)
605 #num_rows = int(600/valinit)
606 #num_cols = int(valinit)
607 num_rows_orig = num_rows
609 num_cols = len(vals)/num_rows
616 print "dim", num_rows, num_cols
617 print "sav img to pgm"
618 fimg = open("dump.pgm", "wb")
619 fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols))
620 for i in xrange(num_cols):
622 #data[-1].append(0.0)
624 for j in xrange(num_rows):
625 if vals[pt_num]>0x10:
628 p=vals[pt_num] * 0x20
633 if my_min == None or my_min>p:
635 if p!=255 and (my_max == None or my_max<p):
649 data = numpy.array(data)
652 ax = pylab.subplot(111)
656 #pylab.subplot(111, polar = True)
658 #theta_max = 4.8*2.3*pi
659 self.theta_max =valinit*pylab.pi
660 self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
666 self.myplot, = pylab.plot(tmp)
676 self.myplot, = pylab.plot(tmpx, tmpy)
680 #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
681 #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
682 #self.sfreq.on_changed(self.update_graph)
687 def do_scan_params(self, args):
688 t = [x for x in shlex.shlex(args)]
692 t = [int(x) for x in t]
693 self.ser.write("scan_params %d %d\n"%tuple(t))
695 def do_graph(self, args):
696 t = [x for x in shlex.shlex(args)]
701 #send speed,debug=off
702 #self.ser.write("scan_params 500 0\n")
703 #send algo 1 wrkazone 1 cx 15 cy 15
704 self.ser.write("scan_img 1 1 15 15\n")
713 scan_start = time.time()
716 self.ser.write("scan_do\n")
718 flog = open('log.txt', 'w')
721 l = self.ser.readline()
729 #self.ser.write("pwm 1A 0\n")
730 #l = self.ser.readline()
731 #l = self.ser.readline()
735 self.ser.write("sample dump 0 0 400 0\n")
740 l = self.ser.readline()
742 if "start dumping" in l:
743 tokens = [x for x in shlex.shlex(l)]
744 num_rows = int(tokens[-1])
745 print "num row: ", num_rows
748 scan_stop = time.time()
749 print "total time:", scan_stop-scan_start
754 l = self.ser.readline()
756 if l[0] in ['s', 'c', 'a']:
760 tokens = [x for x in shlex.shlex(l)]
767 print "total vals:", len(vals)
770 #num_rows = int(600/valinit)
771 #num_cols = int(valinit)
772 num_rows_orig = num_rows
774 num_cols = len(vals)/num_rows
781 print "dim", num_rows, num_cols
782 print "sav img to pgm"
783 fimg = open("dump.pgm", "wb")
784 fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols))
785 for i in xrange(num_cols):
787 #data[-1].append(0.0)
789 for j in xrange(num_rows):
790 if vals[pt_num]>0x10:
793 p=vals[pt_num] * 0x20
798 if my_min == None or my_min>p:
800 if p!=255 and (my_max == None or my_max<p):
814 data = numpy.array(data)
817 ax = pylab.subplot(111)
821 #pylab.subplot(111, polar = True)
823 #theta_max = 4.8*2.3*pi
824 self.theta_max =valinit*pylab.pi
825 self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
831 self.myplot, = pylab.plot(tmp)
841 self.myplot, = pylab.plot(tmpx, tmpy)
845 #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
846 #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
847 #self.sfreq.on_changed(self.update_graph)
852 def bootloader(self, filename, boardnum):
855 self.ser.write("bootloader\n")
859 print "start programming"
860 self.ser.flushInput()
864 while addr < len(buf):
866 if check_crc(self.ser, buf, addr, SPM_PAGE_SIZE) == 0:
867 sys.stdout.write("*")
869 elif prog_page(self.ser, addr,
870 buf[addr:addr+SPM_PAGE_SIZE]) != 0:
872 addr += SPM_PAGE_SIZE
873 if check_crc(self.ser, buf, 0, len(buf)):
880 def do_bootloader(self, args):
881 self.bootloader(args, 0)
883 def do_mainboard(self, args):
884 filename = os.path.join(MICROB_PATH, "../mainboard/main.bin")
885 self.bootloader(filename, 1)
887 def do_mechboard(self, args):
888 filename = os.path.join(MICROB_PATH, "../mechboard/main.bin")
889 self.bootloader(filename, 2)
891 def do_sensorboard(self, args):
892 filename = os.path.join(MICROB_PATH, "../sensorboard/main.bin")
893 self.bootloader(filename, 3)
895 def do_toto(self, args):
898 self.ser.write("pwm s3(3C) 200\n")
900 self.ser.write("pwm s3(3C) 250\n")
902 if __name__ == "__main__":
904 import readline,atexit
908 histfile = os.path.join(os.environ["HOME"], ".microb_history")
909 atexit.register(readline.write_history_file, histfile)
911 readline.read_history_file(histfile)
915 device = "/dev/ttyS0"
916 if len(sys.argv) > 1:
918 interp = Interp(device)
922 except KeyboardInterrupt:
927 log.exception("%s" % l.splitlines()[-1])