- self.ser.write("armxy %d %d %d\n"%(my_h, -my_r, my_ang))
- time.sleep(1)
-
- for i in xrange(-my_r, my_r, 25):
- self.ser.write("armxy %d %d %d\n"%(my_h, i, my_ang))
- self.ser.flushInput()
-
- time.sleep(0.03)
-
- def do_arm_y(self, args):
- my_x = 80
- my_r = 145
- my_ang = 0
- self.ser.write("armxy %d %d %d\n"%(-my_r, my_x, my_ang))
- time.sleep(1)
-
- for i in xrange(-my_r, my_r, 25):
- self.ser.write("armxy %d %d %d\n"%(i, my_x, my_ang))
- self.ser.flushInput()
-
- time.sleep(0.03)
-
- def do_arm_circ(self, args):
- add_h = 120
- add_d = 120
- l = 70
- for i in xrange(0, 360, 10):
- x = l*math.cos(i*math.pi/180)
- y = l*math.sin(i*math.pi/180)
-
-
- self.ser.write("armxy %d %d 90\n"%(x+add_h, y+add_d))
- self.ser.flushInput()
-
- time.sleep(0.05)
-
- def do_arm_init(self, args):
- self.arm_h = 130
- self.arm_v = 130
- self.mov_max = 20
-
- self.ser.write("armxy %d %d\n"%(self.arm_h, self.arm_v))
-
- def arm_py_goto(self, h, v, a):
- """
- dh, dv = h-self.arm_h, v-self.arm_v
- d = math.sqrt(dh**2 + dv**2)
-
- old_h = self.arm_h
- old_v = self.arm_v
-
- mov_todo = int(d/self.mov_max)
- for i in xrange(1, mov_todo):
- p_h = dh*i/mov_todo
- p_v = dv*i/mov_todo
-
- new_h = old_h+p_h
- new_v = old_v+p_v
-
- self.ser.write("armxy %d %d %d\n"%(new_h, new_v, a))
- self.ser.flushInput()
- self.arm_h = new_h
- self.arm_v = new_v
-
- time.sleep(0.04)
-
- self.ser.write("armxy %d %d %d\n"%(h, v, a))
- self.ser.flushInput()
- """
-
- self.ser.write("armxy %d %d %d\n"%(h, v, a))
- self.ser.flushInput()
-
- time.sleep(0.2)
-
-
-
- def do_arm_tt(self, args):
- for i in xrange(2):
- self.arm_py_goto(80, 80, 200)
- self.arm_py_goto(80, 200, 200)
- self.arm_py_goto(200, 200, 200)
- self.arm_py_goto(200, 80, 200)
-
- def do_arm_harve(self, args):
- angl1 = 1
- angl2 = 100
- my_time = 0.03
- self.arm_py_goto(130,130,angl1)
- self.arm_py_goto(-150,60,angl1)
- time.sleep(0.1)
-
- self.ser.write("pwm 1B -3000\n")
- self.ser.flushInput()
- time.sleep(0.2)
-
- self.arm_py_goto(-120,60,angl1)
- time.sleep(2)
- self.arm_py_goto(-120,60,angl2)
- time.sleep(2)
- self.arm_py_goto(-150,60,angl2)
- self.ser.write("pwm 3C -3000\n")
- self.ser.flushInput()
- time.sleep(0.2)
- self.arm_py_goto(-130,60,angl2)
- self.arm_py_goto(0,160,angl2)
-
- #middle point
- self.arm_py_goto(-40,200,angl2)
-
- h = -150
- d = 210
-
- self.arm_py_goto(h,d,angl2)
- time.sleep(.3)
- self.ser.write("pwm 3C 3000\n")
- time.sleep(0.1)
- self.arm_py_goto(h+60,d,angl2)
- time.sleep(0.1)
-
- self.arm_py_goto(h+60,d,angl1)
- time.sleep(0.3)
- self.arm_py_goto(h+40,d,angl1)
- time.sleep(0.3)
- self.arm_py_goto(h+30,d,angl1)
- time.sleep(0.3)
- self.ser.write("pwm 1B 3000\n")
- time.sleep(0.1)
- self.arm_py_goto(h+70,d,angl1)
-
- self.ser.write("pwm 1B 0\n")
- self.ser.write("pwm 3C 0\n")
-
- self.arm_py_goto(130,130,angl2)
-
-
-
- def update_graph(self, val):
- freq = self.sfreq.val
- self.theta_max = freq*math.pi*2.0
- self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
- self.theta = self.theta[:len(self.r)]
-
- self.myplot.set_xdata(self.theta)
- draw()
- """
- def do_graph(self, args):
- self.ser.write("pwm 1A 2000\n")
- time.sleep(0.5)
- print "sampling..."
- self.ser.write("sample start\n")
- while True:
- l = self.ser.readline()
- if "dump end" in l:
- break
- #time.sleep(2)
- self.ser.write("pwm 1A 0\n")
- l = self.ser.readline()
- l = self.ser.readline()
-
- print "dumping..."
- self.ser.write("sample dump\n")
- vals = []
- while True:
- l = self.ser.readline()
- if l[0] in ['s', 'c', 'a']:
- continue
- if l[0] in ['e']:
- break
- tokens = [x for x in shlex.shlex(l)]
- v = int(tokens[0])
- #v = min(v, 150)
- vals.append(v)
- vals.reverse()
- print "total vals:", len(vals)
-
- pylab.subplot(111, polar = True)
- self.r = vals
- valinit = 5.38
- #theta_max = 4.8*2.3*pi
- self.theta_max =valinit*pylab.pi
- self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
-
- self.myplot, = pylab.plot(self.theta, self.r)
-
- #slide bar
- axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
- self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
- self.sfreq.on_changed(self.update_graph)
-
- pylab.show()
- """
-
-
- def do_dump(self, args):
-
- t = [x for x in shlex.shlex(args)]
-
- t.reverse()
- do_img = False
-
- #send speed,debug=off
- #self.ser.write("scan_params 500 0\n")
- #send algo 1 wrkazone 1 cx 15 cy 15
- self.ser.write("scan_img 1 1 15 15\n")
-
- print t
- while len(t):
- x = t.pop()
- if x == 'img':
- do_img = True
-
- print "dumping..."
- self.ser.write("sample dump 0 0 400 0\n")
-
-
-
- while True:
- l = self.ser.readline()
-
- if "start dumping" in l:
- tokens = [x for x in shlex.shlex(l)]
- num_rows = int(tokens[-1])
- print "num row: ", num_rows
- break
- print l.strip()
- #scan_stop = time.time()
- #print "total time:", scan_stop-scan_start
-
-
- vals = []
- while True:
- l = self.ser.readline()
-
- if l[0] in ['s', 'c', 'a']:
- continue
- if l[0] in ['e']:
- break
- tokens = [x for x in shlex.shlex(l)]
- v = int(tokens[0])
- #v = min(v, 150)
- vals.append(v)
-
-
- #vals.reverse()
- print "total vals:", len(vals)
- valinit = 5
-
- #num_rows = int(600/valinit)
- #num_cols = int(valinit)
- num_rows_orig = num_rows
- num_rows *= 1
- num_cols = len(vals)/num_rows
-
- data = []
- pt_num = 0
- my_min = None
- my_max = None
-
- print "dim", num_rows, num_cols
- print "sav img to pgm"
- fimg = open("dump.pgm", "wb")
- fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols))
- for i in xrange(num_cols):
- data.append([])
- #data[-1].append(0.0)
-
- for j in xrange(num_rows):
- if vals[pt_num]>0x10:
- p = 0
- else:
- p=vals[pt_num] * 0x20
- if (p>0xFF):
- p = 0xFF
-
- fimg.write(chr(p))
- if my_min == None or my_min>p:
- my_min = p
- if p!=255 and (my_max == None or my_max<p):
- my_max = p
- if p >= 205:
- p = 0
- p/=1.
-
-
-
- data[-1].append(p)
- pt_num+=1
- #data[-1].append(1.)
- fimg.close()
- print my_min, my_max
- #print data
- data = numpy.array(data)
-
- if do_img:
- ax = pylab.subplot(111)
- ax.imshow(data)
-
-
- #pylab.subplot(111, polar = True)
- self.r = vals
- #theta_max = 4.8*2.3*pi
- self.theta_max =valinit*pylab.pi
- self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
-
- """
- tmp = []
- for x in data:
- tmp+=list(x)
- self.myplot, = pylab.plot(tmp)
-
-
- """
- if not do_img :
- tmpx = []
- tmpy = []
- for x in data:
- tmpy+=list(x)
- tmpx+=range(len(x))
- self.myplot, = pylab.plot(tmpx, tmpy)
-
-
- #slide bar
- #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
- #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
- #self.sfreq.on_changed(self.update_graph)
-
- pylab.show()
-
-
- def do_scan_params(self, args):
- t = [x for x in shlex.shlex(args)]
-
- if len(t)!=2:
- return
- t = [int(x) for x in t]
- self.ser.write("scan_params %d %d\n"%tuple(t))
-
- def do_graph(self, args):
- t = [x for x in shlex.shlex(args)]
-
- t.reverse()
- do_img = False
-
- #send speed,debug=off
- #self.ser.write("scan_params 500 0\n")
- #send algo 1 wrkazone 1 cx 15 cy 15
- self.ser.write("scan_img 1 1 15 15\n")
-
- print t
- while len(t):
- x = t.pop()
- if x == 'img':
- do_img = True
-
-
- scan_start = time.time()
- print "sampling..."
-
- self.ser.write("scan_do\n")
-
- flog = open('log.txt', 'w')
-
- while True:
- l = self.ser.readline()
- flog.write(l)
-
- if "dump end" in l:
- break
- flog.close()
-
- #time.sleep(2)
- #self.ser.write("pwm 1A 0\n")
- #l = self.ser.readline()
- #l = self.ser.readline()
-
-
- print "dumping..."
- self.ser.write("sample dump 0 0 400 0\n")
-
-
-
- while True:
- l = self.ser.readline()
-
- if "start dumping" in l:
- tokens = [x for x in shlex.shlex(l)]
- num_rows = int(tokens[-1])
- print "num row: ", num_rows
- break
- print l.strip()
- scan_stop = time.time()
- print "total time:", scan_stop-scan_start
-
-
- vals = []
- while True:
- l = self.ser.readline()
-
- if l[0] in ['s', 'c', 'a']:
- continue
- if l[0] in ['e']:
- break
- tokens = [x for x in shlex.shlex(l)]
- v = int(tokens[0])
- #v = min(v, 150)
- vals.append(v)
-
-
- #vals.reverse()
- print "total vals:", len(vals)
- valinit = 5
-
- #num_rows = int(600/valinit)
- #num_cols = int(valinit)
- num_rows_orig = num_rows
- num_rows *= 1
- num_cols = len(vals)/num_rows
-
- data = []
- pt_num = 0
- my_min = None
- my_max = None
-
- print "dim", num_rows, num_cols
- print "sav img to pgm"
- fimg = open("dump.pgm", "wb")
- fimg.write("P5\n#toto\n%d %d\n255\n"%(num_rows, num_cols))
- for i in xrange(num_cols):
- data.append([])
- #data[-1].append(0.0)
-
- for j in xrange(num_rows):
- if vals[pt_num]>0x10:
- p = 0
- else:
- p=vals[pt_num] * 0x20
- if (p>0xFF):
- p = 0xFF
-
- fimg.write(chr(p))
- if my_min == None or my_min>p:
- my_min = p
- if p!=255 and (my_max == None or my_max<p):
- my_max = p
- if p >= 205:
- p = 0
- p/=1.
-
-
-
- data[-1].append(p)
- pt_num+=1
- #data[-1].append(1.)
- fimg.close()
- print my_min, my_max
- #print data
- data = numpy.array(data)
-
- if do_img:
- ax = pylab.subplot(111)
- ax.imshow(data)
-
-
- #pylab.subplot(111, polar = True)
- self.r = vals
- #theta_max = 4.8*2.3*pi
- self.theta_max =valinit*pylab.pi
- self.theta = pylab.arange(0.0, self.theta_max, self.theta_max/len(self.r))
-
- """
- tmp = []
- for x in data:
- tmp+=list(x)
- self.myplot, = pylab.plot(tmp)
-
-
- """
- if not do_img :
- tmpx = []
- tmpy = []
- for x in data:
- tmpy+=list(x)
- tmpx+=range(len(x))
- self.myplot, = pylab.plot(tmpx, tmpy)
-
-
- #slide bar
- #axfreq = pylab.axes([0.25, 0.1, 0.65, 0.03])
- #self.sfreq = pylab.Slider(axfreq, "Freq", 1, 20, valinit = valinit)
- #self.sfreq.on_changed(self.update_graph)
-
- pylab.show()
-
-