测试K210代码块-
- def ToBytes(num):
- num_H = (num >> 8) & 0xFF
- num_L = num & 0xFF
- return [num_H, num_L]
-
- def JoinBytes(Bytes):
- low_byte = Bytes[1]
- high_byte = Bytes[0]
- return (high_byte << 8) | low_byte
-
- def GetMinDistance(Radar):
- Radar.ExpressGetData(10)
- DataList = Radar.ReadRadarData()
- if DataList:
- min_radar_data = min(DataList, key=lambda radar_data: radar_data.distance)
- return min_radar_data.ToBytes()
- else:
- return [0xFF,0xFF,0xFF,0xFF]
-
- def SetAngleScope(Scope,Radar):
- MinAngle = JoinBytes(Scope[0:2])
- MaxAngle = JoinBytes(Scope[2:4])
- Maxdistance = JoinBytes(Scope[4:6])
- Radar.SetScope(int(MinAngle),(MaxAngle))
-
- def GetAngleScope(Radar):
- minangle,maxangle,maxdistance = Radar.GetScope()
- return ToBytes(minangle)+ToBytes(maxangle)+ToBytes(maxdistance)
-
-
- def FindfrequentBarCode(List):
- counter = {}
- for s in List:
- if s not in counter:
- counter[s] = 0
- counter[s] += 1
- Frequentstr = max(counter, key=counter.get)
- return int(Frequentstr)
-
- def IdentifyBarCode(Batch,TimeOut):
- timeout = TimeOut
- DataList = []
- while(len(DataList) < Batch and timeout > 0):
- img = sensor.snapshot()
- barcodes = img.find_barcodes()
- if barcodes:
- for barcode in barcodes:
- barcodeData = barcode.payload()
- (x, y, w, h) = barcode.rect()
- #img.draw_rectangle(barcode.rect())
- #img.draw_string(x, y, barcodeData, color=(0,255,0), scale=1)
- #lcd.display(img)
- DataList = DataList + [barcodeData]
- else:
- timeout -= 1
- if len(DataList) > Batch//2:
- return FindfrequentBarCode(DataList)
- return -1
-
-
- def BarcodeTask(Uart,Radar):
- Data = [0x01]
- #IdentifyResult = IdentifyBarCode(10,5)
- #if IdentifyResult == -1:
- Data = Data + [0x00]
- Result = GetMinDistance(Radar)
- Data = Data + Result
- Scope = GetAngleScope(Radar)
- Data = Data + Scope
- Uart.SendData(Data)
-
- def CalculateCenter(rect):
- x, y, width, height = rect
- center_x = x + width // 2
- center_y = y + height // 2
- return [center_x,center_y]
-
- def AverageCoordinate(List):
- x_List, y_List = zip(*List)
- x_avg = sum(x_List) / len(x_List)
- y_avg = sum(y_List) / len(y_List)
- return [x_avg,y_avg]
-
- #Batch:识别Batch帧的数据平均成一帧 TimeOut:尝试TimeOut次未达到收集不齐Batch帧率,收集多少算多少
- def SearchPark(YoloV2Task, Batch, TimeOut):
- ResultList = []
- timeout = TimeOut
- while(len(ResultList) < Batch and timeout > 0):
- img = sensor.snapshot()
- SearchResults = kpu.run_yolo2(YoloV2Task,img)
- if SearchResults:
- timeout = TimeOut
- for Result in SearchResults:
- img.draw_rectangle(Result.rect())
- Coordinate = CalculateCenter(Result.rect())
- ResultList.append(Coordinate)
- #lcd.display(img)
- timeout -= 1
- if ResultList:
- return ResultList
- else:
- return [0xff,0xff]
-
- def SearchParkTask(MyUart):
- Data = [0x02]#发送时带模式号
- ParkingList = SearchPark(ParkingTask, 1, 10)
- if ParkingList[0] != 0xff:
- ParkImgCoordinate = AverageCoordinate(ParkingList)
- else:
- ParkImgCoordinate = ParkingList
- #ParkCoordinate = [(int)(ParkImgCoordinate[0]-112)//1,(int)(112-ParkImgCoordinate[1])//1] # 计算以最中心点为原点的坐标
- ParkCoordinate = [(int)(ParkImgCoordinate[0]),(int)(ParkImgCoordinate[1])]
- Data = Data + ParkCoordinate
- MyUart.SendData(Data)
-
- # Batch:识别多少次 TimeOut:识别不到重试次数
- def JudgeColorBlobs(Batch, TimeOut):
- Color = (37, 95, -28, -6, -39, 29)
- ColorRoi = (54,54,112,112)
- timeout = TimeOut
- SuccessCount = 0
- while(SuccessCount < Batch and timeout > 0):
- Picture = sensor.snapshot()
- ColorBlobs = Picture.find_blobs([Color],roi=ColorRoi, x_stride = 5,y_stride = 5,pixels_threshold=50,merge=True)
- if ColorBlobs:
- SuccessCount += 1
- else:
- timeout -= 1
- if SuccessCount == Batch:
- return 1
- return 0
-
- def JudgeColorTask(MyUart):
- Data = [0x03]#发送时带模式号
- Result = JudgeColorBlobs(5,10)
- Data = Data + [Result]
- MyUart.SendData(Data)
-
- def GetCoordinateTask(MyUart):
- Data = [0x04]#发送时带模式号
- Result = CD.FindXY()
- Data = Data + [(int)(Result[0]),(int)(Result[1])]
- MyUart.SendData(Data)
-
-
- OldRevDatas = [0x00]
- Count = 0
- while True:
- RevDatas = UartToStc32.ReadData()# 串口读到的模式标志为是持久性的 stc发送一次改变模式 直到下一次收到数据后
- if RevDatas:
- RevCmd = RevDatas[0]
- if RevCmd != 0x00 and RevCmd != 0x11 and RevCmd != 0x01 and RevCmd != 0x02 and RevCmd != 0x03 and RevCmd != 0x04 and RevCmd != 0x0F and RevCmd != 0xF0:
- UartToStc32.RcvData = OldRevDatas
- if RevCmd == 0x01:
- BarcodeTask(UartToStc32,MyRadar)
- elif RevCmd == 0x11:
- if len(RevDatas)>=7:
- SetAngleScope(RevDatas[1:7],MyRadar)
- UartToStc32.RcvData = [0x01]
- elif RevCmd == 0x02:
- MyRadar.PauseScan()
- SearchParkTask(UartToStc32)
- elif RevCmd == 0x03:
- MyRadar.PauseScan()
- JudgeColorTask(UartToStc32)
- elif RevCmd == 0x04:
- GetCoordinateTask(UartToStc32)
- elif RevCmd == 0x0F:
- MyData.TakePhoto()
- UartToStc32.SendData([0x0F]+ToBytes(MyData.datanum-MyData.nownum+1))
- UartToStc32.RcvData = OldRevDatas
- elif RevCmd == 0x0E:
- MapImg = MyRadar.CreatMapping()
- MyDataOrigin.TakePhoto(img = MapImg)
- DataList = MyRadar.ReadRadarData()
- MyDbscan.DataList.clear()
- MyDbscan.DataList = DataList
- ClusterList, NoiseList = MyDbscan.GetCluster()
- MapImg = MyRadar.CreatDbscanMapping(ClusterList, NoiseList)
- MyData.TakePhoto(img = MapImg)
- MapImg = MyRadar.CreatAveragRadarDataMapping(ClusterList, MyDbscan)
- MyDataAfter.TakePhoto(img = MapImg)
- UartToStc32.SendData([0x0E]+ToBytes(MyData.datanum-MyData.nownum+1))
- UartToStc32.RcvData = OldRevDatas
- elif RevCmd == 0x00:
- Count += 1
- MyRadar.PauseScan()
- if Count >= 50:
- Count = 0
- UartToStc32.SendData([0x00,0x00,0x00])# 停机
- OldRevDatas = RevDatas[:]
- gc.collect()
复制代码
|