3 import os,sys,termios,atexit
5 from select import select
8 from matplotlib import pylab
17 warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__)
20 log = logging.getLogger("MicrobShell")
21 _handler = logging.StreamHandler()
22 _handler.setFormatter(logging.Formatter("%(levelname)s: %(message)s"))
23 log.addHandler(_handler)
26 MICROB_PATH=os.path.dirname(sys.argv[0])
30 def crc_ccitt_update (crc, data):
31 """crc argument is the previous value of 16 bits crc (the initial
32 value is 0xffff). 'data' is the 8 bits value added to crc. The
33 function returns the new crc value."""
39 ret = (data << 8) & 0xffff
40 ret |= ((crc >> 8) & 0xff)
41 ret ^= ((data >> 4) & 0xff)
42 ret ^= ((data << 3) & 0xffff)
50 crc = crc_ccitt_update(crc, ord(buf[i]))
53 return (crc << 16) + (sum & 0xffff)
55 def prog_page(ser, addr, buf):
56 """program a page from buf at addr"""
58 # switch in program mode
64 if not s.endswith("addr?\r\n"):
65 print "failed (don't match addr)"
67 ser.write("%x\n"%addr)
69 if not s.startswith("ok"):
73 # fill page with buf data
74 page = [ '\xff' ] * SPM_PAGE_SIZE
76 while i < SPM_PAGE_SIZE and i < len(buf):
82 while i < SPM_PAGE_SIZE:
91 avr_crc = int(ser.readline()[0:8], 16)
95 print "failed: bad crc %x %x"%(crc, avr_crc)
101 if not s.startswith("OK"):
106 def read32(ser, addr):
107 """read a 32 bits value at addr"""
109 # switch in program mode
115 if not s.endswith("addr?\r\n"):
116 print "failed (don't match addr)"
118 ser.write("%x\n"%addr)
122 def check_crc(ser, buf, offset, size):
123 """Process the crc of buf, ask for a crc of the flash, and check
124 that value is correct"""
134 if not s.endswith("addr?\r\n"):
135 print "failed <%s>"%s
137 ser.write("%x\n"%offset)
141 if not s.startswith("size?"):
144 ser.write("%x\n"%size)
147 crc = do_crc(buf[offset:offset+size])
148 avr_crc = int(ser.readline()[0:8], 16)
154 def __init__(self, ser, filein, fileout=None):
157 self.fin = open(filein, "a", 0)
159 self.fileout = fileout
160 self.fout = open(fileout, "a", 0)
162 self.fileout = filein
165 return self.ser.fileno()
166 def read(self, *args):
167 res = self.ser.read(*args)
194 def ang2_a_mirror(b):
195 x2 = X+l1*math.cos(b)
196 y2 = Y+l1*math.sin(b)
198 A = (l3**2+x2**2+y2**2-l2**2)/(2*l3)
200 DELTA = -(A**2-x2**2-y2**2)
201 B = +math.sqrt(DELTA)
208 a = math.atan2(s_a, c_a)
209 return x2, y2, c_a, s_a, a
212 def ang2_H_L(l_telemetre, c_a, s_a, a):
214 H = (l_telemetre - l_mirror - d)*math.sin(2*a)
215 L = l_mirror + d + H/math.tan(2*a)
219 for b in xrange(0, 360, 20):
220 b = b*2*math.pi / 360.
222 x2, y2, c_a, s_a, a = ang2_a_mirror(b)
231 print math.sqrt((x2-x1)**2+(y2-y1)**2)
233 H, L = ang2_H_L(400., c_a, s_a, a)
245 class Interp(cmd.Cmd):
247 def __init__(self, tty, baudrate=57600):
248 cmd.Cmd.__init__(self)
249 self.ser = serial.Serial(tty,baudrate=baudrate)
250 self.escape = "\x01" # C-a
251 self.quitraw = "\x02" # C-b
252 self.serial_logging = False
253 self.default_in_log_file = "/tmp/microb.in.log"
254 self.default_out_log_file = "/tmp/microb.out.log"
256 def do_quit(self, args):
259 def do_log(self, args):
260 """Activate serial logs.
261 log <filename> logs input and output to <filename>
262 log <filein> <fileout> logs input to <filein> and output to <fileout>
263 log logs to /tmp/microb.log or the last used file"""
265 if self.serial_logging:
266 log.error("Already logging to %s and %s" % (self.ser.filein,
269 self.serial_logging = True
270 files = [os.path.expanduser(x) for x in args.split()]
272 files = [self.default_in_log_file, self.default_out_log_file]
273 elif len(files) == 1:
274 self.default_in_log_file = files[0]
275 self.default_out_log_file = None
276 elif len(files) == 2:
277 self.default_in_log_file = files[0]
278 self.default_out_log_file = files[1]
280 print "Can't parse arguments"
282 self.ser = SerialLogger(self.ser, *files)
283 log.info("Starting serial logging to %s and %s" % (self.ser.filein,
287 def do_unlog(self, args):
288 if self.serial_logging:
289 log.info("Stopping serial logging to %s and %s" % (self.ser.filein,
291 self.ser = self.ser.ser
292 self.serial_logging = False
294 log.error("No log to stop")
297 def do_raw(self, args):
299 stdin = os.open("/dev/stdin",os.O_RDONLY)
300 stdout = os.open("/dev/stdout",os.O_WRONLY)
302 stdin_termios = termios.tcgetattr(stdin)
303 raw_termios = stdin_termios[:]
306 log.info("Switching to RAW mode")
309 raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT |
310 termios.PARMRK | termios.ISTRIP |
311 termios.INLCR | termios.IGNCR |
312 termios.ICRNL | termios.IXON)
314 raw_termios[1] &= ~termios.OPOST;
316 raw_termios[2] &= ~(termios.CSIZE | termios.PARENB);
317 raw_termios[2] |= termios.CS8;
319 raw_termios[3] &= ~(termios.ECHO | termios.ECHONL |
320 termios.ICANON | termios.ISIG |
323 termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios)
327 ins,outs,errs=select([stdin,self.ser],[],[])
334 self.ser.write(self.escape)
335 elif c == self.quitraw:
338 self.ser.write(self.escape)
346 os.write(stdout,self.ser.read())
348 termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios)
349 log.info("Back to normal mode")
352 def do_arm_x(self, args):
358 self.ser.write("armxy %d %d %d\n"%(my_h, -my_r, my_ang))
361 for i in xrange(-my_r, my_r, 25):
362 self.ser.write("armxy %d %d %d\n"%(my_h, i, my_ang))
363 self.ser.flushInput()
367 def do_arm_y(self, args):
371 self.ser.write("armxy %d %d %d\n"%(-my_r, my_x, my_ang))
374 for i in xrange(-my_r, my_r, 25):
375 self.ser.write("armxy %d %d %d\n"%(i, my_x, my_ang))
376 self.ser.flushInput()
380 def do_arm_circ(self, args):
384 for i in xrange(0, 360, 10):
385 x = l*math.cos(i*math.pi/180)
386 y = l*math.sin(i*math.pi/180)
389 self.ser.write("armxy %d %d 90\n"%(x+add_h, y+add_d))
390 self.ser.flushInput()
394 def do_arm_init(self, args):
399 self.ser.write("armxy %d %d\n"%(self.arm_h, self.arm_v))
401 def arm_py_goto(self, h, v, a):
403 dh, dv = h-self.arm_h, v-self.arm_v
404 d = math.sqrt(dh**2 + dv**2)
409 mov_todo = int(d/self.mov_max)
410 for i in xrange(1, mov_todo):
417 self.ser.write("armxy %d %d %d\n"%(new_h, new_v, a))
418 self.ser.flushInput()
424 self.ser.write("armxy %d %d %d\n"%(h, v, a))
425 self.ser.flushInput()
428 self.ser.write("armxy %d %d %d\n"%(h, v, a))
429 self.ser.flushInput()
435 def do_arm_tt(self, args):
437 self.arm_py_goto(80, 80, 200)
438 self.arm_py_goto(80, 200, 200)
439 self.arm_py_goto(200, 200, 200)
440 self.arm_py_goto(200, 80, 200)
442 def do_arm_harve(self, args):
446 self.arm_py_goto(130,130,angl1)
447 self.arm_py_goto(-150,60,angl1)
450 self.ser.write("pwm 1B -3000\n")
451 self.ser.flushInput()
454 self.arm_py_goto(-120,60,angl1)
456 self.arm_py_goto(-120,60,angl2)
458 self.arm_py_goto(-150,60,angl2)
459 self.ser.write("pwm 3C -3000\n")
460 self.ser.flushInput()
462 self.arm_py_goto(-130,60,angl2)
463 self.arm_py_goto(0,160,angl2)
466 self.arm_py_goto(-40,200,angl2)
471 self.arm_py_goto(h,d,angl2)
473 self.ser.write("pwm 3C 3000\n")
475 self.arm_py_goto(h+60,d,angl2)
478 self.arm_py_goto(h+60,d,angl1)
480 self.arm_py_goto(h+40,d,angl1)
482 self.arm_py_goto(h+30,d,angl1)
484 self.ser.write("pwm 1B 3000\n")
486 self.arm_py_goto(h+70,d,angl1)
488 self.ser.write("pwm 1B 0\n")
489 self.ser.write("pwm 3C 0\n")
491 self.arm_py_goto(130,130,angl2)
495 def update_graph(self, val):
496 freq = self.sfreq.val
497 self.theta_max = freq*math.pi*2.0
498 self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
499 self.theta = self.theta[:len(self.r)]
501 self.myplot.set_xdata(self.theta)
504 def do_graph(self, args):
505 self.ser.write("pwm 1A 2000\n")
508 self.ser.write("sample start\n")
510 l = self.ser.readline()
514 self.ser.write("pwm 1A 0\n")
515 l = self.ser.readline()
516 l = self.ser.readline()
519 self.ser.write("sample dump\n")
522 l = self.ser.readline()
523 if l[0] in ['s', 'c', 'a']:
527 tokens = [x for x in shlex.shlex(l)]
532 print "total vals:", len(vals)
534 pylab.subplot(111, polar = True)
537 #theta_max = 4.8*2.3*pi
538 self.theta_max =valinit*pylab.pi
539 self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
541 self.myplot, = pylab.plot(self.theta, self.r)
544 axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
545 self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
546 self.sfreq.on_changed(self.update_graph)
552 def do_dump(self, args):
554 t = [x for x in shlex.shlex(args)]
559 #send speed,debug=off
560 #self.ser.write("scan_params 500 0\n")
561 #send algo 1 wrkazone 1 cx 15 cy 15
562 self.ser.write("scan_img 1 1 15 15\n")
571 self.ser.write("sample dump 0 0 400 0\n")
576 l = self.ser.readline()
578 if "start dumping" in l:
579 tokens = [x for x in shlex.shlex(l)]
580 num_rows = int(tokens[-1])
581 print "num row: ", num_rows
584 #scan_stop = time.time()
585 #print "total time:", scan_stop-scan_start
590 l = self.ser.readline()
592 if l[0] in ['s', 'c', 'a']:
596 tokens = [x for x in shlex.shlex(l)]
603 print "total vals:", len(vals)
606 #num_rows = int(600/valinit)
607 #num_cols = int(valinit)
608 num_rows_orig = num_rows
610 num_cols = len(vals)/num_rows
617 print "dim", num_rows, num_cols
618 print "sav img to pgm"
619 fimg = open("dump.pgm", "wb")
620 fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols))
621 for i in xrange(num_cols):
623 #data[-1].append(0.0)
625 for j in xrange(num_rows):
626 if vals[pt_num]>0x10:
629 p=vals[pt_num] * 0x20
634 if my_min == None or my_min>p:
636 if p!=255 and (my_max == None or my_max<p):
650 data = numpy.array(data)
653 ax = pylab.subplot(111)
657 #pylab.subplot(111, polar = True)
659 #theta_max = 4.8*2.3*pi
660 self.theta_max =valinit*pylab.pi
661 self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
667 self.myplot, = pylab.plot(tmp)
677 self.myplot, = pylab.plot(tmpx, tmpy)
681 #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
682 #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
683 #self.sfreq.on_changed(self.update_graph)
688 def do_scan_params(self, args):
689 t = [x for x in shlex.shlex(args)]
693 t = [int(x) for x in t]
694 self.ser.write("scan_params %d %d\n"%tuple(t))
696 def do_graph(self, args):
697 t = [x for x in shlex.shlex(args)]
702 #send speed,debug=off
703 #self.ser.write("scan_params 500 0\n")
704 #send algo 1 wrkazone 1 cx 15 cy 15
705 self.ser.write("scan_img 1 1 15 15\n")
714 scan_start = time.time()
717 self.ser.write("scan_do\n")
719 flog = open('log.txt', 'w')
722 l = self.ser.readline()
730 #self.ser.write("pwm 1A 0\n")
731 #l = self.ser.readline()
732 #l = self.ser.readline()
736 self.ser.write("sample dump 0 0 400 0\n")
741 l = self.ser.readline()
743 if "start dumping" in l:
744 tokens = [x for x in shlex.shlex(l)]
745 num_rows = int(tokens[-1])
746 print "num row: ", num_rows
749 scan_stop = time.time()
750 print "total time:", scan_stop-scan_start
755 l = self.ser.readline()
757 if l[0] in ['s', 'c', 'a']:
761 tokens = [x for x in shlex.shlex(l)]
768 print "total vals:", len(vals)
771 #num_rows = int(600/valinit)
772 #num_cols = int(valinit)
773 num_rows_orig = num_rows
775 num_cols = len(vals)/num_rows
782 print "dim", num_rows, num_cols
783 print "sav img to pgm"
784 fimg = open("dump.pgm", "wb")
785 fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols))
786 for i in xrange(num_cols):
788 #data[-1].append(0.0)
790 for j in xrange(num_rows):
791 if vals[pt_num]>0x10:
794 p=vals[pt_num] * 0x20
799 if my_min == None or my_min>p:
801 if p!=255 and (my_max == None or my_max<p):
815 data = numpy.array(data)
818 ax = pylab.subplot(111)
822 #pylab.subplot(111, polar = True)
824 #theta_max = 4.8*2.3*pi
825 self.theta_max =valinit*pylab.pi
826 self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
832 self.myplot, = pylab.plot(tmp)
842 self.myplot, = pylab.plot(tmpx, tmpy)
846 #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
847 #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
848 #self.sfreq.on_changed(self.update_graph)
853 def bootloader(self, filename, boardnum):
856 self.ser.write("bootloader\n")
860 print "start programming"
861 self.ser.flushInput()
865 while addr < len(buf):
867 if check_crc(self.ser, buf, addr, SPM_PAGE_SIZE) == 0:
868 sys.stdout.write("*")
870 elif prog_page(self.ser, addr,
871 buf[addr:addr+SPM_PAGE_SIZE]) != 0:
873 addr += SPM_PAGE_SIZE
874 if check_crc(self.ser, buf, 0, len(buf)):
881 def do_bootloader(self, args):
882 self.bootloader(args, 0)
884 def do_mainboard(self, args):
885 filename = os.path.join(MICROB_PATH, "../mainboard/main.bin")
886 self.bootloader(filename, 1)
888 def do_mechboard(self, args):
889 filename = os.path.join(MICROB_PATH, "../mechboard/main.bin")
890 self.bootloader(filename, 2)
892 def do_sensorboard(self, args):
893 filename = os.path.join(MICROB_PATH, "../sensorboard/main.bin")
894 self.bootloader(filename, 3)
896 def do_toto(self, args):
899 self.ser.write("pwm s3(3C) 200\n")
901 self.ser.write("pwm s3(3C) 250\n")
903 def do_circle(self, arg):
904 arg = re.sub(" *", " ", arg)
908 print "radius coef dspeed aspeed"
915 self.ser.write("event cs on\n")
918 self.ser.write("traj_speed distance %d\n"%dspeed)
920 self.ser.write("traj_speed angle %d\n"%aspeed)
923 self.ser.write("goto d_rel 300\n")
926 self.ser.write("position reset\n")
928 self.ser.write("circle_coef set %f\n"%c)
931 self.ser.write("goto circle_rel 700 460 %d 360\n"%r)
932 self.ser.flushInput()
936 if __name__ == "__main__":
938 import readline,atexit
942 histfile = os.path.join(os.environ["HOME"], ".microb_history")
943 atexit.register(readline.write_history_file, histfile)
945 readline.read_history_file(histfile)
949 device = "/dev/ttyS0"
950 if len(sys.argv) > 1:
952 interp = Interp(device)
956 except KeyboardInterrupt:
961 log.exception("%s" % l.splitlines()[-1])