HSZ_HeSongZhen
/
Sub_Spray_Java
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

修复急停问题

master
HSZ_HeSongZhen 2 weeks ago
parent
1dfec26bd3
commit
87d40dc266
4 changed files with 60 additions and 30 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      src/main/java/org/mgc/serial_comm/SerialComm.java
  2. 18
      src/main/java/org/mgc/serial_comm/SerialCommCtrl.java
  3. 64
      src/main/java/org/mgc/server/command/command_handler/MotorDevice.java
  4. 6
      src/main/java/org/mgc/service/DeviceCommandManager.java

2
src/main/java/org/mgc/serial_comm/SerialComm.java
View File

@ -66,7 +66,7 @@ public class SerialComm {
}
// 延时 1ms
try {
Thread.sleep(5);
Thread.sleep(10);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
log.error("Sleep interrupted", e);

18
src/main/java/org/mgc/serial_comm/SerialCommCtrl.java
View File

@ -17,7 +17,7 @@ public class SerialCommCtrl {
private static final int THREAD_POOL_SIZE = 5; // 线程池大小
// linux 485 com ttyS1
private final SerialComm serialComm = new SerialComm("ttyS1", 115200, 8, 1, 0);
// private final SerialComm serialComm = new SerialComm("COM17", 115200, 8, 1, 0);
// private final SerialComm serialComm = new SerialComm("COM7", 115200, 8, 1, 0);
private final ExecutorService scheduler = Executors.newFixedThreadPool(THREAD_POOL_SIZE);;
private boolean isRunning = false;
@ -139,10 +139,22 @@ public class SerialCommCtrl {
processQueue(waitQueue);
}
}
private void processQueue(Queue<Integer> waitQueue) {
public void pushEvent(BoardEvent event)
{
Queue<Integer> waitQueue = getKeyByValue(event);
processQueue(waitQueue); // 打印queue
if (waitQueue.isEmpty()) {
log.warn("No commands waiting for event: " + event.toString());
} else {
log.info("Commands waiting for event: " + event.toString() + " - " + waitQueue);
}
}
public void processQueue(Queue<Integer> waitQueue) {
for (Integer cmdID : new ArrayList<>(waitQueue)) {
SerialResult result = new SerialResult(FrameSlaveIdInfo.commonSlaveID, cmdID, 0);
SubSprayAppSingleton.getInstance().handleDeviceSerialMessageEvent(result);
eventWaitCmd.remove(cmdID);
}

64
src/main/java/org/mgc/server/command/command_handler/MotorDevice.java
View File

@ -21,6 +21,10 @@ import org.mgc.utils.StringHelper;
import java.io.IOException;
import java.security.InvalidParameterException;
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.Queue;
import java.util.concurrent.ExecutionException;
@Slf4j
@ -120,15 +124,18 @@ public class MotorDevice extends BaseCommandHandler {
if (motorDirect == null || motorDirect == MotorDirect.MotorBackward) {
throw new InvalidParameterException("Motor [Direction]");
}
int timeout = param.has("timeout") ? param.get("timeout").asInt() : MOVE_TIMEOUT_INTERVAL;
double absPosition = Math.abs(position);
position = motorDirect == MotorDirect.MotorForward ? absPosition : -absPosition;
res = moveMotorToPosition(cmdId, axis_, position);
res = moveMotorToPosition(cmdId, axis_, position, timeout);
log.info("[Motor {}] move To Position {}", axis_.getAxisValue(), position);
}
break;
}
case MotorActionOrigin: {
res = motorMoveToHome(cmdId, axis_);
JsonNode param = command.getParam();
int timeout = param != null && param.has("timeout") ? param.get("timeout").asInt() : HOME_TIMEOUT_INTERVAL;
res = motorMoveToHome(cmdId, axis_, timeout);
log.info("[Motor {}] Move to Home {}", axis_.getAxisValue(), axis_);
break;
}
@ -149,16 +156,16 @@ public class MotorDevice extends BaseCommandHandler {
}
}
public int motorMoveToHome(int cmdId, MotorAxis axis) {
public int motorMoveToHome(int cmdId, MotorAxis axis, int timeout) {
int res = -1;
SerialCommand motorMoveHomeCommand = DeviceCommandGenerator.ctrlWaitCmdControl(cmdId, getMotorSlavedId(),
MOVE_HOME_ADDRESS, true, getMoveHomeFinishedEvent());
SerialCommand motorMoveHomeCommand = DeviceCommandGenerator.ctrlWaitCmdControl(cmdId, getMotorSlavedId(axis),
MOVE_HOME_ADDRESS, true, getMoveHomeFinishedEvent(axis));
try {
// 发送 485 指令
DeviceCommandManager deviceCommandManager = SubSprayAppSingleton.getInstance().getDeviceCommandManager();;
CommandFuture motorMoveHomeFuture = deviceCommandManager.sendCommand(motorMoveHomeCommand);
// 等待 30 s
commandWait(HOME_TIMEOUT_INTERVAL, motorMoveHomeFuture);
commandWait(timeout, motorMoveHomeFuture);
// 485 指令 转 data
SerialResult result = motorMoveHomeFuture.getAckFuture().get();
@ -178,7 +185,7 @@ public class MotorDevice extends BaseCommandHandler {
// TODO Open Device
// 拼 485 指令
int res = 0;
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveSingleRegCmdControl(cmdId, getMotorSlavedId(), I_RUN_ADDRESS,
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveSingleRegCmdControl(cmdId, getMotorSlavedId(axis), I_RUN_ADDRESS,
current);
try {
// 发送 485 指令
@ -201,7 +208,7 @@ public class MotorDevice extends BaseCommandHandler {
return res;
}
public int moveMotorToPosition(int cmdId, MotorAxis axis, double position) {
public int moveMotorToPosition(int cmdId, MotorAxis axis, double position, int timeout) {
// 拼 485 指令
double pos_limit = MotorAxis.getPosLimit(axis);
if(Math.abs(position) > pos_limit)
@ -214,14 +221,14 @@ public class MotorDevice extends BaseCommandHandler {
int res = 0;
short[] shortArray = IntToShortArray.intToShortArrayBigEndian(motorPos);
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlWaitMultiRegCmdControl(cmdId, getMotorSlavedId(), V_TARGET_POSITION_ADDRESS,
shortArray, getMoveFinishedEvent());
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlWaitMultiRegCmdControl(cmdId, getMotorSlavedId(axis), V_TARGET_POSITION_ADDRESS,
shortArray, getMoveFinishedEvent(axis));
try {
// 发送 485 指令
DeviceCommandManager deviceCommandManager = SubSprayAppSingleton.getInstance().getDeviceCommandManager();;
CommandFuture ctrlCommandFuture = deviceCommandManager.sendCommand(ctrlCommand);
// 等待
commandWait(MOVE_TIMEOUT_INTERVAL, ctrlCommandFuture);
commandWait(timeout, ctrlCommandFuture);
// 485 指令 转 data
SerialResult result = ctrlCommandFuture.getAckFuture().get();
@ -259,7 +266,7 @@ public class MotorDevice extends BaseCommandHandler {
// 拼 485 指令
int res = 0;
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveMultiRegCmdControl(cmdId, getMotorSlavedId(), V_SPEED_ADDRESS,
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveMultiRegCmdControl(cmdId, getMotorSlavedId(axis), V_SPEED_ADDRESS,
shortArray);
try {
// 发送 485 指令
@ -287,9 +294,9 @@ public class MotorDevice extends BaseCommandHandler {
return 0;
}
int getMotorSlavedId()
int getMotorSlavedId(MotorAxis axis)
{
switch (axis_)
switch (axis)
{
case X -> {
return FrameSlaveIdInfo.MotorXSlavaID;
@ -304,24 +311,24 @@ public class MotorDevice extends BaseCommandHandler {
return 0;
}
BoardEvent getMoveFinishedEvent() {
switch (axis_)
BoardEvent getMoveFinishedEvent(MotorAxis axis) {
switch (axis)
{
case X -> {
return BoardEvent.MOTOR_X_MOVE_FINISHED;
}
case Y -> {
case X -> {
return BoardEvent.MOTOR_X_MOVE_FINISHED;
}
case Y -> {
return BoardEvent.MOTOR_Y_MOVE_FINISHED;
}
case Z -> {
}
case Z -> {
return BoardEvent.MOTOR_Z_MOVE_FINISHED;
}
}
}
return null;
}
BoardEvent getMoveHomeFinishedEvent() {
switch (axis_)
BoardEvent getMoveHomeFinishedEvent(MotorAxis axis) {
switch (axis)
{
case X -> {
return BoardEvent.MOTOR_X_HOME_FINISHED;
@ -339,7 +346,7 @@ public class MotorDevice extends BaseCommandHandler {
public int stopMotor(int cmdId, MotorAxis axis) {
// 拼 485 指令
int res = 0;
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveCmdControl(cmdId, getMotorSlavedId(), STOP_ADDRESS,
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveCmdControl(cmdId, getMotorSlavedId(axis), STOP_ADDRESS,
true);
try {
// 发送 485 指令
@ -351,6 +358,10 @@ public class MotorDevice extends BaseCommandHandler {
// 485 指令 转 data
SerialResult result = ctrlCommandFuture.getAckFuture().get();
res = result.getCode();
deviceCommandManager.pushEvent(getMoveHomeFinishedEvent(axis));
deviceCommandManager.pushEvent(getMoveFinishedEvent(axis));
log.info("\nMotor stopped\n");
// 返回data
} catch (IOException e) {
res = -1;
@ -359,6 +370,7 @@ public class MotorDevice extends BaseCommandHandler {
} catch (InterruptedException e) {
res = -1;
}
return res;
}

6
src/main/java/org/mgc/service/DeviceCommandManager.java
View File

@ -1,10 +1,12 @@
package org.mgc.service;
import lombok.extern.slf4j.Slf4j;
import org.mgc.serial_comm.BoardEvent;
import org.mgc.serial_comm.SerialCommCtrl;
import org.mgc.server.CommandFuture;
import java.io.IOException;
import java.util.Queue;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.ConcurrentMap;
@ -44,6 +46,10 @@ public class DeviceCommandManager {
return commandFuture;
}
public void pushEvent(BoardEvent event) {
serialComm.pushEvent(event);
}
public void handleDeviceResult(SerialResult deviceResult) {
completeCommandAck(deviceResult);
}

Write Preview
Loading…
Cancel
Save