| | |
|---|
| | | |
|---|
| | | // 巷道逻辑 |
|---|
| | | if (!laneBuilder.isInitialized()) { |
|---|
| | | throw new BusinessException("lanes are not initialized"); |
|---|
| | | throw new CoolException("lanes are not initialized"); |
|---|
| | | } |
|---|
| | | LaneDto lastLaneDto = laneBuilder.search(lastCode.getData()); |
|---|
| | | LaneDto nextLaneDto = laneBuilder.search(nextCode.getData()); |
|---|
| | |
|---|
| | | || !Objects.equals(lastLaneDto.getHashCode(), nextLaneDto.getHashCode()) |
|---|
| | | ); |
|---|
| | | |
|---|
| | | |
|---|
| | | |
|---|
| | | // 如果下一个方向正好是作业方向的相反方向，则重置下一个方向为作业方向，标记 reverse = true |
|---|
| | | boolean reverse = false; |
|---|
| | | if (nextDirection.equals(oppWorkDir)) { |
|---|
| | | nextDirection = workDirection; |
|---|
| | | reverse = true; |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 判断当前节点是否可以旋转 |
|---|
| | | if (!lastCode.getCornerBool()) { |
|---|
| | | // 如果是作业方向，但是小车在巷道内方向错误，则停止 |
|---|
| | | if (reverse && !lastDirection.equals(nextDirection)) { |
|---|
| | | throw new CoolException(agvNo + "号小车方向错误，请推至转弯点手动调整"); |
|---|
| | | // 巷道强制转弯，优先级 > workDirection |
|---|
| | | if (firstEnteringLane) { |
|---|
| | | // final double oppLaneDir = (laneDir + 180) % 360; |
|---|
| | | if (!lastDirection.equals(laneDir)) { |
|---|
| | | if (!lastCode.getCornerBool()) { |
|---|
| | | throw new CoolException(agvNo + "号小车进入巷道需调整方向为 " + laneDir + "°，请推至转弯点手动调整"); |
|---|
| | | } |
|---|
| | | // turn |
|---|
| | | actionList.add(new Action( |
|---|
| | | null, // 编号s |
|---|
| | | task.getBusId(), // 总线 |
|---|
| | | task.getId(), // 任务 |
|---|
| | | null, // 动作号 |
|---|
| | | null, // 优先级 |
|---|
| | | ActionTypeType.TurnCorner.desc, // 名称 |
|---|
| | | (double) mapService.spinDirection(lastCode).val, // 属性值 |
|---|
| | | lastCode.getData(), // 地面码 |
|---|
| | | String.valueOf(laneDir), // 动作参数 |
|---|
| | | ActionTypeType.TurnCorner.val(), // 动作类型 |
|---|
| | | actionPrepareSts, // 动作进度 |
|---|
| | | agvId, // AGV |
|---|
| | | now // 工作时间 |
|---|
| | | )); |
|---|
| | | lastDirection = laneDir; |
|---|
| | | } |
|---|
| | | // 如果不是作业方向（另一组相反方向），判断是否相反方向，如果反方向则倒退行走 |
|---|
| | | if (nextDirection.equals(oppLastDir)) { |
|---|
| | | // 倒退时，因为agv方向没变，所以下一个方向还是agv方向，故nextDirection = lastDirection; |
|---|
| | | nextDirection = lastDirection; |
|---|
| | | } else { |
|---|
| | | |
|---|
| | | // 如果下一个方向正好是作业方向的相反方向，则重置下一个方向为作业方向，标记 reverse = true |
|---|
| | | if (nextDirection.equals(oppWorkDir)) { |
|---|
| | | nextDirection = workDirection; |
|---|
| | | reverse = true; |
|---|
| | | } |
|---|
| | | // 总结：1.如果是作业组（差180°）方向，那么agv方向必须是作业方向，如果 reverse 则说明倒退，这时候nextDirection肯定会等于作业方向（前面赋值了），如果不相同，则报错 |
|---|
| | | // 2.如果不是作业组方向（另一组相反方向差180°），因为此函数不能旋转，所以差180°时只能倒退，倒退的时候因为agv不会旋转，所以nextDirection要变成agv方向 |
|---|
| | | } else { |
|---|
| | | if (!lastDirection.equals(nextDirection)) { |
|---|
| | | // 如果下一个方向与agv方向相反，则倒退行走，避免进行毫无意义的转弯动作。 |
|---|
| | | // 但是要注意：如果agv方向与工作方向正好相反，则需要旋转至工作方向，也就是为什么要加!workDirection.equals(oppLastDir)判断 |
|---|
| | | if (nextDirection.equals(oppLastDir) && !workDirection.equals(oppLastDir)) { |
|---|
| | | |
|---|
| | | // 判断当前节点是否可以旋转 |
|---|
| | | if (!lastCode.getCornerBool()) { |
|---|
| | | // 如果是作业方向，但是小车在巷道内方向错误，则停止 |
|---|
| | | if (reverse && !lastDirection.equals(nextDirection)) { |
|---|
| | | throw new CoolException(agvNo + "号小车方向错误，请推至转弯点手动调整"); |
|---|
| | | } |
|---|
| | | // 如果不是作业方向（另一组相反方向），判断是否相反方向，如果反方向则倒退行走 |
|---|
| | | if (nextDirection.equals(oppLastDir)) { |
|---|
| | | // 倒退时，因为agv方向没变，所以下一个方向还是agv方向，故nextDirection = lastDirection; |
|---|
| | | nextDirection = lastDirection; |
|---|
| | | reverse = true; |
|---|
| | | } else { |
|---|
| | | // turn |
|---|
| | | actionList.add(new Action( |
|---|
| | | null, // 编号s |
|---|
| | | task.getBusId(), // 总线 |
|---|
| | | task.getId(), // 任务 |
|---|
| | | null, // 动作号 |
|---|
| | | null, // 优先级 |
|---|
| | | ActionTypeType.TurnCorner.desc, // 名称 |
|---|
| | | (double) mapService.spinDirection(lastCode).val, // 属性值 |
|---|
| | | lastCode.getData(), // 地面码 |
|---|
| | | String.valueOf(nextDirection), // 动作参数 |
|---|
| | | ActionTypeType.TurnCorner.val(), // 动作类型 |
|---|
| | | actionPrepareSts, // 动作进度 |
|---|
| | | agvId, // AGV |
|---|
| | | now // 工作时间 |
|---|
| | | )); |
|---|
| | | } |
|---|
| | | // 总结：1.如果是作业组（差180°）方向，那么agv方向必须是作业方向，如果 reverse 则说明倒退，这时候nextDirection肯定会等于作业方向（前面赋值了），如果不相同，则报错 |
|---|
| | | // 2.如果不是作业组方向（另一组相反方向差180°），因为此函数不能旋转，所以差180°时只能倒退，倒退的时候因为agv不会旋转，所以nextDirection要变成agv方向 |
|---|
| | | } else { |
|---|
| | | if (!lastDirection.equals(nextDirection)) { |
|---|
| | | // 如果下一个方向与agv方向相反，则倒退行走，避免进行毫无意义的转弯动作。 |
|---|
| | | // 但是要注意：如果agv方向与工作方向正好相反，则需要旋转至工作方向，也就是为什么要加!workDirection.equals(oppLastDir)判断 |
|---|
| | | if (nextDirection.equals(oppLastDir) && !workDirection.equals(oppLastDir)) { |
|---|
| | | // 倒退时，因为agv方向没变，所以下一个方向还是agv方向，故nextDirection = lastDirection; |
|---|
| | | nextDirection = lastDirection; |
|---|
| | | reverse = true; |
|---|
| | | } else { |
|---|
| | | // turn |
|---|
| | | actionList.add(new Action( |
|---|
| | | null, // 编号s |
|---|
| | | task.getBusId(), // 总线 |
|---|
| | | task.getId(), // 任务 |
|---|
| | | null, // 动作号 |
|---|
| | | null, // 优先级 |
|---|
| | | ActionTypeType.TurnCorner.desc, // 名称 |
|---|
| | | (double) mapService.spinDirection(lastCode).val, // 属性值 |
|---|
| | | lastCode.getData(), // 地面码 |
|---|
| | | String.valueOf(nextDirection), // 动作参数 |
|---|
| | | ActionTypeType.TurnCorner.val(), // 动作类型 |
|---|
| | | actionPrepareSts, // 动作进度 |
|---|
| | | agvId, // AGV |
|---|
| | | now // 工作时间 |
|---|
| | | )); |
|---|
| | | |
|---|
| | | lastDirection = nextDirection; |
|---|
| | | lastDirection = nextDirection; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | } |
|---|
| | | } |
|---|
