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

update

Browse Source
This commit is contained in:
wait 2025-04-24 17:06:20 +08:00
parent 09582c474b
commit affe72c308
6 changed files with 180 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -571,10 +571,10 @@ public class LoopbackCommunicationAdapter
Step step = command.getStep(); Step step = command.getStep();
getProcessModel().setState(Vehicle.State.EXECUTING); getProcessModel().setState(Vehicle.State.EXECUTING);
System.out.println("start one operation");
if (step.getPath() == null) { if (step.getPath() == null) {
LOG.debug("-Starting operation..."); LOG.debug("-Starting operation...");
//无可执行路径，检查是否需要执行动作 //无可执行路径，检查是否需要执行动作
// operationSimulation(command, 1);
operationExec(command); operationExec(command);
} else { } else {
getProcessModel().getVelocityController().addWayEntry( getProcessModel().getVelocityController().addWayEntry(
@ -656,12 +656,14 @@ public class LoopbackCommunicationAdapter
//若否，更新当前车辆位置，并根据命令是否有操作决定进入操作模拟或完成命令并模拟下一个命令。 //若否，更新当前车辆位置，并根据命令是否有操作决定进入操作模拟或完成命令并模拟下一个命令。
LAST_PASSED_POINT = currentPoint; LAST_PASSED_POINT = currentPoint;
LOG.debug("-Movement finished.");
System.out.println("-emptyOperation:" + command.hasEmptyOperation());
if (!command.hasEmptyOperation()) { if (!command.hasEmptyOperation()) {
//执行AGV动作 //执行AGV动作
LOG.debug("-Starting operation..."); LOG.debug("-Starting operation...");
operationExec(command); operationExec(command);
} else { } else {
LOG.debug("-Movement Finishing command.");
//完成当前命令 //完成当前命令
finishMoveCmd(command); finishMoveCmd(command);
//执行下一个命令 //执行下一个命令
@ -724,91 +726,103 @@ public class LoopbackCommunicationAdapter
*/ */
private void operationExec(MovementCommand command) { private void operationExec(MovementCommand command) {
//todo------------------- System.out.println("operationExec00000000000000");
if (ACTION_EXECUTION_STATUS) { if (ACTION_EXECUTION_STATUS) {
//递归进入 System.out.println("operationExec-----------");
//todo 读取动作执行状态 //todo 读取动作执行状态
byte[] value = ReadValue.command(); byte[] value = ReadValue.command();
int execStatus = value[0]; int execStatus = value[0];
System.out.println();
System.out.println("-execStatus:" + execStatus); System.out.println("-execStatus:" + execStatus);
if (execStatus == 1) { if (execStatus == 1) {
//动作执行完成,执行重置任务 //递归
getExecutor().schedule(
this::readActionState,
200,
TimeUnit.MILLISECONDS
);
resetOperation(); LOG.debug("-Operation exec finished.");
LOG.debug("-Operation exec finished."); ActImmediately.reset();
ACTION_EXECUTION_STATUS = false;
finishMoveCmd(command); //todo 读取动作执行状态
getExecutor().schedule(
this::readActionState,
200,
TimeUnit.MILLISECONDS
);
ACTION_EXECUTION_STATUS = false;
finishMoveCmd(command);
nextCommand();
} else { } else {
LOG.debug("-Operation exec 1111111."); LOG.debug("-Operation exec 1111111.");
//递归 //递归
getExecutor().schedule( getExecutor().schedule(
() -> operationExec(command), () -> operationExec(command),
SIMULATION_PERIOD, 500,
TimeUnit.MILLISECONDS TimeUnit.MILLISECONDS
); );
} }
} else { } else {
System.out.println("operationExec===========");
//首次进入 //首次进入
ACTION_EXECUTION_STATUS = true; ACTION_EXECUTION_STATUS = true;
//获取动作 //获取动作
String operation = command.getOperation(); String operation = command.getOperation();
LOG.debug("-Operation exec start."); LOG.debug("-Operation exec start:" + operation);
//下发立即动作指令 //下发立即动作指令
if ("LIFT".equals(operation)) { if ("LIFT".equals(operation)) {
LOG.debug("-Operation exec LoadOperation."); LOG.debug("-Operation exec LoadOperation.");
resetOperation();
float height = 1.1f; float height = 1.1f;
byte modeOfMovement = 0x01; byte modeOfMovement = 0x01;
//抬升叉齿指令 allotsOperation(height, modeOfMovement);
ActImmediately.command(height, modeOfMovement);
} else if ("DECLINE".equals(operation)) { } else if ("DECLINE".equals(operation)) {
LOG.debug("-Operation exec UnloadOperation."); LOG.debug("-Operation exec UnloadOperation.");
resetOperation();
float height = 0.1f; float height = 0.1f;
byte modeOfMovement = 0x02; byte modeOfMovement = 0x02;
//降叉齿指令 //降叉齿指令
ActImmediately.command(height, modeOfMovement); allotsOperation(height, modeOfMovement);
} else if (operation.equals(this.getRechargeOperation())) { } else if (operation.equals(this.getRechargeOperation())) {
LOG.debug("-Operation exec RechargeOperation."); LOG.debug("-Operation exec RechargeOperation.");
resetOperation(); // allotsOperation();
//充电指令 //充电指令
} else { } else {
LOG.debug("-Operation exec 2222222."); LOG.debug("-Operation exec :" + operation);
//动作未定义，不执行 //动作未定义，不执行
ACTION_EXECUTION_STATUS = false; ACTION_EXECUTION_STATUS = false;
nextCommand(); nextCommand();
} }
LOG.debug("-Operation exec 3333333."); LOG.debug("-Operation exec 222222.");
//递归 //递归
getExecutor().schedule( getExecutor().schedule(
() -> operationExec(command), () -> operationExec(command),
SIMULATION_PERIOD, 500,
TimeUnit.MILLISECONDS TimeUnit.MILLISECONDS
); );
} }
@ -817,18 +831,44 @@ public class LoopbackCommunicationAdapter
//todo------------------- //todo-------------------
} }
private void resetOperation() { private void allotsOperation(float height, byte modeOfMovement) {
LOG.debug("allotsOperation111111");
//todo 读取动作执行状态
getExecutor().schedule(
this::readActionState,
200,
TimeUnit.MILLISECONDS
);
ActImmediately.reset(); ActImmediately.reset();
//todo 读取动作执行状态
getExecutor().schedule(
this::readActionState,
200,
TimeUnit.MILLISECONDS
);
//抬升叉齿指令
ActImmediately.command(height, modeOfMovement);
}
/**
* 读取动作状态
*/
private void readActionState() {
//todo 读取动作执行状态 //todo 读取动作执行状态
byte[] value = ReadValue.command(); byte[] value = ReadValue.command();
int execStatus = value[0]; int execCmd = value[0];
while (execStatus == 2) { if (execCmd == 2) {
LOG.debug("-resetOperation error"); getExecutor().schedule(
byte[] value1 = ReadValue.command(); () -> readActionState(),
execStatus = value1[0]; 200,
TimeUnit.MILLISECONDS
);
} }
} }
/** /**

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

@ -4,6 +4,8 @@ import java.nio.ByteOrder;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.List; import java.util.List;
import org.opentcs.kc.common.byteutils.ByteUtils; import org.opentcs.kc.common.byteutils.ByteUtils;
import org.opentcs.kc.udp.Service.ActImmediately;
import org.opentcs.kc.udp.Service.ReadValue;
import org.opentcs.kc.udp.agv.param.AgvEvent; import org.opentcs.kc.udp.agv.param.AgvEvent;
import org.opentcs.kc.udp.agv.param.AgvEventConstant; import org.opentcs.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kc.udp.agv.param.function.af.QueryRobotStatusRsp; import org.opentcs.kc.udp.agv.param.function.af.QueryRobotStatusRsp;
@ -225,19 +227,90 @@ public class KCCommandDemo {
// } // }
// } // }
{ // {
//0xB2(立即动作指令) // //0xB2(立即动作指令)
AgvEvent agvEvent = actionNow(); // AgvEvent agvEvent = actionNow();
printInfo(agvEvent); // printInfo(agvEvent);
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); // RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if(rcv.isOk()){ // if(rcv.isOk()){
System.out.println("0xB2 ok"); // System.out.println("0xB2 ok");
}else { // }else {
System.out.println(); // System.out.println();
System.out.println("0xB2 fail"); // System.out.println("0xB2 fail");
System.out.println("received transationId : "+ "isok:"+rcv.isOk()); // System.out.println("received transationId : "+ "isok:"+rcv.isOk());
} // }
} // }
// {
//
// //todo 读取动作执行状态
// byte[] value = ReadValue.command();
// int execStatus = value[0];
//
// Thread.sleep(200);
//
// while (execStatus == 2) {
// byte[] value1 = ReadValue.command();
// execStatus = value1[0];
// }
//
//
// ActImmediately.reset();
//
// //todo 读取动作执行状态
// byte[] value2 = ReadValue.command();
// int execStatus2 = value2[0];
//
// Thread.sleep(200);
//
// while (execStatus2 == 2) {
// byte[] value1 = ReadValue.command();
// execStatus2 = value1[0];
// }
//
// float height = 1.1f;
// byte modeOfMovement = 0x01;
//
// //抬升叉齿指令
// ActImmediately.command(height, modeOfMovement);
// }
{
ActImmediately.reset();
}
// {
//
// //todo 读取动作执行状态
// byte[] value = ReadValue.command();
// int execStatus = value[0];
//
// Thread.sleep(200);
//
// while (execStatus == 2) {
// byte[] value1 = ReadValue.command();
// execStatus = value1[0];
// }
//
//
// ActImmediately.reset();
//
// //todo 读取动作执行状态
// byte[] value2 = ReadValue.command();
// int execStatus2 = value2[0];
//
// Thread.sleep(200);
//
// while (execStatus2 == 2) {
// byte[] value1 = ReadValue.command();
// execStatus2 = value1[0];
// }
//
// float height = 0.1f;
// byte modeOfMovement = 0x02;
//
// ActImmediately.command(height, modeOfMovement);
// }
@ -338,26 +411,10 @@ public class KCCommandDemo {
public static AgvEvent actionNow() { public static AgvEvent actionNow() {
AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY); AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY);
//TODO 构建 //TODO 构建
Integer actionID = 5; Integer actionID = 102;
// //构建Action指令 // //构建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个字节 // ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
// (byte) 0x02, //只能执行当前动作 // (byte) 0x02, //只能执行当前动作
// actionID, //actionID // actionID, //actionID
@ -365,10 +422,26 @@ public class KCCommandDemo {
// ActionSet.fork0x12( // ActionSet.fork0x12(
// 1.1f, //升降高度 // 1.1f, //升降高度
// (byte) 0x01, //1上升 2下降 // (byte) 0x01, //1上升 2下降
// (byte)0x00 //01 开始 任务 // (byte)0x01 //01 开始 任务
// ) // )
// ); // );
// agvEvent.setBody(actionReset.toBytes()); // agvEvent.setBody(actionLiftUp.toBytes());
// //重置 空指令
Action actionReset = new Action(
ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
(byte) 0x02, //只能执行当前动作
actionID, //actionID
ActionSet.fork0x12_paramsize,
ActionSet.fork0x12(
0.0f, //升降高度
(byte) 0x01, //1上升 2下降
(byte)0x00 //01 开始 任务
)
);
agvEvent.setBody(actionReset.toBytes());
// //
// //构建Action指令 // //构建Action指令
// Action actionLiftDown = new Action( // Action actionLiftDown = new Action(

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

@ -9,7 +9,7 @@ import org.opentcs.kc.udp.io.UDPClient;
public class ActImmediately extends BaseCommand{ public class ActImmediately extends BaseCommand{
private static Integer actionId = 20; private static Integer actionId = 115;
/** /**
* decs: 立即动作指令 * decs: 立即动作指令

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

@ -49,7 +49,7 @@ public class HybridNavigation
/** /**
* 订单映射最大订单ID. * 订单映射最大订单ID.
*/ */
private static int currentMaxiOrderId = 25; private static int currentMaxiOrderId = 100;
/** /**
@ -78,7 +78,7 @@ public class HybridNavigation
Path[] paths = new Path[]{ Path[] paths = new Path[]{
new Path( new Path(
pathSerialNum, pathID, 0f, (byte) 0x00, (byte) 0x01, ByteUtils.usintTo1Byte(0), 1f, 1f, null pathSerialNum, pathID, 0f, (byte) 0x00, (byte) 0x00, ByteUtils.usintTo1Byte(0), 1f, 1f, null
), ),
}; };
pathSerialNum += 2; pathSerialNum += 2;

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

@ -35,7 +35,7 @@ public class ReadValue extends BaseCommand{
RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent); RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
if (rcv.isOk()) { if (rcv.isOk()) {
System.out.println(); System.out.println();
System.out.println("-received " + "isok:" + rcv.isOk() + " dataBytes:"); System.out.println("-received readvalue" + "isok:" + rcv.isOk() + " dataBytes:");
printInfo(rcv); printInfo(rcv);
ReadRsp readRsp = new ReadRsp(rcv.getDataBytes()); ReadRsp readRsp = new ReadRsp(rcv.getDataBytes());
return readRsp.dataValue; return readRsp.dataValue;

4
opentcs-common/src/main/java/org/opentcs/kc/udp/io/UDPClient.java
View File

@ -154,10 +154,6 @@ public enum UDPClient {
public void subscribeKC(RcvEventPackage rcv, byte[] body) { public void subscribeKC(RcvEventPackage rcv, byte[] body) {
if (rcv.isOk()) { if (rcv.isOk()) {
Date date = new Date();
System.out.println("time:" + date.getTime());
for (byte b : body) { for (byte b : body) {
System.out.print(byteToHex(b) + " "); System.out.print(byteToHex(b) + " ");
} }