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