*

lsh

2025-01-13 24e33bb02f9789d5618e12c5e0e70372831c36c7

*

 src/main/java/com/zy/asrs/service/impl/MainServiceImpl.java

@@ -6,7 +6,6 @@
import com.zy.asrs.entity.*;
import com.zy.asrs.service.BasCircularShuttleService;
import com.zy.asrs.service.BasDevpPositionService;
import com.zy.asrs.service.BasDevpService;
import com.zy.asrs.service.WrkMastService;
import com.zy.asrs.utils.SortTheExecutionOfTheCarUtil;
import com.zy.asrs.utils.TimeCalculatorUtils;
@@ -29,7 +28,7 @@

/**
 * 立体仓库WCS系统主流程业务
 * Created by vincent on 2020/8/6
 * Created by IX on 2024/10/01
 */
@Slf4j
@Service("mainService")
@@ -38,8 +37,6 @@

    @Autowired
    private SlaveProperties slaveProperties;
    @Autowired
    private BasDevpService basDevpService;
    @Autowired
    private BasRgvServiceImpl basRgvService;
    @Autowired
@@ -59,55 +56,36 @@
    private Long rgvCount;
    @Value("${constant-parameters.rgvDate}")
    private Double rgvDate;

    /**
     * 站点任务检测  更新小车位置信息
     */
    public synchronized void updateStePositionNearby1() {
        for (RgvSlave rgvSlave:slaveProperties.getRgv()) {
            RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvSlave.getId());
            RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
            if (rgvProtocol == null) {
                continue;
            }
            try{
                double finalVelocity = 0.0;     // 最终速度 (m/s)
                double distance = (Math.pow(finalVelocity, 2) - Math.pow(rgvProtocol.instantaneousSpeed/60, 2)) / (2 * acceleration);
                BasDevpPosition basDevpPosition = basDevpPositionService.selectOne(new EntityWrapper<BasDevpPosition>().eq("dev_no", 111));
                if (distance*proportion > ((SortTheExecutionOfTheCarUtil.LatelyAndLessThan(basDevpPosition.getPlcPosition(),rgvProtocol.getRgvPos(),perimeter) + (rgvProtocol.instantaneousSpeed/60)*proportion*0.1))){
//                        continue ;
                }
            } catch (Exception e){

            }
        }
    }

    public synchronized void updateStePositionNearby() {
        try{
        try {
            List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("wrk_sts", 1L));
            if (wrkMasts.isEmpty()){
            if (wrkMasts.isEmpty()) {
                return;
            }
            List<BasDevpPosition> basDevpPositions = basDevpPositionService.selectList(new EntityWrapper<BasDevpPosition>().orderBy("plc_position",true));
            if (basDevpPositions.isEmpty()){
            List<BasDevpPosition> basDevpPositions = basDevpPositionService.selectList(new EntityWrapper<BasDevpPosition>().orderBy("plc_position", true));
            if (basDevpPositions.isEmpty()) {
                log.error("获取所有站点信息异常");
                return;
            }
            List<Integer> siteList = SortTheExecutionOfTheCarUtil.WrkMastExtractSites(wrkMasts);
            List<Integer> devpList = SortTheExecutionOfTheCarUtil.BasDevpPositionExtractSites(basDevpPositions);
            Integer siteNo = SortTheExecutionOfTheCarUtil.ObtainDenseAreasFirstPlace(siteList, devpList);
            if (siteNo == null){
            if (siteNo == null) {
                log.error("就近密集原则获取站点失败");
                return;
            }
            Long sitePosition = 0L;
            for (BasDevpPosition basDevpPosition : basDevpPositions){
                if (basDevpPosition.getDevNo().equals(siteNo)){
            for (BasDevpPosition basDevpPosition : basDevpPositions) {
                if (basDevpPosition.getDevNo().equals(siteNo)) {
                    sitePosition = basDevpPosition.getPlcPosition();
                }
            }
            List<List<Long>> rgvPositionList = new ArrayList<>();
            for (RgvSlave rgvSlave:slaveProperties.getRgv()) {
            for (RgvSlave rgvSlave : slaveProperties.getRgv()) {
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvSlave.getId());
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
@@ -118,78 +96,79 @@
                rgvPosition.add(rgvProtocol.getRgvPos());
                rgvPositionList.add(rgvPosition);
            }
            Integer rgvNo = SortTheExecutionOfTheCarUtil.LatelyAndGreaterThan(rgvPositionList, sitePosition,perimeter);
            if (rgvNo == -1){
                log.info("更新小车排序信息异常={}",rgvNo);
            Integer rgvNo = SortTheExecutionOfTheCarUtil.LatelyAndGreaterThan(rgvPositionList, sitePosition, perimeter);
            if (rgvNo == -1) {
                log.info("更新小车排序信息异常={}", rgvNo);
                return;
            }
            List<BasCircularShuttle> basCircularShuttleList = basCircularShuttleService.selectList(new EntityWrapper<BasCircularShuttle>().orderBy("rgv_id", true));
            if (basCircularShuttleList.get(0).getRgvNo().equals(rgvNo)){
            if (basCircularShuttleList.get(0).getRgvNo().equals(rgvNo)) {
                return;
            }

            int[][] ints = new int[basCircularShuttleList.size()][2];
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList){
                ints[basCircularShuttle.getRgvNo()-1] = new int[]{basCircularShuttle.getRgvNo(),basCircularShuttle.getRgvId()};
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                ints[basCircularShuttle.getRgvNo() - 1] = new int[]{basCircularShuttle.getRgvNo(), basCircularShuttle.getRgvId()};
            }
            String[] oldList = new String[ints.length];
            for (int[] rgv : ints){
                oldList[rgv[0]-1] = Arrays.toString(rgv);
            }
//            String[] oldList = new String[ints.length];
//            for (int[] rgv : ints){
//                oldList[rgv[0]-1] = Arrays.toString(rgv);
//            }
            int[][] rgvList = SortTheExecutionOfTheCarUtil.ReorderSteId(ints, rgvNo);
            String[] newList = new String[rgvList.length];
            for (int[] rgv : rgvList){
                newList[rgv[0]-1] = Arrays.toString(rgv);
            }
//            String[] newList = new String[rgvList.length];
//            for (int[] rgv : rgvList){
//                newList[rgv[0]-1] = Arrays.toString(rgv);
//            }

            log.info("更新小车排序信息：原始小车号rgvNo={},小车重新排序信息={},小车原始排序信息={}",rgvNo,Arrays.toString(newList),Arrays.toString(oldList));
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList){
//            log.info("更新小车排序信息：原始小车号rgvNo={},小车重新排序信息={},小车原始排序信息={}",rgvNo,Arrays.toString(newList),Arrays.toString(oldList));
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                int[] rgv = rgvList[basCircularShuttle.getRgvNo() - 1];
                basCircularShuttle.setRgvId(rgv[1]);
                basCircularShuttleService.updateById(basCircularShuttle);
            }

        } catch (Exception e) {
            log.error("自动更新小车排序信息失败，异常："+e);
            log.error("自动更新小车排序信息失败，异常：" + e);
        }
    }

    /**
     * 站点任务检测  更新小车位置信息
     */
    public synchronized void updateStePosition() {
        try{
        try {
            boolean sign = false;
            Integer rgvNo = 0;
            List<BasCircularShuttle> basCircularShuttleList = basCircularShuttleService.selectList(new EntityWrapper<BasCircularShuttle>().orderBy("rgv_id", true));
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList){
            for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, basCircularShuttle.getRgvNo());
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
                    sign = true;
                } else if (rgvProtocol.modeType != RgvModeType.AUTO || (rgvProtocol.getStatusType() != RgvStatusType.IDLE && rgvProtocol.getStatusType() != RgvStatusType.ROAM) ){
                } else if (rgvProtocol.modeType != RgvModeType.AUTO || (rgvProtocol.getStatusType() != RgvStatusType.IDLE && rgvProtocol.getStatusType() != RgvStatusType.ROAM)) {
                    sign = true;
                } else {
                    rgvNo = basCircularShuttle.getRgvNo();
                    break;
                }
            }
            if (sign && rgvNo!=0){
            if (sign && rgvNo != 0) {
                int[][] ints = new int[basCircularShuttleList.size()][2];
                for (BasCircularShuttle basCircularShuttle : basCircularShuttleList){
                    ints[basCircularShuttle.getRgvNo()-1] = new int[]{basCircularShuttle.getRgvNo(),basCircularShuttle.getRgvId()};
                for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                    ints[basCircularShuttle.getRgvNo() - 1] = new int[]{basCircularShuttle.getRgvNo(), basCircularShuttle.getRgvId()};
                }
                String[] oldList = new String[ints.length];
                for (int[] rgv : ints){
                    oldList[rgv[0]-1] = Arrays.toString(rgv);
                }
//                String[] oldList = new String[ints.length];
//                for (int[] rgv : ints){
//                    oldList[rgv[0]-1] = Arrays.toString(rgv);
//                }
                int[][] rgvList = SortTheExecutionOfTheCarUtil.ReorderSteId(ints, rgvNo);
                String[] newList = new String[rgvList.length];
                for (int[] rgv : rgvList){
                    newList[rgv[0]-1] = Arrays.toString(rgv);
                }
//                String[] newList = new String[rgvList.length];
//                for (int[] rgv : rgvList){
//                    newList[rgv[0]-1] = Arrays.toString(rgv);
//                }

                log.info("更新小车排序信息：原始小车号rgvNo={},小车重新排序信息={},小车原始排序信息={}",rgvNo,Arrays.toString(newList),Arrays.toString(oldList));
                for (BasCircularShuttle basCircularShuttle : basCircularShuttleList){
//                log.info("更新小车排序信息：原始小车号rgvNo={},小车重新排序信息={},小车原始排序信息={}",rgvNo,Arrays.toString(newList),Arrays.toString(oldList));
                for (BasCircularShuttle basCircularShuttle : basCircularShuttleList) {
                    int[] rgv = rgvList[basCircularShuttle.getRgvNo() - 1];
                    basCircularShuttle.setRgvId(rgv[1]);
                    basCircularShuttleService.updateById(basCircularShuttle);
@@ -197,20 +176,21 @@
            }

        } catch (Exception e) {
            log.error("自动更新小车排序信息失败，异常："+e);
            log.error("自动更新小车排序信息失败，异常：" + e);
        }
    }

    /**
     * 站点任务检测  下发小车取放任务
     */
    public synchronized void DevpTaskNoRun() {
        try{
            List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("wrk_sts", 1L).orderBy("modi_time",true));
            if (wrkMasts.isEmpty()){
        try {
            List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("wrk_sts", 1L).orderBy("modi_time", true));
            if (wrkMasts.isEmpty()) {
                return;
            }
            long differenceInSeconds = TimeCalculatorUtils.differenceInMilliseconds(wrkMasts.get(0).getModiTime(), new Date());
            if (differenceInSeconds<=100){
            if (differenceInSeconds <= 100) {
                return;
            }

@@ -220,29 +200,29 @@
            if (rgvProtocol == null) {
                return;
            }
            List<BasDevpPosition> basDevpPositions = basDevpPositionService.selectList(new EntityWrapper<BasDevpPosition>().orderBy("plc_position",true));
            Integer devNo = SortTheExecutionOfTheCarUtil.LatelyAndLessThan(basDevpPositions, rgvProtocol.getRgvPos(),perimeter);
            List<BasDevpPosition> basDevpPositions = basDevpPositionService.selectList(new EntityWrapper<BasDevpPosition>().orderBy("plc_position", true));
            Integer devNo = SortTheExecutionOfTheCarUtil.LatelyAndLessThan(basDevpPositions, rgvProtocol.getRgvPos(), perimeter);
            BasDevpPosition[] basDevpPositionsList = SortTheExecutionOfTheCarUtil.devpNoSort(basDevpPositions, devNo);
            BasDevpPosition[] basDevpPositionsListUN = SortTheExecutionOfTheCarUtil.devpNoSortUN(basDevpPositionsList);
            List<List<WrkMast>> wrkMastLists = getWrkMastLists(basDevpPositionsListUN);
            //下发任务
            taskDown(wrkMastLists);
        } catch (Exception e){
            log.error("自动下发小车取放任务失败，异常："+e);
        } catch (Exception e) {
            log.error("自动下发小车取放任务失败，异常：" + e);
        }
    }

    //获取小车取放任务列表失败
    public synchronized List<List<WrkMast>> getWrkMastLists(BasDevpPosition[] basDevpPositionsListUN) {
        List<List<WrkMast>> wrkMastLists = new ArrayList<>();
        try{
        try {
            List<WrkMast> wrkMastlistA = new ArrayList<>();
            List<WrkMast> wrkMastlistB = new ArrayList<>();
            for (BasDevpPosition basDevpPosition : basDevpPositionsListUN){
                List<WrkMast> wrkMastList = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("sta_no", basDevpPosition.getDevNo()).eq("wrk_sts", 1L).orderBy("modi_time",true));
                for (WrkMast wrkMast:wrkMastList){
                    if (!Cools.isEmpty(wrkMast)){
                        if (SortTheExecutionOfTheCarUtil.devpNoSortbj(basDevpPositionsListUN,wrkMast.getSourceStaNo(),wrkMast.getStaNo())){
            for (BasDevpPosition basDevpPosition : basDevpPositionsListUN) {
                List<WrkMast> wrkMastList = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("sta_no", basDevpPosition.getDevNo()).eq("wrk_sts", 1L).orderBy("modi_time", true));
                for (WrkMast wrkMast : wrkMastList) {
                    if (!Cools.isEmpty(wrkMast)) {
                        if (SortTheExecutionOfTheCarUtil.devpNoSortbj(basDevpPositionsListUN, wrkMast.getSourceStaNo(), wrkMast.getStaNo())) {
                            wrkMastlistA.add(wrkMast);
                        } else {
                            wrkMastlistB.add(wrkMast);
@@ -253,8 +233,8 @@
            wrkMastLists.add(wrkMastlistA);
            wrkMastLists.add(wrkMastlistB);
            return wrkMastLists;
        } catch (Exception e){
            log.error("获取小车取放任务列表失败，异常："+e);
        } catch (Exception e) {
            log.error("获取小车取放任务列表失败，异常：" + e);
            return wrkMastLists;
        }
    }
@@ -262,37 +242,36 @@
    public synchronized void taskDown(List<List<WrkMast>> wrkMastLists) {
        long rgvId = 0;
        runRgv:
        for (List<WrkMast> wrkMastList : wrkMastLists){
            for (WrkMast wrkMast: wrkMastList){
                while (rgvId <= rgvCount){
        for (List<WrkMast> wrkMastList : wrkMastLists) {
            for (WrkMast wrkMast : wrkMastList) {
                while (rgvId <= rgvCount) {
                    rgvId++;
                    log.info("存在任务，RGV号={}，任务数据={}", rgvId, JSON.toJSON(wrkMast));

                    if (rgvId > rgvCount){
//                    log.info("存在任务，RGV号={}，任务数据={}", rgvId, JSON.toJSON(wrkMast));
                    if (rgvId > rgvCount) {
                        break runRgv;
                    }
                    BasCircularShuttle basCircularShuttle = basCircularShuttleService.selectOne(new EntityWrapper<BasCircularShuttle>().eq("rgv_id", rgvId));
                    RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, basCircularShuttle.getRgvNo());
                    RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                    if (rgvProtocol == null){
                        continue ;
                    if (rgvProtocol == null) {
                        continue;
                    }

                    List<WrkMast> wrkMasts = wrkMastService.selectList(new EntityWrapper<WrkMast>().eq("rgv_no", rgvProtocol.getRgvNo()).eq("wrk_sts", 2L));
                    if (!wrkMasts.isEmpty()){
                        continue ;
                    if (!wrkMasts.isEmpty()) {
                        continue;
                    }
                    if (rgvProtocol != null
                            && rgvProtocol.modeType == RgvModeType.AUTO
                            && (rgvProtocol.getStatusType() == RgvStatusType.IDLE || rgvProtocol.getStatusType() == RgvStatusType.ROAM)
                            && rgvProtocol.getTaskNo1()==0
                            && rgvProtocol.getAlarm() == 0){
                            && rgvProtocol.getTaskNo1() == 0
                            && rgvProtocol.getAlarm() == 0) {
                        if (rgvProtocol.getStatusType() == RgvStatusType.ROAM) {
                            double finalVelocity = 0.0;     // 最终速度 (m/s)
                            double distance = (Math.pow(finalVelocity, 2) - Math.pow(rgvProtocol.instantaneousSpeed/60, 2)) / (2 * acceleration);
                            double distance = (Math.pow(finalVelocity, 2) - Math.pow(rgvProtocol.instantaneousSpeed / 60, 2)) / (2 * acceleration);
                            BasDevpPosition basDevpPosition = basDevpPositionService.selectOne(new EntityWrapper<BasDevpPosition>().eq("dev_no", wrkMast.getSourceStaNo()));
                            if (distance*proportion > ((SortTheExecutionOfTheCarUtil.LatelyAndLessThan(basDevpPosition.getPlcPosition(),rgvProtocol.getRgvPos(),perimeter) + (rgvProtocol.instantaneousSpeed/60)*proportion*rgvDate))){
                                continue ;
                            if (distance * proportion > ((SortTheExecutionOfTheCarUtil.LatelyAndLessThan(basDevpPosition.getPlcPosition(), rgvProtocol.getRgvPos(), perimeter) + (rgvProtocol.instantaneousSpeed / 60) * proportion * rgvDate))) {
                                continue;
                            }
                        }
                        RgvCommand rgvCommand = new RgvCommand();
@@ -313,16 +292,17 @@
                        wrkMast.setWrkSts(2L);
                        wrkMast.setRgvNo(rgvProtocol.getRgvNo());
                        wrkMast.setAppeTime(new Date());
                        try{
                        try {
                            wrkMastService.updateById(wrkMast);
                            break;
                        }catch (Exception e){
                            log.error("更新小车任务失败,任务号："+wrkMast.getWrkNo());
                        } catch (Exception e) {
                            log.error("更新小车任务失败,任务号：" + wrkMast.getWrkNo());
                        }
                        break runRgv;
                    } else {
                        break runRgv;
                    }
//                    else {
//                        break runRgv;
//                    }
                }
            }
        }
@@ -330,8 +310,8 @@

    //任务完成
    public synchronized void rgvCompleteWrkMastSta() {
        try{
            for (RgvSlave rgvSlave:slaveProperties.getRgv()) {
        try {
            for (RgvSlave rgvSlave : slaveProperties.getRgv()) {
                RgvThread rgvThread = (RgvThread) SlaveConnection.get(SlaveType.Rgv, rgvSlave.getId());
                RgvProtocol rgvProtocol = rgvThread.getRgvProtocol();
                if (rgvProtocol == null) {
@@ -342,23 +322,23 @@
                    log.error("{}号RGV尚未在数据库进行维护！", rgvSlave.getId());
                    continue;
                }
                if (rgvProtocol.getStatusType() == RgvStatusType.WAITING){
                    log.info("{}号小车等待wcs确认，状态{}，参数{}",rgvProtocol.getRgvNo(),rgvProtocol.getStatusType(),rgvProtocol);
                if (rgvProtocol.getStatusType() == RgvStatusType.WAITING) {
                    log.info("{}号小车等待wcs确认，状态{}，参数{}", rgvProtocol.getRgvNo(), rgvProtocol.getStatusType(), rgvProtocol);
                }
                // 只有当RGV等待WCS确认、自动
                if (rgvProtocol.getStatusType() == RgvStatusType.WAITING
                        && rgvProtocol.getModeType() == RgvModeType.AUTO
                        && rgvProtocol.getTaskNo1()!=0
                        && rgvProtocol.getTaskNo1() != 0
                ) {
                    log.info("{}号小车等待wcs确认，状态{}，参数{}",rgvProtocol.getRgvNo(),rgvProtocol.getStatusType(),rgvProtocol);
//                    log.info("{}号小车等待wcs确认，状态{}，参数{}",rgvProtocol.getRgvNo(),rgvProtocol.getStatusType(),rgvProtocol);
                    WrkMast wrkMast = wrkMastService.selectOne(new EntityWrapper<WrkMast>().eq("wrk_no", rgvProtocol.getTaskNo1().longValue()));
                    if (Cools.isEmpty(wrkMast) || !wrkMast.getWrkSts().equals(2L)){
                        log.error("未查到小车执行任务或者执行任务状态不符合！"+wrkMast);
                    if (Cools.isEmpty(wrkMast) || !wrkMast.getWrkSts().equals(2L)) {
                        log.error("未查到小车执行任务或者执行任务状态不符合！" + wrkMast);
                        continue;
                    }
                    boolean rgvComplete = rgvComplete(rgvProtocol.getRgvNo());
                    if (!rgvComplete){
                        log.error("小车复位失败，小车号{}！",rgvProtocol.getRgvNo());
                    if (!rgvComplete) {
                        log.error("小车复位失败，小车号{}！", rgvProtocol.getRgvNo());
                        break;
                    }
                    wrkMast.setWrkSts(3L);
@@ -366,26 +346,26 @@
                    wrkMastService.updateById(wrkMast);
                }
            }
        } catch (Exception e){
            log.error("小车复位线程报错！"+e);
        } catch (Exception e) {
            log.error("小车复位线程报错！" + e);
        }
    }

    /*
     * 小车复位
     * */
    public synchronized boolean rgvComplete(Integer rgvId){
        try{
    public synchronized boolean rgvComplete(Integer rgvNo) {
        try {
            //  命令下发区 --------------------------------------------------------------------------
            if (!MessageQueue.offer(SlaveType.Rgv, rgvId, new Task(3, new RgvCommand()))) {
                log.error("RGV命令下发失败，RGV号={}",rgvId);
            if (!MessageQueue.offer(SlaveType.Rgv, rgvNo, new Task(3, new RgvCommand()))) {
                log.error("RGV命令下发失败，RGV号={}", rgvNo);
                return false;
            } else {
                log.info("RGV命令下发成功，RGV号={}",rgvId);
                log.info("RGV命令下发成功，RGV号={}", rgvNo);
                return true;
            }
        }catch (Exception e){
            log.error("RGV命令下发失败，RGV号={}。异常："+e,rgvId);
        } catch (Exception e) {
            log.error("RGV命令下发失败，RGV号={}。异常：" + e, rgvNo);
            return false;
        }
    }

 src/main/java/com/zy/core/MainProcess.java

@@ -45,7 +45,6 @@
                        continue;
                    }
                    //更新位置信息
//                    mainService.updateStePositionNearby1();
                    mainService.updateStePositionNearby();

                    mainService.updateStePosition();