| | |
|---|
| | | List<Action> actionList = new ArrayList<>(); |
|---|
| | | // start node |
|---|
| | | Code lastCode = codeService.getCacheById(agvDetail.getRecentCode()); |
|---|
| | | Double lastDirection = agvDetail.getAgvAngle(); |
|---|
| | | Double lastDirection = MapService.mapToNearest(agvDetail.getAgvAngle()); |
|---|
| | | if (!lastCode.getData().equals(pathList.get(0))) { |
|---|
| | | throw new CoolException("AGV[" + agvNo + "]定位偏移..."); |
|---|
| | | } |
|---|
| | |
|---|
| | | Code nextCode = codeService.getCacheByData(next); |
|---|
| | | Double nextDirection = mapService.calculateDirection(lastCode, nextCode, angleOffsetVal); |
|---|
| | | |
|---|
| | | // 如果去下个节点的方向与当前方向差180度，则不需要旋转，改为反方向行走，nextDirection继续赋值是为了避免 ‘第一个动作一定是 turn’ 的判断 |
|---|
| | | // 第一步：如果下一个方向正好是作业方向的相反方向，则重置下一个方向为作业方向，标记 reverse = true |
|---|
| | | boolean reverse = false; |
|---|
| | | if (nextDirection.equals((workDirection + 180) % 360)) { |
|---|
| | | nextDirection = workDirection; |
|---|
| | | reverse = true; |
|---|
| | | } |
|---|
| | | |
|---|
| | | // 第二步：判断当前节点是否可以旋转 |
|---|
| | | if (!lastCode.getCornerBool()) { |
|---|
| | | // 如果是作业方向，但是小车在巷道内方向错误，则停止 |
|---|
| | | if (reverse && !lastDirection.equals(nextDirection)) { |
|---|
| | | // throw new CoolException(agvNo + "号小车方向错误，请推至转弯点手动调整"); |
|---|
| | | } |
|---|
| | | // 如果不是作业方向，判断是否相反方向，如果反方向则倒退行走 |
|---|
| | | if (nextDirection.equals((lastDirection + 180) % 360)) { |
|---|
| | | nextDirection = lastDirection; |
|---|
| | | reverse = true; |
|---|
| | | } |
|---|
| | | } else { |
|---|
| | | if (!lastDirection.equals(nextDirection)) { |
|---|
| | | if (lastDirection.equals((nextDirection + 180) % 360)) { |
|---|
| | | // 如果下个节点方向与当前agv方向相反，则倒退行走，但是如果当前agv方向正好与工作方向相反，则旋转至工作方向 |
|---|
| | | if (nextDirection.equals((lastDirection + 180) % 360) && !workDirection.equals((lastDirection + 180) % 360)) { |
|---|
| | | nextDirection = lastDirection; |
|---|
| | | reverse = true; |
|---|
| | | } else { |
|---|
| | | // turn |
|---|
| | | actionList.add(new Action( |
|---|
| | | null, // 编号 |
|---|
| | | null, // 编号s |
|---|
| | | task.getBusId(), // 总线 |
|---|
| | | task.getId(), // 任务 |
|---|
| | | null, // 动作号 |
|---|
