package com.zy.core.thread;
|
|
import HslCommunication.Core.Transfer.DataFormat;
|
import HslCommunication.Core.Types.OperateResult;
|
import HslCommunication.Core.Types.OperateResultExOne;
|
import HslCommunication.ModBus.ModbusTcpNet;
|
import com.alibaba.fastjson.JSON;
|
import com.core.common.DateUtils;
|
import com.core.common.SpringUtils;
|
import com.core.exception.CoolException;
|
import com.zy.asrs.entity.BasDevp;
|
import com.zy.asrs.entity.BasShuttle;
|
import com.zy.asrs.entity.BasShuttleOpt;
|
import com.zy.asrs.entity.LocMast;
|
import com.zy.asrs.service.BasDevpService;
|
import com.zy.asrs.service.BasShuttleOptService;
|
import com.zy.asrs.service.BasShuttleService;
|
import com.zy.asrs.service.LocMastService;
|
import com.zy.common.model.NavigateNode;
|
import com.zy.common.utils.CommonUtils;
|
import com.zy.common.utils.NavigatePositionConvert;
|
import com.zy.common.utils.NavigateUtils;
|
import com.zy.common.utils.RedisUtil;
|
import com.zy.core.News;
|
import com.zy.core.ThreadHandler;
|
import com.zy.core.cache.MessageQueue;
|
import com.zy.core.cache.OutputQueue;
|
import com.zy.core.cache.SlaveConnection;
|
import com.zy.core.enums.*;
|
import com.zy.core.model.ShuttleSlave;
|
import com.zy.core.model.Task;
|
import com.zy.core.model.command.*;
|
import com.zy.core.model.protocol.LiftProtocol;
|
import com.zy.core.model.protocol.ShuttleProtocol;
|
import lombok.Data;
|
import lombok.extern.slf4j.Slf4j;
|
|
import java.text.MessageFormat;
|
import java.util.ArrayList;
|
import java.util.Date;
|
import java.util.List;
|
|
/**
|
* 四向穿梭车线程
|
*/
|
@Data
|
@Slf4j
|
public class ShuttleThread implements Runnable, ThreadHandler {
|
|
private ModbusTcpNet modbusTcpNet;
|
private ShuttleSlave slave;
|
private ShuttleProtocol shuttleProtocol;
|
private RedisUtil redisUtil;
|
|
public ShuttleThread(ShuttleSlave slave,RedisUtil redisUtil) {
|
this.slave = slave;
|
this.redisUtil = redisUtil;
|
}
|
|
@Override
|
public void run() {
|
this.connect();
|
while (true) {
|
try {
|
int step = 1;
|
Task task = MessageQueue.poll(SlaveType.Shuttle, slave.getId());
|
if (task != null) {
|
step = task.getStep();
|
}
|
switch (step) {
|
// 读数据
|
case 1:
|
readStatus();
|
break;
|
// 写入数据
|
case 2:
|
write((ShuttleCommand) task.getData());
|
break;
|
//下发任务
|
case 3:
|
assignWork((ShuttleAssignCommand) task.getData());
|
break;
|
default:
|
break;
|
}
|
Thread.sleep(500);
|
} catch (Exception e) {
|
e.printStackTrace();
|
}
|
}
|
}
|
|
@Override
|
public boolean connect() {
|
boolean result = false;
|
//-------------------------四向穿梭车连接方法------------------------//
|
modbusTcpNet = new ModbusTcpNet(slave.getIp(), slave.getPort(), (byte) 0x01);
|
// 当你需要指定格式的数据解析时，就需要设置下面的这个信息
|
modbusTcpNet.setDataFormat(DataFormat.ABCD);
|
OperateResult connect = modbusTcpNet.ConnectServer();
|
if(connect.IsSuccess){
|
result = true;
|
OutputQueue.CRN.offer(MessageFormat.format( "【{0}】四向穿梭车plc连接成功 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort()));
|
log.info("四向穿梭车plc连接成功 ===>> [id:{}] [ip:{}] [port:{}] ", slave.getId(), slave.getIp(), slave.getPort());
|
} else {
|
OutputQueue.CRN.offer(MessageFormat.format("【{0}】四向穿梭车plc连接失败！！！ ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort()));
|
log.error("四向穿梭车plc连接失败！！！ ===>> [id:{}] [ip:{}] [port:{}] ", slave.getId(), slave.getIp(), slave.getPort());
|
}
|
modbusTcpNet.ConnectClose();
|
//-------------------------四向穿梭车连接方法------------------------//
|
return result;
|
}
|
|
@Override
|
public void close() {
|
modbusTcpNet.ConnectClose();
|
}
|
|
private void readStatus() {
|
try {
|
OperateResultExOne<byte[]> result = modbusTcpNet.Read("200", (short) 17);
|
if (result.IsSuccess) {
|
if (null == shuttleProtocol) {
|
shuttleProtocol = new ShuttleProtocol();
|
shuttleProtocol.setShuttleNo(slave.getId().shortValue());
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
|
}
|
|
//----------读取四向穿梭车状态-----------
|
//获取数据
|
byte[] content = result.Content;
|
//小车忙状态位
|
shuttleProtocol.setBusyStatus(modbusTcpNet.getByteTransform().TransInt16(content,0));
|
//当前二维码
|
shuttleProtocol.setCurrentCode(modbusTcpNet.getByteTransform().TransInt16(content,2));
|
//电池电量百分比
|
shuttleProtocol.setBatteryPower(modbusTcpNet.getByteTransform().TransInt16(content,4));
|
//电池温度
|
shuttleProtocol.setBatteryTemp(modbusTcpNet.getByteTransform().TransInt16(content,6));
|
//错误编号
|
shuttleProtocol.setErrorCode(modbusTcpNet.getByteTransform().TransInt16(content,8));
|
//Plc输出状态IO
|
boolean[] booleans = modbusTcpNet.getByteTransform().TransBool(content, 10, 2);
|
shuttleProtocol.setPlcOutputLift(booleans[1]);
|
shuttleProtocol.setPlcOutputTransfer(booleans[2]);
|
shuttleProtocol.setPlcOutputBrake(booleans[3]);
|
shuttleProtocol.setPlcOutputCharge(booleans[4]);
|
shuttleProtocol.setPlcOutputStatusIO(modbusTcpNet.getByteTransform().TransInt16(content, 10));
|
//错误信息码
|
shuttleProtocol.setStatusErrorCode(modbusTcpNet.getByteTransform().TransInt16(content,12));
|
//PLC输入状态
|
shuttleProtocol.setPlcInputStatus(modbusTcpNet.getByteTransform().TransInt16(content,14));
|
//当前或者之前读到的二维码值
|
shuttleProtocol.setCurrentOrBeforeCode(modbusTcpNet.getByteTransform().TransInt16(content,16));
|
//读到的二维码X方向偏移量
|
shuttleProtocol.setCodeOffsetX(modbusTcpNet.getByteTransform().TransInt16(content,18));
|
//读到的二维码Y方向偏移量
|
shuttleProtocol.setCodeOffsetY(modbusTcpNet.getByteTransform().TransInt16(content,20));
|
//当前的电压值
|
shuttleProtocol.setCurrentVoltage(modbusTcpNet.getByteTransform().TransInt16(content,22));
|
//当前的模拟量值
|
shuttleProtocol.setCurrentAnalogValue(modbusTcpNet.getByteTransform().TransInt16(content,24));
|
//当前的升降伺服速度
|
shuttleProtocol.setCurrentLiftServoSpeed(modbusTcpNet.getByteTransform().TransInt16(content,26));
|
//当前的行走伺服速度
|
shuttleProtocol.setCurrentMoveServoSpeed(modbusTcpNet.getByteTransform().TransInt16(content,28));
|
//当前的升降伺服负载率
|
shuttleProtocol.setCurrentLiftServoLoad(modbusTcpNet.getByteTransform().TransInt16(content,30));
|
//当前的行走伺服负载率
|
shuttleProtocol.setCurrentMoveServoLoad(modbusTcpNet.getByteTransform().TransInt16(content,32));
|
|
///读取四向穿梭车状态-end
|
|
//小车处于忙碌状态，将标记置为true
|
if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.BUSY) {
|
shuttleProtocol.setPakMk(true);
|
}
|
|
if (shuttleProtocol.getErrorCode() != 0 && shuttleProtocol.getProtocolStatusType() == ShuttleProtocolStatusType.WORKING) {
|
//出现错误
|
resetAndTryFix(shuttleProtocol.getTaskNo());
|
}
|
|
if(shuttleProtocol.getProtocolStatusType() == ShuttleProtocolStatusType.FIXING
|
&& shuttleProtocol.getTaskNo() != 0
|
&& shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE){
|
//处于故障修复状态
|
//执行下一步指令
|
executeWork(shuttleProtocol.getTaskNo());
|
}
|
|
//四向穿梭车空闲、有任务、标记为true、存在任务指令，需要执行任务的下一条指令
|
if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE
|
&& shuttleProtocol.getTaskNo() != 0
|
&& shuttleProtocol.getPakMk()) {
|
//执行下一步指令
|
executeWork(shuttleProtocol.getTaskNo());
|
}
|
|
//检测是否有提升机锁定标记，有则检测提升机是否到位，是否能走下一步命令
|
if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE
|
&& shuttleProtocol.getTaskNo() != 0) {
|
Object o = redisUtil.get("wrk_no_" + shuttleProtocol.getTaskNo());
|
if (o != null) {
|
ShuttleRedisCommand redisCommand = JSON.parseObject(o.toString(), ShuttleRedisCommand.class);
|
if (redisCommand.getLiftSecurityMk()) {
|
//执行下一步指令
|
executeWork(shuttleProtocol.getTaskNo());
|
}
|
}
|
}
|
|
//将四向穿梭车状态保存至数据库
|
BasShuttleService shuttleService = SpringUtils.getBean(BasShuttleService.class);
|
BasShuttle basShuttle = shuttleService.selectById(shuttleProtocol.getShuttleNo());
|
if (basShuttle == null) {
|
basShuttle = new BasShuttle();
|
//四向穿梭车号
|
basShuttle.setShuttleNo(slave.getId());
|
shuttleService.insert(basShuttle);
|
}
|
//小车忙状态位
|
basShuttle.setBusyStatus(shuttleProtocol.getBusyStatus().intValue());
|
//当前二维码
|
basShuttle.setCurrentCode(shuttleProtocol.getCurrentCode().intValue());
|
//电池电量百分比
|
basShuttle.setBatteryPower(shuttleProtocol.getBatteryPower().intValue());
|
//电池温度
|
basShuttle.setBatteryTemp(shuttleProtocol.getBatteryTemp().intValue());
|
//错误编号
|
basShuttle.setErrorCode(shuttleProtocol.getErrorCode().intValue());
|
//Plc输出状态IO
|
basShuttle.setPlcOutputStatusIo(shuttleProtocol.getPlcOutputStatusIO().intValue());
|
//错误信息码
|
basShuttle.setStatusErrorCode(shuttleProtocol.getStatusErrorCode().intValue());
|
//PLC输入状态
|
basShuttle.setPlcInputStatus(shuttleProtocol.getPlcInputStatus().intValue());
|
//当前或者之前读到的二维码值
|
basShuttle.setCurrentOrBeforeCode(shuttleProtocol.getCurrentOrBeforeCode().intValue());
|
//读到的二维码X方向偏移量
|
basShuttle.setCodeOffsetX(shuttleProtocol.getCodeOffsetX().intValue());
|
//读到的二维码Y方向偏移量
|
basShuttle.setCodeOffsetY(shuttleProtocol.getCodeOffsetY().intValue());
|
//当前的电压值
|
basShuttle.setCurrentVoltage(shuttleProtocol.getCurrentVoltage().intValue());
|
//当前的模拟量值
|
basShuttle.setCurrentAnalogValue(shuttleProtocol.getCurrentAnalogValue().intValue());
|
//当前的升降伺服速度
|
basShuttle.setCurrentLiftServoSpeed(shuttleProtocol.getCurrentLiftServoSpeed().intValue());
|
//当前的行走伺服速度
|
basShuttle.setCurrentMoveServoSpeed(shuttleProtocol.getCurrentMoveServoSpeed().intValue());
|
//当前的升降伺服负载率
|
basShuttle.setCurrentLiftServoLoad(shuttleProtocol.getCurrentLiftServoLoad().intValue());
|
//当前的行走伺服负载率
|
basShuttle.setCurrentMoveServoLoad(shuttleProtocol.getCurrentMoveServoLoad().intValue());
|
//当前小车状态（内部自我维护）
|
basShuttle.setShuttleStatus(shuttleProtocol.getProtocolStatus());
|
//任务号
|
basShuttle.setWrkNo(shuttleProtocol.getTaskNo().intValue());
|
//修改时间
|
basShuttle.setUpdateTime(new Date());
|
//作业标记
|
basShuttle.setPakMk(shuttleProtocol.getPakMk());
|
if (shuttleService.updateById(basShuttle)) {
|
OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】[id:{1}] <<<<< 实时数据更新成功",DateUtils.convert(new Date()), slave.getId()));
|
// log.info(MessageFormat.format("【{0}】[id:{1}] <<<<< 实时数据更新成功",DateUtils.convert(new Date()), slave.getId()));
|
}
|
|
}else {
|
OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】{1}四向穿梭车plc状态信息失败", DateUtils.convert(new Date()), slave.getId()));
|
throw new CoolException(MessageFormat.format( "四向穿梭车plc状态信息失败 ===>> [id:{0}] [ip:{1}] [port:{2}]", slave.getId(), slave.getIp(), slave.getPort()));
|
}
|
} catch (Exception e) {
|
e.printStackTrace();
|
OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】四向穿梭车plc状态信息失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort()));
|
initShuttle();
|
}
|
}
|
|
private boolean write(ShuttleCommand command){
|
if (null == command) {
|
News.error("四向穿梭车写入命令为空");
|
return false;
|
}
|
|
BasShuttleService shuttleService = SpringUtils.getBean(BasShuttleService.class);
|
if (shuttleService == null) {
|
News.error("系统错误");
|
return false;
|
}
|
|
BasShuttle basShuttle = shuttleService.selectById(slave.getId().shortValue());
|
if (basShuttle == null) {
|
News.error("四向穿梭车不存在");
|
return false;
|
}
|
|
command.setShuttleNo(slave.getId().shortValue());
|
// 开始任务
|
short[] array = new short[17];
|
//控制指令字
|
array[0] = command.getCommandWord();
|
if (command.getStartCodeNum() != null) {
|
//启始二维编号
|
array[1] = command.getStartCodeNum();
|
}
|
|
if (command.getMiddleCodeNum() != null) {
|
//中间二维编号
|
array[2] = command.getMiddleCodeNum();
|
}
|
|
if (command.getDistCodeNum() != null) {
|
//目标二维编号
|
array[3] = command.getDistCodeNum();
|
}
|
|
if (command.getStartToDistDistance() != null) {
|
//起点到目标点的距离长度，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间
|
short[] startToDistDistances = CommonUtils.intToShorts(command.getStartToDistDistance());
|
array[4] = startToDistDistances[0];
|
array[5] = startToDistDistances[1];
|
}
|
|
if (command.getMiddleToDistDistance() != null) {
|
//中间点到目标点的距离长度，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间
|
short[] middleToDistDistances = CommonUtils.intToShorts(command.getMiddleToDistDistance());
|
array[6] = middleToDistDistances[0];
|
array[7] = middleToDistDistances[1];
|
}
|
|
array[8] = basShuttle.getRunSpeed().shortValue();//四向穿梭车运行速度，从系统数据库读出
|
if (command.getRunDirection() != null) {
|
//小车运行方向
|
array[8] = command.getRunDirection();
|
}
|
|
if (command.getPalletLift() != null) {
|
//托盘顶升
|
array[9] = command.getPalletLift();
|
}
|
|
if (command.getForceMoveDistance() != null) {
|
//小车强制移动距离，先将int转为byte数组，再将byte数组转成两个short，分别占用4和5空间
|
short[] forceMoveDistances = CommonUtils.intToShorts(command.getForceMoveDistance());
|
array[10] = forceMoveDistances[0];
|
array[11] = forceMoveDistances[1];
|
}
|
|
if (command.getChargeSwitch() != null) {
|
//充电开关
|
array[12] = command.getChargeSwitch();
|
}
|
|
if (command.getIOControl() != null) {
|
//小车IO控制
|
array[13] = command.getIOControl();
|
}
|
|
if (command.getRunSpeed() != null) {
|
//小车运行速度
|
array[14] = command.getRunSpeed();
|
}
|
|
if (command.getRadarTmp() != null) {
|
//小车雷达备用
|
array[15] = command.getRadarTmp();
|
}
|
|
//指令结束位
|
array[16] = command.getCommandEnd();
|
|
OperateResult result = modbusTcpNet.Write("0", array);;
|
if (result != null && result.IsSuccess) {
|
News.info("四向穿梭车命令下发[id:{}] >>>>> {}", slave.getId(), JSON.toJSON(command));
|
OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】[id:{1}] >>>>> 命令下发： {2}", DateUtils.convert(new Date()), slave.getId(), JSON.toJSON(command)));
|
return true;
|
} else {
|
OutputQueue.SHUTTLE.offer(MessageFormat.format("【{0}】写入四向穿梭车plc数据失败 ===>> [id:{1}] [ip:{2}] [port:{3}]", DateUtils.convert(new Date()), slave.getId(), slave.getIp(), slave.getPort()));
|
News.error("写入四向穿梭车plc数据失败 ===>> [id:{}] [ip:{}] [port:{}]", slave.getId(), slave.getIp(), slave.getPort());
|
return false;
|
}
|
}
|
|
/**
|
* 初始化四向穿梭车
|
*/
|
private void initShuttle() {
|
if (null == shuttleProtocol) {
|
shuttleProtocol = new ShuttleProtocol();
|
}
|
}
|
|
//分配任务
|
private void assignWork(ShuttleAssignCommand assignCommand) {
|
if (!assignCommand.getAuto()) {
|
List<ShuttleCommand> commands = new ArrayList<>();
|
ShuttleCommand command = new ShuttleCommand();
|
LocMastService locMastService = SpringUtils.getBean(LocMastService.class);
|
switch (assignCommand.getTaskMode()) {
|
case 1://入库
|
case 2://出库
|
//小车移动到提升机口，计算路径
|
//计算小车起点到中点所需命令
|
LocMast currentLocMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
|
List<NavigateNode> firstMastResult = NavigateUtils.calc(currentLocMast.getLocNo(), assignCommand.getSourceLocNo(), ShuttleTaskModeType.PAK_IN.id);
|
if (firstMastResult != null) {
|
//获取分段路径
|
ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(firstMastResult);
|
//将每一段路径分成command指令
|
for (ArrayList<NavigateNode> nodes : data) {
|
//开始路径
|
NavigateNode startPath = nodes.get(0);
|
//目标路径
|
NavigateNode endPath = nodes.get(nodes.size() - 1);
|
Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
|
|
ShuttleCommand command1 = new ShuttleCommand();
|
command1.setCommandWord((short) 1);
|
command1.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), 1));
|
command1.setMiddleCodeNum((short) 1);
|
command1.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), 1));
|
command1.setStartToDistDistance(allDistance);
|
command1.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
|
command1.setForceMoveDistance(0);
|
command1.setIOControl((short) 0);
|
command1.setRunSpeed((short) 1000);
|
command1.setCommandEnd((short) 1);
|
commands.add(command1);
|
}
|
|
//托盘顶升
|
ShuttleCommand command2 = new ShuttleCommand();
|
command2.setCommandWord((short) 2);
|
command2.setStartCodeNum((short) 0);
|
command2.setMiddleCodeNum((short) 0);
|
command2.setDistCodeNum((short) 0);
|
command2.setStartToDistDistance(0);
|
command2.setMiddleToDistDistance(0);
|
command2.setRunDirection((short) 0);
|
command2.setForceMoveDistance(0);
|
command2.setPalletLift((short) 1);
|
command2.setRunSpeed((short) 0);
|
command2.setCommandEnd((short) 1);
|
commands.add(command2);
|
}else {
|
//没有计算到路径，可能存在小车位置就是起点位置
|
if (currentLocMast.getLocNo().equals(assignCommand.getSourceLocNo())) {
|
//小车位置就是起点位置，无需移动，直接顶升
|
//托盘顶升
|
ShuttleCommand command2 = new ShuttleCommand();
|
command2.setCommandWord((short) 2);
|
command2.setStartCodeNum((short) 0);
|
command2.setMiddleCodeNum((short) 0);
|
command2.setDistCodeNum((short) 0);
|
command2.setStartToDistDistance(0);
|
command2.setMiddleToDistDistance(0);
|
command2.setRunDirection((short) 0);
|
command2.setForceMoveDistance(0);
|
command2.setPalletLift((short) 1);
|
command2.setRunSpeed((short) 0);
|
command2.setCommandEnd((short) 1);
|
commands.add(command2);
|
}
|
}
|
|
//计算中点到终点路径
|
List<NavigateNode> secMastResult = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
|
if (secMastResult != null) {
|
//获取分段路径
|
ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(secMastResult);
|
//将每一段路径分成command指令
|
for (ArrayList<NavigateNode> nodes : data) {
|
//开始路径
|
NavigateNode startPath = nodes.get(0);
|
//目标路径
|
NavigateNode endPath = nodes.get(nodes.size() - 1);
|
Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
|
|
ShuttleCommand command1 = new ShuttleCommand();
|
command1.setCommandWord((short) 1);
|
command1.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getX(), startPath.getY(), 1));
|
command1.setMiddleCodeNum((short) 1);
|
command1.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getX(), endPath.getY(), 1));
|
command1.setStartToDistDistance(allDistance);
|
command1.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
|
command1.setForceMoveDistance(0);
|
command1.setIOControl((short) 0);
|
command1.setRunSpeed((short) 1000);
|
command1.setCommandEnd((short) 1);
|
commands.add(command1);
|
}
|
|
//托盘下降
|
ShuttleCommand command2 = new ShuttleCommand();
|
command2.setCommandWord((short) 2);
|
command2.setStartCodeNum((short) 0);
|
command2.setMiddleCodeNum((short) 0);
|
command2.setDistCodeNum((short) 0);
|
command2.setStartToDistDistance(0);
|
command2.setMiddleToDistDistance(0);
|
command2.setRunDirection((short) 0);
|
command2.setForceMoveDistance(0);
|
command2.setPalletLift((short) 2);
|
command2.setRunSpeed((short) 0);
|
command2.setCommandEnd((short) 1);
|
commands.add(command2);
|
}
|
break;
|
case 3://托盘顶升
|
case 4://托盘下降
|
command.setCommandWord((short) 2);
|
command.setStartCodeNum((short) 0);
|
command.setMiddleCodeNum((short) 0);
|
command.setDistCodeNum((short) 0);
|
command.setStartToDistDistance(0);
|
command.setMiddleToDistDistance(0);
|
command.setRunDirection((short) 0);
|
command.setForceMoveDistance(0);
|
command.setPalletLift(assignCommand.getTaskMode() == 3 ? (short)1 : (short)2);
|
command.setRunSpeed((short) 0);
|
command.setCommandEnd((short) 1);
|
commands.add(command);
|
break;
|
case 5://强制左移
|
case 6://强制右移
|
case 7://强制上移
|
case 8://强制下移
|
command.setCommandWord((short) 3);
|
command.setStartCodeNum((short) 0);
|
command.setMiddleCodeNum((short) 0);
|
command.setDistCodeNum((short) 0);
|
command.setStartToDistDistance(600);
|
command.setRunDirection((short) (assignCommand.getTaskMode() - 4));
|
command.setForceMoveDistance(600);
|
command.setIOControl((short) 0);
|
command.setCommandEnd((short) 1);
|
command.setRunSpeed((short) 1000);
|
commands.add(command);
|
break;
|
case 9://状态复位
|
command.setCommandWord((short) 6);
|
command.setStartCodeNum((short) 0);
|
command.setMiddleCodeNum((short) 0);
|
command.setDistCodeNum((short) 0);
|
command.setStartToDistDistance(0);
|
command.setMiddleToDistDistance(0);
|
command.setRunDirection((short) 0);
|
command.setPalletLift((short) 0);
|
command.setPalletLift((short) 0);
|
command.setForceMoveDistance(0);
|
command.setChargeSwitch((short) 0);
|
command.setIOControl((short) 0);
|
command.setRunSpeed((short) 0);
|
command.setCommandEnd((short) 1);
|
commands.add(command);
|
break;
|
case 10://向正方向(左)寻库位
|
command.setCommandWord((short) 4);
|
command.setRunDirection((short) 1);
|
command.setStartToDistDistance(3000);
|
command.setRunSpeed((short) 1000);
|
command.setCommandEnd((short) 1);
|
commands.add(command);
|
break;
|
case 11://向负方向(右)寻库位
|
command.setCommandWord((short) 4);
|
command.setRunDirection((short) 2);
|
command.setStartToDistDistance(3000);
|
command.setRunSpeed((short) 1000);
|
command.setCommandEnd((short) 1);
|
commands.add(command);
|
break;
|
case 12://向正方向(前)寻库位
|
command.setCommandWord((short) 4);
|
command.setRunDirection((short) 4);
|
command.setStartToDistDistance(3000);
|
command.setRunSpeed((short) 1000);
|
command.setCommandEnd((short) 1);
|
commands.add(command);
|
break;
|
case 13://向负方向(后)寻库位
|
command.setCommandWord((short) 4);
|
command.setRunDirection((short) 3);
|
command.setStartToDistDistance(3000);
|
command.setRunSpeed((short) 1000);
|
command.setCommandEnd((short) 1);
|
commands.add(command);
|
break;
|
case 14://移动到目标库位
|
LocMast locMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
|
List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), assignCommand.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
|
if (result != null) {
|
//获取分段路径
|
ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(result);
|
//将每一段路径分成command指令
|
for (ArrayList<NavigateNode> nodes : data) {
|
//开始路径
|
NavigateNode startPath = nodes.get(0);
|
//目标路径
|
NavigateNode endPath = nodes.get(nodes.size() - 1);
|
Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
|
|
LocMast distLocMast = locMastService.queryByLoc(assignCommand.getLocNo());
|
String qrCodeValue = distLocMast.getQrCodeValue();
|
command.setCommandWord((short) 1);
|
command.setStartCodeNum(shuttleProtocol.getCurrentCode());
|
command.setMiddleCodeNum((short) 1);
|
command.setDistCodeNum((short) Integer.parseInt(qrCodeValue));
|
command.setStartToDistDistance(allDistance);
|
command.setRunSpeed((short) 1000);
|
command.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
|
command.setForceMoveDistance(0);
|
command.setIOControl((short) 0);
|
command.setCommandEnd((short) 1);
|
commands.add(command);
|
}
|
}
|
break;
|
case 15://充电开关
|
command.setCommandWord((short) 8);
|
command.setIOControl((short) 4);
|
command.setCommandEnd((short) 1);
|
commands.add(command);
|
break;
|
default:
|
}
|
assignCommand.setCommands(commands);
|
}
|
|
ShuttleRedisCommand redisCommand = new ShuttleRedisCommand();
|
redisCommand.setShuttleNo(assignCommand.getShuttleNo());//四向穿梭车号
|
redisCommand.setWrkNo(assignCommand.getTaskNo());//工作号
|
redisCommand.setCommandStep(0);//命令执行步序
|
redisCommand.setAssignCommand(assignCommand);//命令
|
redisCommand.setErrorCommands(new ArrayList<ShuttleCommand>());//发生错误时尝试执行的指令，优先级最高
|
shuttleProtocol.setTaskNo(assignCommand.getTaskNo());
|
shuttleProtocol.setAssignCommand(assignCommand);
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
|
//任务数据保存到redis
|
redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(redisCommand));
|
//执行下发任务
|
executeWork(assignCommand.getTaskNo());
|
}
|
|
//执行下发的指令
|
private boolean executeWork(Short wrkNo) {
|
//读取redis数据
|
if (wrkNo == null) {
|
return false;
|
}
|
|
Object o = redisUtil.get("wrk_no_" + wrkNo);
|
if (o == null) {
|
return false;
|
}
|
ShuttleRedisCommand redisCommand = JSON.parseObject(o.toString(), ShuttleRedisCommand.class);
|
|
if (!checkLiftStation(wrkNo)) {//检测是否有提升机站点，有则调度提升机
|
return false;
|
}
|
|
//将标记置为false(防止重发)
|
shuttleProtocol.setPakMk(false);
|
|
List<ShuttleCommand> errorCommands = redisCommand.getErrorCommands();
|
if (errorCommands.size() > 0) {
|
//优先执行该指令
|
ShuttleCommand errorCommand = errorCommands.get(0);//取出指令
|
|
if(errorCommand.getCommandWord() == 1){//正常行走命令，需要先执行完找库位命令后，再执行
|
LocMastService locMastService = SpringUtils.getBean(LocMastService.class);
|
LocMast locMast = locMastService.queryByQrCode(shuttleProtocol.getCurrentCode().toString());
|
LocMast distLocMast = locMastService.queryByQrCode(errorCommand.getStartCodeNum().toString());
|
if (shuttleProtocol.getCurrentCode().equals(errorCommand.getStartCodeNum())) {
|
//起点和终点属于同一库位，无需再执行移动操作
|
errorCommands.remove(0);//移除该命令
|
redisCommand.setErrorCommands(new ArrayList<ShuttleCommand>());
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
|
//当前步序
|
int commandStep = redisCommand.getCommandStep();
|
//步序回退
|
commandStep--;
|
redisCommand.setCommandStep(commandStep);
|
//任务数据保存到redis
|
redisUtil.set("wrk_no_" + wrkNo, JSON.toJSONString(redisCommand));
|
shuttleProtocol.setPakMk(true);
|
return true;
|
}else {
|
List<NavigateNode> result = NavigateUtils.calc(locMast.getLocNo(), distLocMast.getLocNo(), ShuttleTaskModeType.PAK_IN.id);
|
if (result != null) {
|
//获取分段路径
|
ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(result);
|
//将每一段路径分成command指令
|
for (ArrayList<NavigateNode> nodes : data) {
|
//开始路径
|
NavigateNode startPath = nodes.get(0);
|
//目标路径
|
NavigateNode endPath = nodes.get(nodes.size() - 1);
|
Integer allDistance = NavigateUtils.getCurrentPathAllDistance(nodes);//计算当前路径行走总距离
|
|
String qrCodeValue = distLocMast.getQrCodeValue();
|
errorCommand.setCommandWord((short) 1);
|
errorCommand.setStartCodeNum(shuttleProtocol.getCurrentCode());
|
errorCommand.setMiddleCodeNum((short) 1);
|
errorCommand.setDistCodeNum((short) Integer.parseInt(qrCodeValue));
|
errorCommand.setStartToDistDistance(allDistance);
|
errorCommand.setRunSpeed((short) 1000);
|
errorCommand.setRunDirection(ShuttleRunDirection.get(startPath.getDirection()).id);
|
errorCommand.setForceMoveDistance(0);
|
errorCommand.setIOControl((short) 0);
|
errorCommand.setCommandEnd((short) 1);
|
break;
|
}
|
}
|
}
|
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WORKING);
|
//当前步序
|
int commandStep = redisCommand.getCommandStep();
|
//步序回退
|
commandStep--;
|
redisCommand.setCommandStep(commandStep);
|
}
|
|
if (!write(errorCommand)) {
|
News.error("四向穿梭车命令下发失败，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand));
|
return false;
|
} else {
|
News.info("四向穿梭车命令下发成功，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(errorCommand));
|
errorCommands.remove(0);
|
redisCommand.setErrorCommands(errorCommands);
|
//任务数据保存到redis
|
redisUtil.set("wrk_no_" + wrkNo, JSON.toJSONString(redisCommand));
|
return true;
|
}
|
}
|
|
List<ShuttleCommand> commands = redisCommand.getAssignCommand().getCommands();
|
//当前步序
|
int commandStep = redisCommand.getCommandStep();
|
//path路径数目
|
int size = commands.size();
|
ShuttleAssignCommand assignCommand = redisCommand.getAssignCommand();
|
|
//取出命令
|
ShuttleCommand command = commands.get(commandStep);
|
|
if (assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_IN.id.shortValue()
|
|| assignCommand.getTaskMode() == ShuttleTaskModeType.PAK_OUT.id.shortValue()
|
) {
|
//小车失去坐标，禁止下发命令
|
if (shuttleProtocol.getCurrentCode() == 0) {
|
return false;
|
}
|
}
|
|
//下发命令
|
if (!write(command)) {
|
News.error("四向穿梭车命令下发失败，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
|
return false;
|
} else {
|
News.info("四向穿梭车命令下发成功，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
|
|
//判断数据是否执行完成
|
if (commandStep < size - 1) {
|
//更新redis数据
|
//步序增加
|
commandStep++;
|
redisCommand.setCommandStep(commandStep);
|
//任务数据保存到redis
|
redisUtil.set("wrk_no_" + redisCommand.getWrkNo(), JSON.toJSONString(redisCommand));
|
}else {
|
//已执行完成
|
//保存数据到数据库做流水
|
BasShuttleOptService shuttleOptService = SpringUtils.getBean(BasShuttleOptService.class);
|
if (shuttleOptService != null) {
|
BasShuttleOpt opt = new BasShuttleOpt(
|
assignCommand.getTaskNo().intValue(),
|
assignCommand.getShuttleNo().intValue(),
|
new Date(),
|
ShuttleTaskModeType.get(assignCommand.getTaskMode()).desc,
|
assignCommand.getSourceLocNo(),
|
assignCommand.getLocNo(),
|
null,
|
null,
|
null,
|
JSON.toJSONString(assignCommand)
|
);
|
shuttleOptService.insert(opt);
|
}
|
|
if (redisCommand.getLiftSecurityMk()) {
|
//曾锁定过提升机，需要进行解锁
|
LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1);
|
LiftProtocol liftProtocol = liftThread.getLiftProtocol();
|
if (liftProtocol != null) {
|
liftProtocol.setSecurityMk(false);
|
}
|
}
|
|
//删除redis
|
redisUtil.del("wrk_no_" + redisCommand.getWrkNo());
|
|
if (!assignCommand.getAuto()) {
|
//手动模式不抛出等待状态，直接复位
|
//设置四向穿梭车为空闲状态
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
|
//任务号清零
|
shuttleProtocol.setTaskNo((short) 0);
|
//标记复位
|
shuttleProtocol.setPakMk(true);
|
News.info("四向穿梭车手动任务执行完成，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
|
}else {
|
if (!assignCommand.getCharge()) {
|
//对主线程抛出等待确认状态waiting
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WAITING);
|
}else {
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.CHARGING_WAITING);
|
}
|
News.info("四向穿梭车任务执行完成等待确认中，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
|
}
|
|
}
|
|
}
|
return true;
|
}
|
|
/**
|
* 检测是否有提升机站点，有则调度提升机
|
*/
|
private boolean checkLiftStation(Short wrkNo) {
|
//读取redis数据
|
if (wrkNo == null) {
|
return false;
|
}
|
|
Object o = redisUtil.get("wrk_no_" + wrkNo);
|
if (o == null) {
|
return false;
|
}
|
ShuttleRedisCommand redisCommand = JSON.parseObject(o.toString(), ShuttleRedisCommand.class);
|
//当前步序
|
int commandStep = redisCommand.getCommandStep();
|
|
//检测是否存在提升机口的指令
|
List<ShuttleCommand> commands = redisCommand.getAssignCommand().getCommands();
|
BasDevpService basDevpService = SpringUtils.getBean(BasDevpService.class);
|
ArrayList<Short> qrCodeValues = new ArrayList<>();
|
for (BasDevp basDevp : basDevpService.selectList(null)) {
|
//将所有提升机口二维码存入list
|
qrCodeValues.add(Short.parseShort(basDevp.getQrCodeValue()));
|
}
|
|
//遍历所有指令，判断是否有到提升机口的指令，并获取到达该提升机口所需步序
|
int step = 0;
|
ShuttleCommand command = null;
|
for (int i = 1; i < commands.size(); i++) {
|
command = commands.get(i);
|
if (qrCodeValues.contains(command.getDistCodeNum())) {
|
//存在
|
step = i + 1;
|
break;
|
}
|
}
|
|
if (step == 0) {
|
//无需后续检测，直接放行
|
return true;
|
}
|
|
//判断下一步是否为提升机口
|
if (commandStep + 1 != step) {
|
//下一步不是提升机口，跳过后续流程
|
return true;
|
}
|
|
//拿到提升机线程
|
LiftThread liftThread = (LiftThread) SlaveConnection.get(SlaveType.Lift, 1);
|
if (liftThread == null) {
|
return false;
|
}
|
LiftProtocol liftProtocol = liftThread.getLiftProtocol();
|
if (liftProtocol == null) {
|
return false;
|
}
|
|
//获取四向穿梭车当前楼层
|
String shuttleLocNo = shuttleProtocol.getCurrentLocNo();//二维码对应库位号
|
Integer shuttleLocNoLev = Integer.parseInt(shuttleLocNo.substring(shuttleLocNo.length() - 2, shuttleLocNo.length()));//库位号对应层高
|
|
//程序走到这，表示提升机可能一直就在当前层，可能经过了移动到达了该层
|
if (liftProtocol.getProtocolStatusType() == LiftProtocolStatusType.WAITING) {
|
//提升机等待确认
|
//设置提升机为空闲状态
|
liftProtocol.setProtocolStatus(LiftProtocolStatusType.IDLE);
|
//任务号清零
|
liftProtocol.setTaskNo((short) 0);
|
//标记复位
|
liftProtocol.setPakMk(true);
|
//任务指令清零
|
liftProtocol.setAssignCommand(null);
|
//提升机解锁
|
liftProtocol.setLiftLock(false);
|
}
|
|
//判断提升机是否在目标楼层
|
if (liftProtocol.getLev().intValue() == shuttleLocNoLev) {
|
//同一层，直接放行
|
return true;
|
}
|
|
//提升机和穿梭车处于不同楼层，需要进行调度
|
if (!liftProtocol.isIdle()) {
|
//提升机不是空闲
|
return false;
|
}
|
|
//给提升机分配任务
|
liftProtocol.setLiftLock(true);//锁定提升机
|
liftProtocol.setTaskNo(shuttleProtocol.getTaskNo());//设置任务号
|
liftProtocol.setShuttleNo(shuttleProtocol.getShuttleNo());//设置四向穿梭车号
|
liftProtocol.setProtocolStatus(LiftProtocolStatusType.WORKING);//设置提升机状态为工作中
|
liftProtocol.setSecurityMk(true);//标记置为true，防止其他任务占用当前提升机
|
redisCommand.setLiftSecurityMk(true);//标记置为true，防止其他任务占用当前提升机
|
//任务数据保存到redis
|
redisUtil.set("wrk_no_" + wrkNo, JSON.toJSONString(redisCommand));
|
|
//命令list
|
ArrayList<LiftCommand> liftCommands = new ArrayList<>();
|
LiftCommand liftCommand = new LiftCommand();
|
liftCommand.setLiftNo(liftProtocol.getLiftNo());//提升机号
|
liftCommand.setTaskNo(liftProtocol.getTaskNo());//任务号
|
liftCommand.setRun((short) 1);//升降
|
liftCommand.setDistPosition(shuttleLocNoLev.shortValue());//目标楼层(穿梭车所在楼层)
|
liftCommand.setLiftLock(true);//锁定提升机
|
liftCommands.add(liftCommand);//将命令添加进list
|
|
LiftAssignCommand liftAssignCommand = new LiftAssignCommand();
|
liftAssignCommand.setCommands(liftCommands);
|
liftAssignCommand.setLiftNo(liftProtocol.getLiftNo());
|
liftAssignCommand.setTaskNo(liftProtocol.getTaskNo());
|
//下发任务
|
MessageQueue.offer(SlaveType.Lift, liftProtocol.getLiftNo().intValue(), new Task(3, liftAssignCommand));
|
|
return false;
|
}
|
|
/**
|
* 复位并尝试修复错误
|
*/
|
private boolean resetAndTryFix(Short wrkNo) {
|
//读取redis数据
|
if (wrkNo == null) {
|
return false;
|
}
|
|
Object o = redisUtil.get("wrk_no_" + wrkNo);
|
if (o == null) {
|
return false;
|
}
|
|
ShuttleRedisCommand redisCommand = JSON.parseObject(o.toString(), ShuttleRedisCommand.class);
|
List<ShuttleCommand> commands = redisCommand.getAssignCommand().getCommands();
|
//当前步序
|
int commandStep = redisCommand.getCommandStep();
|
//path路径数目
|
int size = commands.size();
|
|
ArrayList<ShuttleCommand> list = new ArrayList<>();
|
|
//取出命令
|
ShuttleCommand command = commands.get(commandStep - 1);
|
|
//复位命令
|
ShuttleCommand resetCommand = new ShuttleCommand();
|
resetCommand.setCommandWord((short) 6);
|
resetCommand.setStartCodeNum((short) 0);
|
resetCommand.setMiddleCodeNum((short) 0);
|
resetCommand.setDistCodeNum((short) 0);
|
resetCommand.setStartToDistDistance(0);
|
resetCommand.setMiddleToDistDistance(0);
|
resetCommand.setRunDirection((short) 0);
|
resetCommand.setPalletLift((short) 0);
|
resetCommand.setPalletLift((short) 0);
|
resetCommand.setForceMoveDistance(0);
|
resetCommand.setChargeSwitch((short) 0);
|
resetCommand.setIOControl((short) 0);
|
resetCommand.setRunSpeed((short) 0);
|
resetCommand.setCommandEnd((short) 1);
|
list.add(resetCommand);
|
|
//车辆空闲，等待写入找库位命令
|
//找库位命令
|
ShuttleCommand searchCommand = new ShuttleCommand();
|
searchCommand.setCommandWord((short) 4);
|
short direction = 1;
|
switch (command.getRunDirection()) {
|
case 1:
|
direction = 2;
|
break;
|
case 2:
|
direction = 1;
|
break;
|
case 3:
|
direction = 4;
|
break;
|
case 4:
|
direction = 3;
|
break;
|
default:
|
direction = 1;
|
}
|
|
searchCommand.setRunDirection(direction);//运行方向
|
searchCommand.setStartToDistDistance(1200);
|
searchCommand.setRunSpeed((short) 1000);
|
searchCommand.setCommandEnd((short) 1);
|
list.add(searchCommand);
|
|
//移动车辆，需要在执行完寻找定位点后再执行
|
ShuttleCommand moveCommand = new ShuttleCommand();
|
moveCommand.setCommandWord((short) 1);
|
moveCommand.setStartCodeNum(command.getStartCodeNum());//存入目标库位号
|
list.add(moveCommand);
|
|
redisCommand.setErrorCommands(list);
|
//任务数据保存到redis
|
redisUtil.set("wrk_no_" + wrkNo, JSON.toJSONString(redisCommand));
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.FIXING);
|
return true;
|
}
|
|
/******************************************************************************************/
|
/**************************************** 测试专用 *****************************************/
|
/*****************************************************************************************/
|
public static void main(String[] args) throws InterruptedException {
|
ShuttleSlave slave = new ShuttleSlave();
|
slave.setId(1);
|
slave.setIp("192.168.4.24");
|
slave.setPort(502);
|
// ShuttleThread thread = new ShuttleThread(slave);
|
// thread.connect();
|
// thread.readStatus();
|
//
|
// ShuttleCommand command = new ShuttleCommand();
|
// command.setCommandWord((short) 0);
|
// command.setStartCodeNum((short) 12323);
|
// command.setMiddleCodeNum((short) 22323);
|
// command.setDistCodeNum((short) 29999);
|
// command.setStartToDistDistance(109999);
|
// command.setMiddleToDistDistance(5000);
|
// command.setRunDirection((short) 1);
|
// command.setPalletLift((short) 2);
|
// command.setForceMoveDistance(3000);
|
// command.setChargeSwitch((short) 2);
|
// command.setIOControl((short) 0);
|
// command.setRunSpeed((short) 0);
|
// command.setRadarTmp((short) 0);
|
// command.setCommandEnd((short) 1);
|
// thread.write(command);
|
|
}
|
}
|
