cobboard: prepare state machine
[aversive.git] / projects / microb2010 / microb_cmd / microbcmd.py
1 #! /usr/bin/env python
2
3 import os,sys,termios,atexit
4 import serial
5 from select import select
6 import cmd
7 #import pylab
8 from  matplotlib import pylab 
9 from math import *
10
11 import numpy 
12 import shlex
13 import time
14 import math
15 import warnings
16 warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__)
17
18 import logging
19 log = logging.getLogger("MicrobShell")
20 _handler = logging.StreamHandler()
21 _handler.setFormatter(logging.Formatter("%(levelname)s: %(message)s"))
22 log.addHandler(_handler)
23 log.setLevel(1)
24
25 MICROB_PATH=os.path.dirname(sys.argv[0])
26
27 SPM_PAGE_SIZE = 256
28
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."""
33
34     data ^= (crc & 0xff)
35     data ^= (data << 4)
36     data &= 0xff
37     
38     ret = (data << 8) & 0xffff
39     ret |= ((crc >> 8) & 0xff)
40     ret ^= ((data >> 4) & 0xff)
41     ret ^= ((data << 3) & 0xffff)
42     return ret
43
44 def do_crc(buf):
45     i = 0
46     crc = 0xffff
47     sum = 0
48     while i < len(buf):
49         crc = crc_ccitt_update(crc, ord(buf[i]))
50         sum +=  ord(buf[i])
51         i += 1
52     return (crc << 16) + (sum & 0xffff)
53
54 def prog_page(ser, addr, buf):
55     """program a page from buf at addr"""
56     
57     # switch in program mode 
58     ser.flushInput()
59     ser.write('p')
60
61     # send address
62     s = ser.readline()
63     if not s.endswith("addr?\r\n"):
64         print "failed (don't match addr)"
65         return -1
66     ser.write("%x\n"%addr)
67     s = ser.readline()
68     if not s.startswith("ok"):
69         print "failed"
70         return -1
71
72     # fill page with buf data
73     page = [ '\xff' ] * SPM_PAGE_SIZE
74     i = 0
75     while i < SPM_PAGE_SIZE and i < len(buf):
76         page[i] = buf[i]
77         i += 1
78     
79     # send data
80     i = 0
81     while i < SPM_PAGE_SIZE:
82         c = page[i]
83         ser.write(c)
84         i += 1
85
86     sys.stdout.write(".")
87     sys.stdout.flush()
88
89     # compare crc
90     avr_crc = int(ser.readline()[0:8], 16)
91
92     crc = do_crc(page)
93     if crc != avr_crc:
94         print "failed: bad crc %x %x"%(crc, avr_crc)
95         ser.write('n')
96         return -1
97
98     ser.write('y')
99     s = ser.readline()
100     if not s.startswith("OK"):
101         print "failed"
102         return -1
103     return 0
104
105 def read32(ser, addr):
106     """read a 32 bits value at addr"""
107     
108     # switch in program mode 
109     ser.flushInput()
110     ser.write('d')
111
112     # send address
113     s = ser.readline()
114     if not s.endswith("addr?\r\n"):
115         print "failed (don't match addr)"
116         return -1
117     ser.write("%x\n"%addr)
118     s = ser.readline()
119     return int(s)
120
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"""
124     if size <= 0:
125         return 0
126
127     # go in CRC mode
128     ser.flushInput()
129     ser.write('c')
130
131     # send addr
132     s = ser.readline()
133     if not s.endswith("addr?\r\n"):
134         print "failed <%s>"%s
135         return -1
136     ser.write("%x\n"%offset)
137
138     # send size
139     s = ser.readline()
140     if not s.startswith("size?"):
141         print "failed"
142         return -1
143     ser.write("%x\n"%size)
144
145     # compare CRC
146     crc = do_crc(buf[offset:offset+size])
147     avr_crc = int(ser.readline()[0:8], 16)
148     if crc != avr_crc:
149         return -1
150     return 0
151
152 class SerialLogger:
153     def __init__(self, ser, filein, fileout=None):
154         self.ser = ser
155         self.filein = filein
156         self.fin = open(filein, "a", 0)
157         if fileout:
158             self.fileout = fileout
159             self.fout = open(fileout, "a", 0)
160         else:
161             self.fileout = filein
162             self.fout = self.fin
163     def fileno(self):
164         return self.ser.fileno()
165     def read(self, *args):
166         res = self.ser.read(*args)
167         self.fin.write(res)
168         return res
169     def write(self, s):
170         self.fout.write(s)
171         self.ser.write(s)
172
173
174
175
176
177 """
178 fig = figure()
179
180 ax = subplot(111)
181
182
183 X = 45.
184 Y = -10.
185 l1 = 9.
186 l2 = 21.13
187 l3 = 47.14
188
189 l_mirror = 249.
190 h_mirror = 13.
191
192
193 def ang2_a_mirror(b):
194     x2 = X+l1*math.cos(b)
195     y2 = Y+l1*math.sin(b)
196
197     A = (l3**2+x2**2+y2**2-l2**2)/(2*l3)
198
199     DELTA = -(A**2-x2**2-y2**2)
200     B = +math.sqrt(DELTA)
201
202     D = x2**2+y2**2
203
204     c_a = (x2*A+y2*B)/D
205     s_a = -(x2*B-y2*A)/D
206
207     a = math.atan2(s_a, c_a)
208     return x2, y2, c_a, s_a, a
209
210
211 def ang2_H_L(l_telemetre, c_a, s_a, a):
212     d = h_mirror*c_a/s_a
213     H = (l_telemetre - l_mirror - d)*math.sin(2*a)
214     L = l_mirror + d + H/math.tan(2*a)
215     return H, L
216
217 all_p = []
218 for b in xrange(0, 360, 20):
219     b = b*2*math.pi / 360.
220
221     x2, y2, c_a, s_a, a = ang2_a_mirror(b)
222     x1 = l3*c_a
223     y1 = l3*s_a
224     
225     px = [0, x1, x2, X]
226     py = [0, y1, y2, Y]
227
228     all_p+=[px, py]
229
230     print math.sqrt((x2-x1)**2+(y2-y1)**2)
231
232     H, L = ang2_H_L(400., c_a, s_a, a)
233     print H, L
234     
235 ax.plot(*all_p)
236
237 show()
238
239 """
240          
241
242
243
244 class Interp(cmd.Cmd):
245     prompt = "Microb> "
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"
254
255     def do_quit(self, args):
256         return True
257
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"""
263
264         if self.serial_logging:
265             log.error("Already logging to %s and %s" % (self.ser.filein, 
266                                                         self.ser.fileout))
267         else:
268             self.serial_logging = True
269             files = [os.path.expanduser(x) for x in args.split()]
270             if len(files) == 0:
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]
278             else:
279                 print "Can't parse arguments"
280
281             self.ser = SerialLogger(self.ser, *files)
282             log.info("Starting serial logging to %s and %s" % (self.ser.filein, 
283                                                                self.ser.fileout))
284
285
286     def do_unlog(self, args):
287         if self.serial_logging:
288             log.info("Stopping serial logging to %s and %s" % (self.ser.filein, 
289                                                                self.ser.fileout))
290             self.ser = self.ser.ser
291             self.serial_logging = False
292         else:
293             log.error("No log to stop")
294         
295
296     def do_raw(self, args):
297         "Switch to RAW mode"
298         stdin = os.open("/dev/stdin",os.O_RDONLY)
299         stdout = os.open("/dev/stdout",os.O_WRONLY)
300
301         stdin_termios = termios.tcgetattr(stdin)
302         raw_termios = stdin_termios[:]
303         
304         try:
305             log.info("Switching to RAW mode")
306
307             # iflag
308             raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT | 
309                                 termios.PARMRK | termios.ISTRIP | 
310                                 termios.INLCR | termios.IGNCR | 
311                                 termios.ICRNL | termios.IXON)
312             # oflag
313             raw_termios[1] &= ~termios.OPOST;
314             # cflag
315             raw_termios[2] &= ~(termios.CSIZE | termios.PARENB);
316             raw_termios[2] |= termios.CS8;
317             # lflag
318             raw_termios[3] &= ~(termios.ECHO | termios.ECHONL | 
319                                 termios.ICANON | termios.ISIG | 
320                                 termios.IEXTEN);
321
322             termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios)
323
324             mode = "normal"
325             while True:
326                 ins,outs,errs=select([stdin,self.ser],[],[])
327                 for x in ins:
328                     if x == stdin:
329                         c = os.read(stdin,1)
330                         if mode  == "escape":
331                             mode =="normal"
332                             if c == self.escape:
333                                 self.ser.write(self.escape)
334                             elif c == self.quitraw:
335                                 return
336                             else:
337                                 self.ser.write(self.escape)
338                                 self.ser.write(c)
339                         else:
340                             if c == self.escape:
341                                 mode = "escape"
342                             else:
343                                 self.ser.write(c)
344                     elif x == self.ser:
345                         os.write(stdout,self.ser.read())
346         finally:
347             termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios)
348             log.info("Back to normal mode")
349             
350
351     def do_arm_x(self, args):
352         fsdf
353         my_h = 100
354         my_r = 220
355         my_ang = 90
356
357         self.ser.write("armxy %d %d %d\n"%(my_h, -my_r, my_ang))
358         time.sleep(1)
359         
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()
363             
364             time.sleep(0.03)
365
366     def do_arm_y(self, args):
367         my_x = 80
368         my_r = 145
369         my_ang = 0
370         self.ser.write("armxy %d %d %d\n"%(-my_r, my_x, my_ang))
371         time.sleep(1)
372         
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()
376             
377             time.sleep(0.03)
378
379     def do_arm_circ(self, args):
380         add_h = 120
381         add_d = 120
382         l = 70
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)
386             
387             
388             self.ser.write("armxy %d %d 90\n"%(x+add_h, y+add_d))
389             self.ser.flushInput()
390             
391             time.sleep(0.05)
392
393     def do_arm_init(self, args):
394         self.arm_h = 130
395         self.arm_v = 130
396         self.mov_max = 20
397         
398         self.ser.write("armxy %d %d\n"%(self.arm_h, self.arm_v))        
399
400     def arm_py_goto(self, h, v, a):
401         """
402         dh, dv = h-self.arm_h, v-self.arm_v
403         d = math.sqrt(dh**2 + dv**2)
404
405         old_h = self.arm_h
406         old_v = self.arm_v
407         
408         mov_todo = int(d/self.mov_max)
409         for i in xrange(1, mov_todo):
410             p_h = dh*i/mov_todo
411             p_v = dv*i/mov_todo
412
413             new_h = old_h+p_h
414             new_v = old_v+p_v
415             
416             self.ser.write("armxy %d %d %d\n"%(new_h, new_v, a))
417             self.ser.flushInput()
418             self.arm_h = new_h
419             self.arm_v = new_v
420
421             time.sleep(0.04)
422             
423         self.ser.write("armxy %d %d %d\n"%(h, v, a))
424         self.ser.flushInput()
425         """
426
427         self.ser.write("armxy %d %d %d\n"%(h, v, a))
428         self.ser.flushInput()
429
430         time.sleep(0.2)
431     
432             
433                 
434     def do_arm_tt(self, args):
435         for i in xrange(2):
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)
440
441     def do_arm_harve(self, args):
442         angl1 = 1
443         angl2 = 100
444         my_time = 0.03
445         self.arm_py_goto(130,130,angl1)
446         self.arm_py_goto(-150,60,angl1)
447         time.sleep(0.1)
448         
449         self.ser.write("pwm 1B -3000\n")
450         self.ser.flushInput()
451         time.sleep(0.2)
452
453         self.arm_py_goto(-120,60,angl1)
454         time.sleep(2)
455         self.arm_py_goto(-120,60,angl2)
456         time.sleep(2)
457         self.arm_py_goto(-150,60,angl2)
458         self.ser.write("pwm 3C -3000\n")
459         self.ser.flushInput()
460         time.sleep(0.2)
461         self.arm_py_goto(-130,60,angl2)
462         self.arm_py_goto(0,160,angl2)
463
464         #middle point
465         self.arm_py_goto(-40,200,angl2)
466
467         h = -150
468         d = 210
469         
470         self.arm_py_goto(h,d,angl2)
471         time.sleep(.3)
472         self.ser.write("pwm 3C 3000\n")
473         time.sleep(0.1)
474         self.arm_py_goto(h+60,d,angl2)
475         time.sleep(0.1)
476
477         self.arm_py_goto(h+60,d,angl1)
478         time.sleep(0.3)
479         self.arm_py_goto(h+40,d,angl1)
480         time.sleep(0.3)
481         self.arm_py_goto(h+30,d,angl1)
482         time.sleep(0.3)
483         self.ser.write("pwm 1B 3000\n")
484         time.sleep(0.1)
485         self.arm_py_goto(h+70,d,angl1)
486
487         self.ser.write("pwm 1B 0\n")
488         self.ser.write("pwm 3C 0\n")
489
490         self.arm_py_goto(130,130,angl2)
491
492         
493         
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)]
499
500         self.myplot.set_xdata(self.theta)
501         draw()
502     """    
503     def do_graph(self, args):
504         self.ser.write("pwm 1A 2000\n")
505         time.sleep(0.5)
506         print "sampling..."
507         self.ser.write("sample start\n")
508         while True:
509             l = self.ser.readline()
510             if "dump end" in l:
511                 break
512         #time.sleep(2)
513         self.ser.write("pwm 1A 0\n")
514         l = self.ser.readline()
515         l = self.ser.readline()
516
517         print "dumping..."
518         self.ser.write("sample dump\n")
519         vals = []
520         while True:
521             l = self.ser.readline()
522             if l[0] in ['s', 'c', 'a']:
523                 continue
524             if l[0] in ['e']:
525                 break
526             tokens = [x for x in shlex.shlex(l)]
527             v = int(tokens[0])
528             #v = min(v, 150)
529             vals.append(v)
530         vals.reverse()
531         print "total vals:", len(vals)
532
533         pylab.subplot(111, polar = True)
534         self.r = vals
535         valinit = 5.38
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))
539         
540         self.myplot, = pylab.plot(self.theta, self.r)
541
542         #slide bar
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)
546
547         pylab.show()
548     """
549
550
551     def do_dump(self, args):
552
553         t = [x for x in shlex.shlex(args)]
554         
555         t.reverse()
556         do_img = False
557
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")
562         
563         print t
564         while len(t):
565             x = t.pop()
566             if x == 'img':
567                 do_img = True
568
569         print "dumping..."
570         self.ser.write("sample dump 0 0 400 0\n")
571
572
573
574         while True:
575             l = self.ser.readline()
576
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
581                 break
582             print l.strip()
583         #scan_stop = time.time()
584         #print "total time:", scan_stop-scan_start
585
586
587         vals = []
588         while True:
589             l = self.ser.readline()
590
591             if l[0] in ['s', 'c', 'a']:
592                 continue
593             if l[0] in ['e']:
594                 break
595             tokens = [x for x in shlex.shlex(l)]
596             v = int(tokens[0])
597             #v = min(v, 150)
598             vals.append(v)
599
600
601         #vals.reverse()
602         print "total vals:", len(vals)
603         valinit = 5
604
605         #num_rows = int(600/valinit)
606         #num_cols = int(valinit)
607         num_rows_orig = num_rows
608         num_rows *= 1
609         num_cols = len(vals)/num_rows
610
611         data = []
612         pt_num = 0
613         my_min = None
614         my_max = None
615
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):
621             data.append([])
622             #data[-1].append(0.0)
623             
624             for j in xrange(num_rows):
625                 if vals[pt_num]>0x10:
626                     p = 0
627                 else:
628                     p=vals[pt_num] * 0x20
629                 if (p>0xFF):
630                     p = 0xFF
631                    
632                 fimg.write(chr(p))
633                 if my_min == None or my_min>p:
634                     my_min = p
635                 if  p!=255 and (my_max == None or my_max<p):
636                     my_max = p
637                 if p >= 205:
638                     p = 0
639                 p/=1.
640
641
642                 
643                 data[-1].append(p)
644                 pt_num+=1
645             #data[-1].append(1.)
646         fimg.close()
647         print my_min, my_max
648         #print data
649         data = numpy.array(data)
650
651         if do_img:
652             ax = pylab.subplot(111)
653             ax.imshow(data)
654         
655         
656         #pylab.subplot(111, polar = True)
657         self.r = vals
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))
661
662         """
663         tmp = []
664         for x in data:
665             tmp+=list(x)
666         self.myplot, = pylab.plot(tmp)
667         
668
669         """
670         if not do_img :
671             tmpx = []
672             tmpy = []
673             for x in data:
674                 tmpy+=list(x)
675                 tmpx+=range(len(x))
676             self.myplot, = pylab.plot(tmpx, tmpy)
677         
678
679         #slide bar
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)
683
684         pylab.show()
685
686
687     def do_scan_params(self, args):
688         t = [x for x in shlex.shlex(args)]
689         
690         if len(t)!=2:
691             return
692         t = [int(x) for x in t]
693         self.ser.write("scan_params %d %d\n"%tuple(t))
694
695     def do_graph(self, args):
696         t = [x for x in shlex.shlex(args)]
697         
698         t.reverse()
699         do_img = False
700
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")
705         
706         print t
707         while len(t):
708             x = t.pop()
709             if x == 'img':
710                 do_img = True
711                 
712             
713         scan_start = time.time()
714         print "sampling..."
715
716         self.ser.write("scan_do\n")
717
718         flog = open('log.txt', 'w')
719
720         while True:
721             l = self.ser.readline()
722             flog.write(l)
723
724             if "dump end" in l:
725                 break
726         flog.close()
727
728         #time.sleep(2)
729         #self.ser.write("pwm 1A 0\n")
730         #l = self.ser.readline()
731         #l = self.ser.readline()
732
733
734         print "dumping..."
735         self.ser.write("sample dump 0 0 400 0\n")
736
737
738
739         while True:
740             l = self.ser.readline()
741
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
746                 break
747             print l.strip()
748         scan_stop = time.time()
749         print "total time:", scan_stop-scan_start
750
751
752         vals = []
753         while True:
754             l = self.ser.readline()
755
756             if l[0] in ['s', 'c', 'a']:
757                 continue
758             if l[0] in ['e']:
759                 break
760             tokens = [x for x in shlex.shlex(l)]
761             v = int(tokens[0])
762             #v = min(v, 150)
763             vals.append(v)
764
765
766         #vals.reverse()
767         print "total vals:", len(vals)
768         valinit = 5
769
770         #num_rows = int(600/valinit)
771         #num_cols = int(valinit)
772         num_rows_orig = num_rows
773         num_rows *= 1
774         num_cols = len(vals)/num_rows
775
776         data = []
777         pt_num = 0
778         my_min = None
779         my_max = None
780
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):
786             data.append([])
787             #data[-1].append(0.0)
788             
789             for j in xrange(num_rows):
790                 if vals[pt_num]>0x10:
791                     p = 0
792                 else:
793                     p=vals[pt_num] * 0x20
794                 if (p>0xFF):
795                     p = 0xFF
796                    
797                 fimg.write(chr(p))
798                 if my_min == None or my_min>p:
799                     my_min = p
800                 if  p!=255 and (my_max == None or my_max<p):
801                     my_max = p
802                 if p >= 205:
803                     p = 0
804                 p/=1.
805
806
807                 
808                 data[-1].append(p)
809                 pt_num+=1
810             #data[-1].append(1.)
811         fimg.close()
812         print my_min, my_max
813         #print data
814         data = numpy.array(data)
815
816         if do_img:
817             ax = pylab.subplot(111)
818             ax.imshow(data)
819         
820         
821         #pylab.subplot(111, polar = True)
822         self.r = vals
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))
826
827         """
828         tmp = []
829         for x in data:
830             tmp+=list(x)
831         self.myplot, = pylab.plot(tmp)
832         
833
834         """
835         if not do_img :
836             tmpx = []
837             tmpy = []
838             for x in data:
839                 tmpy+=list(x)
840                 tmpx+=range(len(x))
841             self.myplot, = pylab.plot(tmpx, tmpy)
842         
843
844         #slide bar
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)
848
849         pylab.show()
850
851         
852     def bootloader(self, filename, boardnum):
853         self.ser.write("\n")
854         time.sleep(0.4)
855         self.ser.write("bootloader\n")
856         time.sleep(0.4)
857         self.ser.write("\n")
858
859         print "start programming"
860         self.ser.flushInput()
861         f = open(filename)
862         buf = f.read()
863         addr = 0
864         while addr < len(buf):
865             #time.sleep(0.1)
866             if check_crc(self.ser, buf, addr, SPM_PAGE_SIZE) == 0:
867                 sys.stdout.write("*")
868                 sys.stdout.flush()
869             elif prog_page(self.ser, addr, 
870                          buf[addr:addr+SPM_PAGE_SIZE]) != 0:
871                 return
872             addr += SPM_PAGE_SIZE
873         if check_crc(self.ser, buf, 0, len(buf)):
874             print "crc failed"
875             return
876         print "Done."
877         self.ser.write("x")
878         self.do_raw("")
879         
880     def do_bootloader(self, args):
881         self.bootloader(args, 0)
882
883     def do_mainboard(self, args):
884         filename = os.path.join(MICROB_PATH, "../mainboard/main.bin")
885         self.bootloader(filename, 1)
886
887     def do_mechboard(self, args):
888         filename = os.path.join(MICROB_PATH, "../mechboard/main.bin")
889         self.bootloader(filename, 2)
890
891     def do_sensorboard(self, args):
892         filename = os.path.join(MICROB_PATH, "../sensorboard/main.bin")
893         self.bootloader(filename, 3)
894
895     def do_toto(self, args):
896         for i in range(10):
897             time.sleep(1)
898             self.ser.write("pwm s3(3C) 200\n")
899             time.sleep(1)
900             self.ser.write("pwm s3(3C) 250\n")
901
902 if __name__ == "__main__":
903     try:
904         import readline,atexit
905     except ImportError:
906         pass
907     else:
908         histfile = os.path.join(os.environ["HOME"], ".microb_history")
909         atexit.register(readline.write_history_file, histfile)
910         try:
911             readline.read_history_file(histfile)
912         except IOError:
913             pass
914
915     device = "/dev/ttyS0"
916     if len(sys.argv) > 1:
917         device = sys.argv[1]
918     interp = Interp(device)
919     while 1:
920         try:
921             interp.cmdloop()
922         except KeyboardInterrupt:
923             print
924         except Exception,e:
925             l = str(e).strip()
926             if l:
927                 log.exception("%s" % l.splitlines()[-1])
928             continue
929         break