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
Unified 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 // 延时 1ms
try { try {
Thread.sleep(5);
Thread.sleep(10);
} catch (InterruptedException e) { } catch (InterruptedException e) {
Thread.currentThread().interrupt(); Thread.currentThread().interrupt();
log.error("Sleep interrupted", e); 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; // 线程池大小 private static final int THREAD_POOL_SIZE = 5; // 线程池大小
// linux 485 com ttyS1 // linux 485 com ttyS1
private final SerialComm serialComm = new SerialComm("ttyS1", 115200, 8, 1, 0); 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 final ExecutorService scheduler = Executors.newFixedThreadPool(THREAD_POOL_SIZE);;
private boolean isRunning = false; private boolean isRunning = false;
@ -139,10 +139,22 @@ public class SerialCommCtrl {
processQueue(waitQueue); 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)) { for (Integer cmdID : new ArrayList<>(waitQueue)) {
SerialResult result = new SerialResult(FrameSlaveIdInfo.commonSlaveID, cmdID, 0); SerialResult result = new SerialResult(FrameSlaveIdInfo.commonSlaveID, cmdID, 0);
SubSprayAppSingleton.getInstance().handleDeviceSerialMessageEvent(result); SubSprayAppSingleton.getInstance().handleDeviceSerialMessageEvent(result);
eventWaitCmd.remove(cmdID); 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.io.IOException;
import java.security.InvalidParameterException; 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; import java.util.concurrent.ExecutionException;
@Slf4j @Slf4j
@ -120,15 +124,18 @@ public class MotorDevice extends BaseCommandHandler {
if (motorDirect == null || motorDirect == MotorDirect.MotorBackward) { if (motorDirect == null || motorDirect == MotorDirect.MotorBackward) {
throw new InvalidParameterException("Motor [Direction]"); throw new InvalidParameterException("Motor [Direction]");
} }
int timeout = param.has("timeout") ? param.get("timeout").asInt() : MOVE_TIMEOUT_INTERVAL;
double absPosition = Math.abs(position); double absPosition = Math.abs(position);
position = motorDirect == MotorDirect.MotorForward ? absPosition : -absPosition; 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); log.info("[Motor {}] move To Position {}", axis_.getAxisValue(), position);
} }
break; break;
} }
case MotorActionOrigin: { 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_); log.info("[Motor {}] Move to Home {}", axis_.getAxisValue(), axis_);
break; 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; 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 { try {
// 发送 485 指令 // 发送 485 指令
DeviceCommandManager deviceCommandManager = SubSprayAppSingleton.getInstance().getDeviceCommandManager();; DeviceCommandManager deviceCommandManager = SubSprayAppSingleton.getInstance().getDeviceCommandManager();;
CommandFuture motorMoveHomeFuture = deviceCommandManager.sendCommand(motorMoveHomeCommand); CommandFuture motorMoveHomeFuture = deviceCommandManager.sendCommand(motorMoveHomeCommand);
// 等待 30 s // 等待 30 s
commandWait(HOME_TIMEOUT_INTERVAL, motorMoveHomeFuture);
commandWait(timeout, motorMoveHomeFuture);
// 485 指令 转 data // 485 指令 转 data
SerialResult result = motorMoveHomeFuture.getAckFuture().get(); SerialResult result = motorMoveHomeFuture.getAckFuture().get();
@ -178,7 +185,7 @@ public class MotorDevice extends BaseCommandHandler {
// TODO Open Device // TODO Open Device
// 拼 485 指令 // 拼 485 指令
int res = 0; int res = 0;
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveSingleRegCmdControl(cmdId, getMotorSlavedId(), I_RUN_ADDRESS,
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveSingleRegCmdControl(cmdId, getMotorSlavedId(axis), I_RUN_ADDRESS,
current); current);
try { try {
// 发送 485 指令 // 发送 485 指令
@ -201,7 +208,7 @@ public class MotorDevice extends BaseCommandHandler {
return res; return res;
} }
public int moveMotorToPosition(int cmdId, MotorAxis axis, double position) {
public int moveMotorToPosition(int cmdId, MotorAxis axis, double position, int timeout) {
// 拼 485 指令 // 拼 485 指令
double pos_limit = MotorAxis.getPosLimit(axis); double pos_limit = MotorAxis.getPosLimit(axis);
if(Math.abs(position) > pos_limit) if(Math.abs(position) > pos_limit)
@ -214,14 +221,14 @@ public class MotorDevice extends BaseCommandHandler {
int res = 0; int res = 0;
short[] shortArray = IntToShortArray.intToShortArrayBigEndian(motorPos); 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 { try {
// 发送 485 指令 // 发送 485 指令
DeviceCommandManager deviceCommandManager = SubSprayAppSingleton.getInstance().getDeviceCommandManager();; DeviceCommandManager deviceCommandManager = SubSprayAppSingleton.getInstance().getDeviceCommandManager();;
CommandFuture ctrlCommandFuture = deviceCommandManager.sendCommand(ctrlCommand); CommandFuture ctrlCommandFuture = deviceCommandManager.sendCommand(ctrlCommand);
// 等待 // 等待
commandWait(MOVE_TIMEOUT_INTERVAL, ctrlCommandFuture);
commandWait(timeout, ctrlCommandFuture);
// 485 指令 转 data // 485 指令 转 data
SerialResult result = ctrlCommandFuture.getAckFuture().get(); SerialResult result = ctrlCommandFuture.getAckFuture().get();
@ -259,7 +266,7 @@ public class MotorDevice extends BaseCommandHandler {
// 拼 485 指令 // 拼 485 指令
int res = 0; int res = 0;
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveMultiRegCmdControl(cmdId, getMotorSlavedId(), V_SPEED_ADDRESS,
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveMultiRegCmdControl(cmdId, getMotorSlavedId(axis), V_SPEED_ADDRESS,
shortArray); shortArray);
try { try {
// 发送 485 指令 // 发送 485 指令
@ -287,9 +294,9 @@ public class MotorDevice extends BaseCommandHandler {
return 0; return 0;
} }
int getMotorSlavedId()
int getMotorSlavedId(MotorAxis axis)
{ {
switch (axis_)
switch (axis)
{ {
case X -> { case X -> {
return FrameSlaveIdInfo.MotorXSlavaID; return FrameSlaveIdInfo.MotorXSlavaID;
@ -304,24 +311,24 @@ public class MotorDevice extends BaseCommandHandler {
return 0; 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; return BoardEvent.MOTOR_Y_MOVE_FINISHED;
}
case Z -> {
}
case Z -> {
return BoardEvent.MOTOR_Z_MOVE_FINISHED; return BoardEvent.MOTOR_Z_MOVE_FINISHED;
}
}
} }
return null; return null;
} }
BoardEvent getMoveHomeFinishedEvent() {
switch (axis_)
BoardEvent getMoveHomeFinishedEvent(MotorAxis axis) {
switch (axis)
{ {
case X -> { case X -> {
return BoardEvent.MOTOR_X_HOME_FINISHED; return BoardEvent.MOTOR_X_HOME_FINISHED;
@ -339,7 +346,7 @@ public class MotorDevice extends BaseCommandHandler {
public int stopMotor(int cmdId, MotorAxis axis) { public int stopMotor(int cmdId, MotorAxis axis) {
// 拼 485 指令 // 拼 485 指令
int res = 0; int res = 0;
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveCmdControl(cmdId, getMotorSlavedId(), STOP_ADDRESS,
SerialCommand ctrlCommand = DeviceCommandGenerator.ctrlSlaveCmdControl(cmdId, getMotorSlavedId(axis), STOP_ADDRESS,
true); true);
try { try {
// 发送 485 指令 // 发送 485 指令
@ -351,6 +358,10 @@ public class MotorDevice extends BaseCommandHandler {
// 485 指令 转 data // 485 指令 转 data
SerialResult result = ctrlCommandFuture.getAckFuture().get(); SerialResult result = ctrlCommandFuture.getAckFuture().get();
res = result.getCode(); res = result.getCode();
deviceCommandManager.pushEvent(getMoveHomeFinishedEvent(axis));
deviceCommandManager.pushEvent(getMoveFinishedEvent(axis));
log.info("\nMotor stopped\n");
// 返回data // 返回data
} catch (IOException e) { } catch (IOException e) {
res = -1; res = -1;
@ -359,6 +370,7 @@ public class MotorDevice extends BaseCommandHandler {
} catch (InterruptedException e) { } catch (InterruptedException e) {
res = -1; res = -1;
} }
return res; return res;
} }

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

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

Write Preview
Loading…
Cancel
Save