|  |  |  | 
|---|
|  |  |  | for (FuncSta funcSta : funcStaList) { | 
|---|
|  |  |  | boolean beIdle = true; | 
|---|
|  |  |  | Agv agv = agvService.findByPosition(funcSta.getCode()); | 
|---|
|  |  |  | if (null == agv) { | 
|---|
|  |  |  | beIdle = false; | 
|---|
|  |  |  | } else { | 
|---|
|  |  |  | AgvModel agvModel = agvModelService.getById(agv.getAgvModel()); | 
|---|
|  |  |  | AgvDetail agvDetail = agvDetailService.selectByAgvId(agv.getId()); | 
|---|
|  |  |  |  | 
|---|
|  |  |  | 
|---|
|  |  |  | default: | 
|---|
|  |  |  | break; | 
|---|
|  |  |  | } | 
|---|
|  |  |  | } | 
|---|
|  |  |  |  | 
|---|
|  |  |  | if (beIdle) { | 
|---|
|  |  |  | funcSta.setState(FuncStaStateType.IDLE.toString()); | 
|---|
|  |  |  | funcSta.setUpdateTime(new Date()); | 
|---|
|  |  |  | 
|---|
|  |  |  | break; | 
|---|
|  |  |  | case TO_CHARGE: | 
|---|
|  |  |  | // 检验方向 | 
|---|
|  |  |  | FuncSta chargeFuncSta = funcStaService.query(agvId, lastCode.getId(), FuncStaType.CHARGE.toString()); | 
|---|
|  |  |  | FuncSta chargeFuncSta = funcStaService.query(lastCode.getId(), FuncStaType.CHARGE.toString()); | 
|---|
|  |  |  | Double chargeDirection = Double.parseDouble(chargeFuncSta.getAngle()); | 
|---|
|  |  |  | if (!lastDirection.equals(chargeDirection)) { | 
|---|
|  |  |  | actionList.add(new Action( | 
|---|
|  |  |  | 
|---|
|  |  |  |  | 
|---|
|  |  |  | double deltaX = x1 - x0; | 
|---|
|  |  |  | double deltaY = y1 - y0; | 
|---|
|  |  |  | double angle = Math.atan2(deltaX, deltaY); | 
|---|
|  |  |  | double angle = -Math.atan2(deltaX, deltaY); | 
|---|
|  |  |  | int angleOffsetVal = configService.getVal("mapAngleOffsetVal", Integer.class); | 
|---|
|  |  |  | angle = Math.toDegrees(angle) + angleOffsetVal; | 
|---|
|  |  |  | angle = (angle + 360) % 360; // 将角度转换为正值 | 
|---|
|  |  |  | 
|---|
|  |  |  | public Code getRandomCode(AgvDetail agvDetail) { | 
|---|
|  |  |  | Code startCode = codeService.getById(agvDetail.getRecentCode()); | 
|---|
|  |  |  | List<String> notInCodeList = new ArrayList<>(); | 
|---|
|  |  |  | notInCodeList.add("00000061"); | 
|---|
|  |  |  | notInCodeList.add("00000151"); | 
|---|
|  |  |  | List<Code> list = codeService.list(new LambdaQueryWrapper<Code>().notIn(Code::getData, notInCodeList)); | 
|---|
|  |  |  | Collections.shuffle(list); | 
|---|
|  |  |  | for (Code endCode : list) { | 
|---|
|  |  |  | 
|---|
|  |  |  |  | 
|---|
|  |  |  | FuncSta getByCodeAndType(Long codeId, String type); | 
|---|
|  |  |  |  | 
|---|
|  |  |  | FuncSta query(Long agvId, Long codeId, String type); | 
|---|
|  |  |  | FuncSta query(Long codeId, String type); | 
|---|
|  |  |  |  | 
|---|
|  |  |  | List<FuncSta> findInIdleStatus(FuncStaType type, Long agvId); | 
|---|
|  |  |  |  | 
|---|
|  |  |  | 
|---|
|  |  |  | if (null == agvModel) { | 
|---|
|  |  |  | agvModel = agvModelService.getById(agv.getAgvModel()); | 
|---|
|  |  |  | } | 
|---|
|  |  |  | return agvDetail.getVol() < Math.min(agv.getChargeLine(), agvModel.getLowBattery()); | 
|---|
|  |  |  | if (null == agv.getChargeLine()) { | 
|---|
|  |  |  | return agvDetail.getVol() < agvModel.getLowBattery(); | 
|---|
|  |  |  | } else { | 
|---|
|  |  |  | return agvDetail.getVol() < Math.max(agv.getChargeLine(), agvModel.getLowBattery()); | 
|---|
|  |  |  | } | 
|---|
|  |  |  | } | 
|---|
|  |  |  |  | 
|---|
|  |  |  | } | 
|---|
|  |  |  | 
|---|
|  |  |  | if (null == agvDetail) { | 
|---|
|  |  |  | continue; | 
|---|
|  |  |  | } | 
|---|
|  |  |  | if (null == agvDetail.getRecentCode()) { | 
|---|
|  |  |  | continue; | 
|---|
|  |  |  | } | 
|---|
|  |  |  | if (agvDetail.getRecentCode().equals(codeId)) { | 
|---|
|  |  |  | return agv; | 
|---|
|  |  |  | } | 
|---|
|  |  |  | 
|---|
|  |  |  | } | 
|---|
|  |  |  |  | 
|---|
|  |  |  | @Override | 
|---|
|  |  |  | public FuncSta query(Long agvId, Long codeId, String type) { | 
|---|
|  |  |  | public FuncSta query(Long codeId, String type) { | 
|---|
|  |  |  | List<FuncSta> list = this.list(new LambdaQueryWrapper<FuncSta>() | 
|---|
|  |  |  | .eq(FuncSta::getAgvId, agvId) | 
|---|
|  |  |  | .eq(FuncSta::getCode, codeId) | 
|---|
|  |  |  | .eq(FuncSta::getType, type) | 
|---|
|  |  |  | ); | 
|---|