agv/opentcs
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

调试货叉指令

Browse Source
This commit is contained in:
wait 2025-04-23 17:13:59 +08:00
parent 503b7e9f4c
commit 09582c474b
5 changed files with 284 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java
View File

@ -34,11 +34,13 @@ import org.opentcs.drivers.vehicle.SimVehicleCommAdapter;
import org.opentcs.drivers.vehicle.VehicleCommAdapter;
import org.opentcs.drivers.vehicle.VehicleProcessModel;
import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO;
import org.opentcs.kc.udp.Service.ActImmediately;
import org.opentcs.kc.udp.Service.ConfirmRelocation;
import org.opentcs.kc.udp.Service.HybridNavigation;
import org.opentcs.kc.udp.Service.ManualPosition;
import org.opentcs.kc.udp.Service.QryRobotRunStatus;
import org.opentcs.kc.udp.Service.QryRobotStatus;
import org.opentcs.kc.udp.Service.ReadValue;
import org.opentcs.kc.udp.Service.SubCargoStatus;
import org.opentcs.kc.udp.Service.SubRobotStatue;
import org.opentcs.kc.udp.Service.SwitchAutomaticMode;
@ -148,6 +150,10 @@ public class LoopbackCommunicationAdapter
* 用于判断是否切换执行订单
*/
private static String uniqueOrderName;
/**
* 动作执行状态
*/
private static boolean ACTION_EXECUTION_STATUS;
/**
* Creates a new instance.
@ -320,7 +326,7 @@ public class LoopbackCommunicationAdapter
System.out.println("send cmd print end");
//AGV控制器执行命令
//AGV控制器执行命令------todo 待修改-》opentcs首次会下两个指令，未进入线程中不进行任务阻塞下发。
HybridNavigation.command(orderName, sourcePoint, destinationPointName, operation);
//检查当前车辆模型是否处于单步模式且未运行，若满足条件则设置运行状态为true。
@ -331,7 +337,7 @@ public class LoopbackCommunicationAdapter
if (uniqueOrderName == null || !uniqueOrderName.equals(orderName)) {
//设置唯一订单名称
uniqueOrderName = orderName;
//记录订单起点
//记录订单起点---更新复位后可
LAST_PASSED_POINT = sourcePoint;
}
@ -567,7 +573,7 @@ public class LoopbackCommunicationAdapter
if (step.getPath() == null) {
LOG.debug("-Starting operation...");
//动作执行待完成
//无可执行路径，检查是否需要执行动作
// operationSimulation(command, 1);
operationExec(command);
} else {
@ -718,6 +724,111 @@ public class LoopbackCommunicationAdapter
*/
private void operationExec(MovementCommand command) {
//todo-------------------
if (ACTION_EXECUTION_STATUS) {
//递归进入
//todo 读取动作执行状态
byte[] value = ReadValue.command();
int execStatus = value[0];
System.out.println("-execStatus:" + execStatus);
if (execStatus == 1) {
//动作执行完成,执行重置任务
resetOperation();
LOG.debug("-Operation exec finished.");
ACTION_EXECUTION_STATUS = false;
finishMoveCmd(command);
} else {
LOG.debug("-Operation exec 1111111.");
//递归
getExecutor().schedule(
() -> operationExec(command),
SIMULATION_PERIOD,
TimeUnit.MILLISECONDS
);
}
} else {
//首次进入
ACTION_EXECUTION_STATUS = true;
//获取动作
String operation = command.getOperation();
LOG.debug("-Operation exec start.");
//下发立即动作指令
if ("LIFT".equals(operation)) {
LOG.debug("-Operation exec LoadOperation.");
resetOperation();
float height = 1.1f;
byte modeOfMovement = 0x01;
//抬升叉齿指令
ActImmediately.command(height, modeOfMovement);
} else if ("DECLINE".equals(operation)) {
LOG.debug("-Operation exec UnloadOperation.");
resetOperation();
float height = 0.1f;
byte modeOfMovement = 0x02;
//降叉齿指令
ActImmediately.command(height, modeOfMovement);
} else if (operation.equals(this.getRechargeOperation())) {
LOG.debug("-Operation exec RechargeOperation.");
resetOperation();
//充电指令
} else {
LOG.debug("-Operation exec 2222222.");
//动作未定义，不执行
ACTION_EXECUTION_STATUS = false;
nextCommand();
}
LOG.debug("-Operation exec 3333333.");
//递归
getExecutor().schedule(
() -> operationExec(command),
SIMULATION_PERIOD,
TimeUnit.MILLISECONDS
);
}
//todo-------------------
}
private void resetOperation() {
ActImmediately.reset();
//todo 读取动作执行状态
byte[] value = ReadValue.command();
int execStatus = value[0];
while (execStatus == 2) {
LOG.debug("-resetOperation error");
byte[] value1 = ReadValue.command();
execStatus = value1[0];
}
}
/**
@ -824,7 +935,7 @@ public class LoopbackCommunicationAdapter
}
/**
* 结束移动命令。
* 结束命令。
* @param command 指令
*/
private void finishMoveCmd(MovementCommand command) {
@ -834,6 +945,8 @@ public class LoopbackCommunicationAdapter
getProcessModel().setState(Vehicle.State.IDLE);
//清除订单对应唯一ID
HybridNavigation.delUniqueOrderID(command);
LOG.debug("-callback wms");
}
//如果传入指令和移动指令队列第一条数据相同

47
opentcs-common/src/main/java/org/opentcs/kc/udp/KCCommandDemo.java
View File

@ -338,45 +338,46 @@ public class KCCommandDemo {
public static AgvEvent actionNow() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY);
//TODO 构建
Integer orderId = 1;
Integer actionID = 5;
// //构建Action指令
// Action actionLiftUp = new Action(
Action actionLiftUp = new Action(
ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
(byte) 0x02, //只能执行当前动作
actionID, //actionID
ActionSet.fork0x12_paramsize,
ActionSet.fork0x12(
1.1f, //升降高度
(byte) 0x01, //1上升 2下降
(byte)0x01 //01 开始 任务
)
);
agvEvent.setBody(actionLiftUp.toBytes());
// //重置 空指令
// Action actionReset = new Action(
// ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
// (byte) 0x02, //只能执行当前动作
// 1, //actionID
// actionID, //actionID
// ActionSet.fork0x12_paramsize,
// ActionSet.fork0x12(
// 1.1f, //升降高度
// (byte) 0x01, //1上升 2下降
// (byte)0x01 //01 开始 任务
// (byte)0x00 //01 开始 任务
// )
// );
// agvEvent.setBody(actionLiftUp.toBytes());
//重置 空指令
Action actionReset = new Action(
ActionSet.cancel0x03, //机器人移动到正常点 停止，2个字节
(byte) 0x02, //只能执行当前动作
2, //actionID
ActionSet.cancle0x03_paramsize,
ActionSet.cancel0x12(
1, //订单id
(byte)0x01 //1 立即停止， 2 正常移动到点再停止
)
);
agvEvent.setBody(actionReset.toBytes());
// );
// agvEvent.setBody(actionReset.toBytes());
//
// //构建Action指令
// Action actionLiftDown = new Action(
// ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
// (byte) 0x02, //只能执行当前动作
// 1, //actionID
// actionID, //actionID
// ActionSet.fork0x12_paramsize,
// ActionSet.fork0x12(
// 1.1f, //升降高度
// 0.1f, //升降高度
// (byte) 0x02, //1上升 2下降
// (byte)0x01 //01 开始 任务
// )

92
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ActImmediately.java Normal file
View File

@ -0,0 +1,92 @@
package org.opentcs.kc.udp.Service;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.navigation.Action;
import org.opentcs.kc.udp.agv.param.function.navigation.ActionSet;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class ActImmediately extends BaseCommand{
private static Integer actionId = 20;
/**
* decs: 立即动作指令
* 指令：0xB2
* author: caixiang
* date: 2025/1/17 16:25
*/
private static AgvEvent actionNow(float execuHeight, byte modeOfMovement) {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY);
//TODO 构建
//构建Action指令
Action actionLiftUp = new Action(
ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
(byte) 0x02, //只能执行当前动作
actionId, //动作ID
ActionSet.fork0x12_paramsize,
ActionSet.fork0x12(
execuHeight, //升降高度
modeOfMovement, //1上升 2下降
(byte)0x01 //01 开始任务
)
);
actionId++;
agvEvent.setBody(actionLiftUp.toBytes());
return agvEvent;
}
private static AgvEvent resetCmd() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY);
//重置 空指令
Action actionReset = new Action(
ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
(byte) 0x02, //只能执行当前动作
actionId, //actionID
ActionSet.fork0x12_paramsize,
ActionSet.fork0x12(
0.1f, //升降高度
(byte) 0x01, //1上升 2下降
(byte)0x00 //00 无任务
)
);
agvEvent.setBody(actionReset.toBytes());
actionId++;
agvEvent.setBody(actionReset.toBytes());
return agvEvent;
}
public static void command(float execuHeight, byte modeOfMovement){
//0xB2(立即动作指令)
AgvEvent agvEvent = actionNow(execuHeight, modeOfMovement);
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if(rcv.isOk()){
System.out.println("0xB2 forklift ok");
}else {
System.out.println();
System.out.println("0xB2 forklift fail");
System.out.println("received transationId : "+ "isok:"+rcv.isOk());
}
}
public static void reset(){
//0xB2(立即动作指令)
AgvEvent agvEvent = resetCmd();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if(rcv.isOk()){
System.out.println("-0xB2 reset ok");
}else {
System.out.println();
System.out.println("-0xB2 reset fail");
System.out.println("-received transationId : "+ "isok:"+rcv.isOk());
}
}
}

2
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/HybridNavigation.java
View File

@ -49,7 +49,7 @@ public class HybridNavigation
/**
* 订单映射最大订单ID.
*/
private static int currentMaxiOrderId = 0;
private static int currentMaxiOrderId = 25;
/**

50
opentcs-common/src/main/java/org/opentcs/kc/udp/Service/ReadValue.java Normal file
View File

@ -0,0 +1,50 @@
package org.opentcs.kc.udp.Service;
import java.util.ArrayList;
import java.util.List;
import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.read.ReadParam;
import org.opentcs.kc.udp.agv.param.function.read.ReadRsp;
import org.opentcs.kc.udp.agv.param.function.read.ReadStrValue;
import org.opentcs.kc.udp.agv.param.function.read.ReadValueMember;
import org.opentcs.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kc.udp.io.UDPClient;
public class ReadValue extends BaseCommand{
private static AgvEvent readValue() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_READ);
List<ReadValueMember> readValueMemberList = new ArrayList<>();
ReadValueMember readValueMember1 = new ReadValueMember(Short.valueOf("10"), Short.valueOf("1"));
readValueMemberList.add(readValueMember1);
List<ReadStrValue> readStrValueList = new ArrayList<>();
ReadStrValue readStrValue = new ReadStrValue("Lift", 1, readValueMemberList);
readStrValueList.add(readStrValue);
ReadParam readParam = new ReadParam(agvEvent.getTransationId(), readStrValueList);
agvEvent.setBody(readParam.toBytes());
return agvEvent;
}
public static byte[] command() {
AgvEvent agvEvent = readValue();
printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) {
System.out.println();
System.out.println("-received " + "isok:" + rcv.isOk() + " dataBytes:");
printInfo(rcv);
ReadRsp readRsp = new ReadRsp(rcv.getDataBytes());
return readRsp.dataValue;
}
else {
System.out.println();
System.out.println("-received transationId : " + "isok:" + rcv.isOk());
throw new RuntimeException("-read failed");
}
}
}