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 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