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.alibaba.fastjson.JSONObject;
|
import com.core.common.DateUtils;
|
import com.core.common.SpringUtils;
|
import com.core.exception.CoolException;
|
import com.zy.asrs.entity.BasShuttleOpt;
|
import com.zy.asrs.service.BasShuttleOptService;
|
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.enums.*;
|
import com.zy.core.model.ShuttleSlave;
|
import com.zy.core.model.Task;
|
import com.zy.core.model.command.ShuttleAssignCommand;
|
import com.zy.core.model.command.ShuttleCommand;
|
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.HashMap;
|
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
|
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
|
|
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()));
|
|
//小车处于忙碌状态，将标记置为true
|
if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.BUSY) {
|
shuttleProtocol.setPakMk(true);
|
}
|
|
//四向穿梭车空闲、有任务且标记为true，需要执行任务的下一条指令
|
if (shuttleProtocol.getBusyStatusType() == ShuttleStatusType.IDLE && shuttleProtocol.getTaskNo() != 0 && shuttleProtocol.getPakMk()) {
|
//执行下一步指令
|
executeWork(shuttleProtocol.getAssignCommand());
|
}
|
|
}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;
|
}
|
|
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];
|
}
|
|
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) {
|
//将此map存入redis中
|
HashMap<String, Object> map = new HashMap<>();
|
|
//计算路径
|
List<NavigateNode> calc = NavigateUtils.calc(assignCommand.getSourceLocNo(), assignCommand.getDistLocNo(), assignCommand.getTaskMode().intValue());
|
if (calc != null) {
|
//获取分段路径
|
ArrayList<ArrayList<NavigateNode>> data = NavigateUtils.getSectionPath(calc);
|
//路径数据
|
map.put("path", data);
|
}
|
|
//工作号
|
map.put("wrk_no", assignCommand.getTaskNo());
|
//命令执行步序
|
map.put("commandStep", 0);
|
//命令
|
map.put("assignCommand", assignCommand);
|
shuttleProtocol.setTaskNo(assignCommand.getTaskNo());
|
shuttleProtocol.setAssignCommand(assignCommand);
|
//任务数据保存到redis
|
redisUtil.set("wrk_no_" + assignCommand.getTaskNo(), JSON.toJSONString(map));
|
//执行下发任务
|
executeWork(assignCommand);
|
}
|
|
//执行下发的指令
|
private void executeWork(ShuttleAssignCommand assignCommand) {
|
//读取redis数据
|
Object o = redisUtil.get("wrk_no_" + assignCommand.getTaskNo());
|
HashMap map = JSON.parseObject(o.toString(), HashMap.class);
|
//当前步序
|
int commandStep = Integer.parseInt(map.get("commandStep").toString());
|
//path路径数目
|
int size = 0;
|
//下发命令
|
ShuttleCommand command = new ShuttleCommand();
|
switch (assignCommand.getTaskMode()) {
|
case 1://入库
|
case 2://出库
|
//当前路径数据
|
Object data = map.get("path");
|
ArrayList pathList = JSON.parseObject(data.toString(), ArrayList.class);
|
//取第一条路径
|
Object o1 = pathList.get(commandStep);
|
ArrayList path = JSON.parseObject(o1.toString(), ArrayList.class);
|
|
size = path.size();
|
//开始路径
|
JSONObject startPath = JSON.parseObject(path.get(0).toString());
|
//目标路径
|
JSONObject endPath = JSON.parseObject(path.get(size - 1).toString());
|
|
command.setCommandWord((short) 1);
|
command.setStartCodeNum(NavigatePositionConvert.xyToPosition(startPath.getIntValue("x"), startPath.getIntValue("y")));
|
command.setMiddleCodeNum((short) 0);
|
command.setDistCodeNum(NavigatePositionConvert.xyToPosition(endPath.getIntValue("x"), endPath.getIntValue("y")));
|
command.setStartToDistDistance(1000);
|
command.setMiddleToDistDistance(1000);
|
command.setRunDirection(ShuttleRunDirection.get(startPath.get("direction").toString()).id);
|
command.setPalletLift((short) 1);
|
command.setForceMoveDistance(1000);
|
command.setChargeSwitch((short) 2);
|
command.setIOControl((short) 0);
|
command.setRunSpeed((short) 1000);
|
command.setRadarTmp((short) 0);
|
|
break;
|
case 3://托盘顶升
|
case 4://托盘下降
|
command.setCommandWord((short) 2);
|
command.setPalletLift(assignCommand.getTaskMode() == 3 ? (short)1 : (short)2);
|
command.setCommandEnd((short) 1);
|
break;
|
case 5://强制左移
|
case 6://强制右移
|
case 7://强制上移
|
case 8://强制下移
|
command.setCommandWord((short) 3);
|
command.setForceMoveDistance(1000);
|
command.setRunDirection((short) (assignCommand.getTaskMode() - 4));
|
command.setCommandEnd((short) 1);
|
break;
|
case 9://状态复位
|
command.setCommandWord((short) 0);
|
//设置四向穿梭车为空闲状态
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.IDLE);
|
//任务号清零
|
shuttleProtocol.setTaskNo((short) 0);
|
break;
|
default:
|
}
|
|
command.setCommandEnd((short) 1);
|
//下发命令
|
if (!write(command)) {
|
News.error("四向穿梭车命令下发失败，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
|
} else {
|
News.info("四向穿梭车命令下发成功，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
|
|
//将标记置为false(防止重发)
|
shuttleProtocol.setPakMk(false);
|
|
//判断数据是否执行完成
|
if (commandStep < size) {
|
//更新redis数据
|
//步序增加
|
commandStep++;
|
map.put("commandStep", commandStep);
|
//任务数据保存到redis
|
redisUtil.set("wrk_no_" + map.get("wrk_no").toString(), JSON.toJSONString(map));
|
}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.getDistLocNo(),
|
null,
|
null,
|
null,
|
JSON.toJSONString(command)
|
);
|
shuttleOptService.insert(opt);
|
}
|
//删除redis
|
redisUtil.del("wrk_no_" + map.get("wrk_no").toString());
|
|
//对主线程抛出等待确认状态waiting
|
shuttleProtocol.setProtocolStatus(ShuttleProtocolStatusType.WAITING);
|
|
News.info("四向穿梭车任务执行完成等待确认中，穿梭车号={}，任务数据={}", shuttleProtocol.getShuttleNo(), JSON.toJSON(command));
|
}
|
|
}
|
}
|
|
/******************************************************************************************/
|
/**************************************** 测试专用 *****************************************/
|
/*****************************************************************************************/
|
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);
|
|
}
|
}
|
