from sets import Set import math, datetime import wx import ConfigParser, serial import threading, thread rate = 4800 id_SPACE = 11000 id_TEST = 11001 gverbose = True toolBtns = [ (True, id_TEST, "test", "Test", "Test Mode" ), (False, id_SPACE, 10), ] class Radar(): def __init__(self): # position set self.pos = 0 self.max_queue = 100 self.endpos = 0 self.queue = [] def add_obj(self, iangle, angle, poslist): self.queue.append((iangle,angle,poslist)) if len(self.queue) > self.max_queue: del self.queue[0:(self.max_queue/2)] print len(self.queue) def batchread(): global gr gr = Radar() comm = 1 ser = serial.Serial("COM%d" % comm, rate) # temperature c = 25 # speed of sound sp = 331.5 * math.sqrt(1 + ((c) / 275.15)) while (1): e = True angle1 = ord(ser.read(1)) angle2 = ord(ser.read(1)) angle = (angle1 << 8) + angle2 if angle < 400: samples = ord(ser.read(1)) if (samples <= 5): poslist = [] for i in range(0, samples): x1 = ord(ser.read(1)) x2 = ord(ser.read(1)) x = (x1 << 8) + x2 d = float(x) * sp / 20000.0 poslist.append(d) x = ord(ser.read(1)) if x == 0xff: x = ord(ser.read(1)) if x == 0xff: e = False if samples > 0: a = float(angle) / 400.0 * math.pi gr.add_obj(angle, a,poslist) print "Add", len(gr.queue), angle, poslist else: print "." if e: print "Error, Skip to next record" while (1): x = ord(ser.read(1)) if x == 0xff: x = ord(ser.read(1)) if x == 0xff: break if len(gr.queue) >= 500: break GuiStart() ############################################################################# ########################################################################## class radarWin(wx.Window): def __init__(self, parent, log, id, size): wx.Window.__init__(self,parent,id,size=size) self.log = log self.parent = parent #self.FitInside() self.SetBackgroundColour(wx.BLACK) self.size = self.GetClientSize() self.pen = wx.Pen(wx.Color(255,0,0), 2, wx.SOLID) self.penline = wx.Pen(wx.Color(0,255,0), 2) self.penradarline = wx.Pen(wx.Color(255,255,0), 2) self.pens= [] for i in range(0,255): self.pens.append(wx.Pen(wx.Color(i+1,i+1,i+1), 2, wx.SOLID)) self.posdict = {} self.maxdist = 160.0 self.ilastangle = -1 self.timer = wx.Timer(self) self.timer.Start(100) self.Bind(wx.EVT_SIZE, self.onSizeEvent) ## self.Bind(wx.EVT_LEFT_DOWN, self.onLeftDown) ## self.Bind(wx.EVT_LEFT_DCLICK, self.onLeftDoubleClickEvent) ## self.Bind(wx.EVT_RIGHT_DOWN, self.onRightClick) ## self.Bind(wx.EVT_MOTION, self.onMotionEvent) ## self.Bind(wx.EVT_LEFT_UP, self.onLeftUp) self.Bind(wx.EVT_PAINT, self.onPaintEvent) ## #self.Bind(wx.EVT_SCROLL_ENDSCROLL, self.onEndScrollEvent) ## self.Bind(wx.EVT_SCROLLWIN_THUMBRELEASE, self.onEndScrollEvent) ## self.Bind(wx.EVT_SCROLL_THUMBTRACK, self.onEndScrollEvent) ## self.Bind(wx.EVT_SCROLL_THUMBTRACK, self.onEndScrollEvent) #self.Bind(wx.EVT_IDLE, self.onIdleEvent) self.Bind(wx.EVT_TIMER, self.onIdleEvent) def onSizeEvent(self, event): self.size = self.GetClientSize() #self.Recalculate() event.Skip() def onIdleEvent(self, event): event.Skip() dc = wx.ClientDC(self) self.paint(dc) #self.Refresh() def onPaintEvent(self, event): dc = wx.PaintDC(self) self.paint(dc) def paint(self,dc): global gr global glock #dc = wx.PaintDC(self) self.PrepareDC(dc) dc.BeginDrawing() ## for i in range(0,255): ## dc.SetPen(self.pens[i]) ## dc.DrawCircle(self.size[0] * i / 255,100,2) midx = self.size[0] / 2 ratio = float(midx * 2) / self.maxdist bottom = self.size[1] dc.SetPen(self.pen) dc.SetBrush(wx.TRANSPARENT_BRUSH) dc.DrawCircle(midx,bottom,20) dc.SetPen(self.pens[254]) glock.acquire() #clean up the previous radar line if self.ilastangle >= 0: dc.SetPen(self.pens[0]) d = self.maxdist * ratio y = bottom - int(math.sin(self.lastangle) * d) x = midx + int(math.cos(self.lastangle) * d) dc.DrawLine(midx,bottom,x,y) # redraw the affected dots for iangle in range(self.ilastangle - 5, self.ilastangle + 5): if iangle in self.posdict: angle, oldplist = self.posdict[iangle] dc.SetPen(self.pens[254]) for i in oldplist: d = i * ratio y = bottom - int(math.sin(angle) * d) x = midx + int(math.cos(angle) * d) dc.DrawCircle(x,y,2) for info in gr.queue: iangle, angle, poslist = info if iangle in self.posdict: a, oldplist = self.posdict[iangle] dc.SetPen(self.pens[0]) for i in oldplist: d = i * ratio y = bottom - int(math.sin(angle) * d) x = midx + int(math.cos(angle) * d) dc.DrawCircle(x,y,2) dc.SetPen(self.pens[254]) for i in poslist: d = i * ratio y = bottom - int(math.sin(angle) * d) x = midx + int(math.cos(angle) * d) dc.DrawCircle(x,y,2) self.posdict[iangle] = (angle, poslist) if len(gr.queue) > 0: iangle, angle, poslist = gr.queue[-1] self.ilastangle = iangle self.lastangle = angle dc.SetPen(self.penradarline) d = self.maxdist * ratio y = bottom - int(math.sin(self.lastangle) * d) x = midx + int(math.cos(self.lastangle) * d) dc.DrawLine(midx,bottom,x,y) dc.SetPen(self.penline) d = self.maxdist/8 while d < self.maxdist: dc.DrawCircle(midx,bottom,int(d * ratio)) d = d + self.maxdist/8 angles = [ math.pi, math.pi*3/4.0, 0, math.pi/4.0, math.pi/2] d = self.maxdist * ratio for angle in angles: y = bottom - int(math.sin(angle) * d) x = midx + int(math.cos(angle) * d) dc.DrawLine(midx,bottom,x,y) glock.release() dc.EndDrawing() # croChartPanel contains the # radarWin optional toolBtns window class radarPanel(wx.Panel): def __init__(self, parent, id, log, style, sizehint, sidebtn=True, loadbtn=False, cancelbtn=True): wx.Panel.__init__(self, parent, id, style=style) self.log = log self.sidebtn = sidebtn self.radarWin = radarWin(self, log, -1,sizehint) if sidebtn or cancelbtn or loadbtn: toolSizer = wx.BoxSizer(wx.VERTICAL) else: toolSizer = None if sidebtn: self.toolBtns = [] for tp in toolBtns: ## if tp[0]: ## btn = stCommon.BitmapToggleButton(self, tp[1], tp[2], tp[4]) ## else: ## btn = stCommon.BitmapButton(self, tp[1], tp[2], tp[4]) if tp[1] == id_SPACE: toolSizer.AddSpacer((1,tp[2])) continue if tp[0]: btn = wx.ToggleButton(self, tp[1], tp[3]) btn.Bind(wx.EVT_TOGGLEBUTTON, self.onToolIconClick) else: btn = wx.Button(self, tp[1], tp[3]) btn.Bind(wx.EVT_BUTTON, self.onToolIconClick) btn.SetToolTipString(tp[4]) toolSizer.Add(btn,1,wx.LEFT | wx.RIGHT) self.toolBtns.append(btn) toolSizer.AddSpacer((1,10)) if loadbtn: btn = wx.Button(self, -1, "Load Default") #self.Bind(wx.EVT_BUTTON, self.OnLoadDefault, btn) toolSizer.Add(btn,1,wx.LEFT | wx.RIGHT) btn = wx.Button(self, -1, "Load File") #self.Bind(wx.EVT_BUTTON, self.OnLoad, btn) toolSizer.Add(btn,1,wx.LEFT | wx.RIGHT) if cancelbtn: btn = wx.Button(self, wx.ID_CANCEL, "Cancel") toolSizer.Add(btn,1,wx.LEFT | wx.RIGHT) if sidebtn: statusSizer = wx.BoxSizer(wx.VERTICAL) self.lblStatus1 = wx.StaticText(self) statusSizer.Add(self.lblStatus1,1,wx.LEFT) #self.toolBtns.append(self.lblStatus1) statusSizer.AddSpacer((1,10)) self.lblStatusMark = wx.StaticText(self) statusSizer.Add(self.lblStatusMark,1,wx.LEFT) #self.toolBtns.append(self.lblStatusMark) statusSizer.AddSpacer((1,20)) self.lblStatus2 = wx.StaticText(self) statusSizer.Add(self.lblStatus2,1,wx.LEFT) #self.toolBtns.append(self.lblStatus2) toolSizer.AddSpacer((1,10)) toolSizer.Add(statusSizer, 0) vSizer = wx.BoxSizer(wx.VERTICAL) vSizer.Add(self.radarWin, 1, wx.EXPAND) topSizer = wx.BoxSizer(wx.HORIZONTAL) topSizer.Add(vSizer, 0) if toolSizer != None: topSizer.Add(toolSizer, 0) self.SetAutoLayout(True) self.SetSizer(topSizer) def SetStatusMessage(self, text): self.lblStatus2.SetLabel(text) def onToolIconClick(self, event): global g_invert """ Respond to the user clicking on one of our tool icons. """ id = event.GetEventObject().GetId() event.Skip() # Dlg contains radarPanel class radarDlg(wx.Dialog): def __init__( self, parent, log, ID=-1, title="Radar", sizehint=(800, 600), size=(800, 600), pos=wx.DefaultPosition, style=wx.DEFAULT_DIALOG_STYLE ): wx.Dialog.__init__(self, parent, ID, title, pos, size, style) self.log = log self.topPanel = radarPanel(self, -1, self.log, sizehint=sizehint, style=wx.SIMPLE_BORDER) box = wx.BoxSizer(wx.VERTICAL) box.Add(self.topPanel, 1) ## btnCancel = wx.Button(self, wx.ID_CANCEL, "Cancel") ## box.Add(btnCancel, 0) ## btnCancel.Bind(wx.EVT_BUTTON, self.OnCancel) box.SetSizeHints(self) self.SetSizer(box) #parent.Fit() box.Fit(self) #self.Fit() ## def OnCancel(self, evt): ## self.EndModal(0) class RadarApp(wx.App): def OnInit(self, loadfile=None): if False: frame = radarFrame(None, self, sizehint=(800, 600)) frame.Centre() frame.Show(True) return True else: dlg = radarDlg(None, self, sizehint=(800, 600)) dlg.CenterOnParent() dlg.ShowModal() dlg.Destroy() return False def write(self, s): print s def GuiStart(): _app = RadarApp(0) _app.MainLoop() ################################################################################ class JobThreadBase(): def __init__(self, parent, log): self.parent = parent self.log = log self.keepGoing = self.running = False def Start(self): self.keepGoing = self.running = True thread.start_new_thread(self.Run, ()) def Stop(self): self.keepGoing = False def IsRunning(self): return self.running def IsKeepGoing(self): return self.keepGoing class SerJobThread(JobThreadBase): def __init__(self, log, port, rate): global gr global glock self.ser = serial.Serial("COM%d" % port, rate) #self.ser.setTimeout(timeout) self.port = port self.log = log self.lock = threading.Semaphore() gr = Radar() glock = threading.Semaphore() def Run(self): global glock global gverbose try: # temperature c = 25 # speed of sound sp = 331.5 * math.sqrt(1 + ((c) / 275.15)) while (self.keepGoing): e = True angle1 = ord(self.ser.read(1)) angle2 = ord(self.ser.read(1)) angle = (angle1 << 8) + angle2 if angle < 400: samples = ord(self.ser.read(1)) if (samples <= 5): poslist = [] for i in range(0, samples): x1 = ord(self.ser.read(1)) x2 = ord(self.ser.read(1)) x = (x1 << 8) + x2 d = float(x) * sp / 20000.0 poslist.append(d) x = ord(self.ser.read(1)) if x == 0xff: x = ord(self.ser.read(1)) if x == 0xff: e = False if samples >= 0: a = float(angle) / 400.0 * math.pi glock.acquire() gr.add_obj(angle,a,poslist) glock.release() if gverbose: print "Add", len(gr.queue), angle, poslist else: if gverbose: print "." if e: self.log.write("Error, Skip to next record\n") while (self.keepGoing): x = ord(self.ser.read(1)) if x == 0xff: x = ord(self.ser.read(1)) if x == 0xff: break except Exception, e: self.log.write("SerJobThread Exception %s" % (e)) self.running = False class mylog(): def write(self, s): print s if __name__ == '__main__': log = mylog() st1 = SerJobThread(log, 1, rate) st1.Start() GuiStart()