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