From 8c0b7839a60da1dd3e99443e67cc23d0edd73c00 Mon Sep 17 00:00:00 2001 From: HSZ_HeSongZhen <210202959@qq.com> Date: Thu, 24 Jul 2025 09:48:18 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=A4=B9=E7=88=AA=E6=8C=87?= =?UTF-8?q?=E4=BB=A4=E6=94=AF=E6=8C=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../hardware/command/handlers/ClawHandler.java | 61 ++++++++++++++++++++++ .../hardware/controller/ServoController.java | 32 ++++++------ .../hardware/drivers/MiniServoDriverWrapper.java | 35 +++++++++++++ .../hardware/service/ServoService.java | 28 +++++----- .../hardware/type/Servo/DeviceServoId.java | 33 ------------ .../hardware/type/Servo/LeisaiServoMId.java | 2 - .../hardware/type/Servo/LiquidArmMId.java | 1 - .../hardware/type/Servo/MiniServoId.java | 31 +++++++++++ 8 files changed, 157 insertions(+), 66 deletions(-) create mode 100644 src/main/java/com/iflytop/colortitration/hardware/command/handlers/ClawHandler.java create mode 100644 src/main/java/com/iflytop/colortitration/hardware/drivers/MiniServoDriverWrapper.java delete mode 100644 src/main/java/com/iflytop/colortitration/hardware/type/Servo/DeviceServoId.java create mode 100644 src/main/java/com/iflytop/colortitration/hardware/type/Servo/MiniServoId.java diff --git a/src/main/java/com/iflytop/colortitration/hardware/command/handlers/ClawHandler.java b/src/main/java/com/iflytop/colortitration/hardware/command/handlers/ClawHandler.java new file mode 100644 index 0000000..1578372 --- /dev/null +++ b/src/main/java/com/iflytop/colortitration/hardware/command/handlers/ClawHandler.java @@ -0,0 +1,61 @@ +package com.iflytop.colortitration.hardware.command.handlers; + + +import com.iflytop.colortitration.app.core.command.DeviceCommand; +import com.iflytop.colortitration.common.enums.Action; +import com.iflytop.colortitration.common.enums.Device; +import com.iflytop.colortitration.hardware.command.CommandHandler; +import com.iflytop.colortitration.hardware.drivers.MiniServoDriverWrapper; +import com.iflytop.colortitration.hardware.type.MId; +import com.iflytop.colortitration.hardware.type.Servo.MiniServoMId; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Component; + +import java.util.Map; +import java.util.Set; +import java.util.stream.Collectors; + +@Slf4j +@Component +@RequiredArgsConstructor +public class ClawHandler extends CommandHandler { + private final MiniServoDriverWrapper driver_; + + private final Map servoMIdMap_ = Map.ofEntries( + Map.entry(Device.CLAW, MiniServoMId.RobotArmClawSV) + ); + private final Map supportCmdDeviceMIdMap = servoMIdMap_.entrySet().stream(). + collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid)); + private final Set supportActions = Set.of(Action.SET, Action.ORIGIN, Action.MOVE, Action.STOP); + + @Override + protected Map getSupportCmdDeviceMIdMap() + { + return supportCmdDeviceMIdMap; + } + + @Override + protected Set getSupportActions() { + return supportActions; + } + + @Override + public void handleCommand(DeviceCommand command) throws Exception { + // 发送命令 + MiniServoMId servoMId = servoMIdMap_.get(command.getDevice()); + if(command.getAction() == Action.STOP) { + driver_.stop(servoMId); + } + else if(command.getAction() == Action.MOVE) { + double position = command.getParam().getPosition(); + driver_.moveTo(servoMId, position); + } + else if(command.getAction() == Action.SET) { + Double speed = command.getParam().getSpeed(); + if(speed != null) { + driver_.setSpeed(servoMId, speed); + } + } + } +} diff --git a/src/main/java/com/iflytop/colortitration/hardware/controller/ServoController.java b/src/main/java/com/iflytop/colortitration/hardware/controller/ServoController.java index c26d9fb..e918392 100644 --- a/src/main/java/com/iflytop/colortitration/hardware/controller/ServoController.java +++ b/src/main/java/com/iflytop/colortitration/hardware/controller/ServoController.java @@ -4,7 +4,7 @@ import com.iflytop.colortitration.app.service.DeviceParamConfigService; import com.iflytop.colortitration.common.result.Result; import com.iflytop.colortitration.hardware.exception.HardwareException; import com.iflytop.colortitration.hardware.service.ServoService; -import com.iflytop.colortitration.hardware.type.Servo.DeviceServoId; +import com.iflytop.colortitration.hardware.type.Servo.MiniServoId; import com.iflytop.colortitration.hardware.type.Servo.MiniServoRegIndex; import io.swagger.v3.oas.annotations.Operation; import io.swagger.v3.oas.annotations.tags.Tag; @@ -31,7 +31,7 @@ public class ServoController { @Operation(summary = "获取设备列表") public Map getDeviceList() { Map map = new HashMap<>(); - for(DeviceServoId id : DeviceServoId.values()) { + for(MiniServoId id : MiniServoId.values()) { map.put(id.name(), id.getDescription()); } return map; @@ -51,55 +51,55 @@ public class ServoController { // 基础操作 @PostMapping("/enable") @Operation(summary = "电源开启") - public Result enable(@RequestParam DeviceServoId deviceId) throws HardwareException { + public Result enable(@RequestParam MiniServoId deviceId) throws HardwareException { servoService.enable(deviceId, true); return Result.success(); } @PostMapping("/disable") @Operation(summary = "电源关闭") - public Result disable(@RequestParam DeviceServoId deviceId) throws HardwareException { + public Result disable(@RequestParam MiniServoId deviceId) throws HardwareException { servoService.enable(deviceId, false); return Result.success(); } @PostMapping("/stop") @Operation(summary = "停止") - public Result stop(@RequestParam DeviceServoId deviceId) throws HardwareException { + public Result stop(@RequestParam MiniServoId deviceId) throws HardwareException { servoService.stop(deviceId); return Result.success(); } @PostMapping("/move-to") @Operation(summary = "移动(绝对)") - public Result moveTo(@RequestParam DeviceServoId deviceId, @RequestParam Integer pos) throws HardwareException { + public Result moveTo(@RequestParam MiniServoId deviceId, @RequestParam Integer pos) throws HardwareException { servoService.moveTo(deviceId, pos); return Result.success(); } @PostMapping("/rotate-forward") @Operation(summary = "正转") - public Result rotateForward(@RequestParam DeviceServoId deviceId) throws HardwareException { + public Result rotateForward(@RequestParam MiniServoId deviceId) throws HardwareException { servoService.rotate(deviceId, 1); return Result.success(); } @PostMapping("/rotate-backward") @Operation(summary = "反转") - public Result rotateBackward(@RequestParam DeviceServoId deviceId) throws HardwareException { + public Result rotateBackward(@RequestParam MiniServoId deviceId) throws HardwareException { servoService.rotate(deviceId, 0); return Result.success(); } @PostMapping("/rotate-with-torque") @Operation(summary = "旋转(力矩)") - public Result rotateWithTorque(@RequestParam DeviceServoId deviceId, @RequestParam Integer pos) throws HardwareException { + public Result rotateWithTorque(@RequestParam MiniServoId deviceId, @RequestParam Integer pos) throws HardwareException { servoService.rotateWithTorque(deviceId, pos); return Result.success(); } @PostMapping("/move-to-zero") @Operation(summary = "回Home") - public Result moveToZero(@RequestParam DeviceServoId deviceId) throws HardwareException { + public Result moveToZero(@RequestParam MiniServoId deviceId) throws HardwareException { servoService.moveToZero(deviceId); return Result.success(); } @@ -108,7 +108,7 @@ public class ServoController { @PostMapping("/limit-velocity") @Operation(summary = "设置限速") public Result setLimitVelocity( - @RequestParam DeviceServoId deviceId, + @RequestParam MiniServoId deviceId, @RequestParam Integer val) throws HardwareException { servoService.setLimitVelocity(deviceId, val); deviceParamConfigService.setModuleAndReg(deviceId.getServoMId().mid.name(), MiniServoRegIndex.kreg_mini_servo_limit_velocity.regIndex.name(),val); @@ -118,7 +118,7 @@ public class ServoController { @PostMapping("/limit-torque") @Operation(summary = "设置限力矩") public Result setLimitTorque( - @RequestParam DeviceServoId deviceId, + @RequestParam MiniServoId deviceId, @RequestParam Integer val) throws HardwareException { servoService.setLimitTorque(deviceId, val); deviceParamConfigService.setModuleAndReg(deviceId.getServoMId().mid.name(), MiniServoRegIndex.kreg_mini_servo_limit_torque.regIndex.name(),val); @@ -128,7 +128,7 @@ public class ServoController { @PostMapping("/protective-torque") @Operation(summary = "设置保护力矩") public Result setProtectiveTorque( - @RequestParam DeviceServoId deviceId, + @RequestParam MiniServoId deviceId, @RequestParam Integer val) throws HardwareException { servoService.setProtectiveTorque(deviceId, val); deviceParamConfigService.setModuleAndReg(deviceId.getServoMId().mid.name(), MiniServoRegIndex.kreg_mini_servo_protective_torque.regIndex.name(),val); @@ -138,7 +138,7 @@ public class ServoController { @PostMapping("/reg") @Operation(summary = "设置寄存器") public Result setReg( - @RequestParam DeviceServoId deviceId, + @RequestParam MiniServoId deviceId, @RequestParam MiniServoRegIndex reg, @RequestParam Integer val) throws HardwareException { servoService.setReg(deviceId, reg, val); @@ -149,13 +149,13 @@ public class ServoController { // 状态查询 @GetMapping("/position") @Operation(summary = "读取位置") - public Integer readPos(@RequestParam DeviceServoId deviceId) throws HardwareException { + public Integer readPos(@RequestParam MiniServoId deviceId) throws HardwareException { return servoService.readPos(deviceId); } @GetMapping("/regs") @Operation(summary = "获取所有寄存器") - public Map getAllReg(@RequestParam DeviceServoId deviceId) { + public Map getAllReg(@RequestParam MiniServoId deviceId) { return servoService.getAllReg(deviceId); } } diff --git a/src/main/java/com/iflytop/colortitration/hardware/drivers/MiniServoDriverWrapper.java b/src/main/java/com/iflytop/colortitration/hardware/drivers/MiniServoDriverWrapper.java new file mode 100644 index 0000000..84f7e65 --- /dev/null +++ b/src/main/java/com/iflytop/colortitration/hardware/drivers/MiniServoDriverWrapper.java @@ -0,0 +1,35 @@ +package com.iflytop.colortitration.hardware.drivers; + + +import com.iflytop.colortitration.hardware.exception.HardwareException; +import com.iflytop.colortitration.hardware.type.Servo.MiniServoMId; +import com.iflytop.colortitration.hardware.type.Servo.MiniServoRegIndex; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Component; +import static java.lang.Math.abs; + +@Slf4j +@Component +@RequiredArgsConstructor +public class MiniServoDriverWrapper { + private final MiniServoDriver miniServoDriver; + // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ==== + public void setSpeed(MiniServoMId servoMid, double speed) { + log.info("[ {} ] Setting speed to {}", servoMid.mid.getDescription(), speed); + } + + // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ==== + public void moveTo(MiniServoMId servoMid, double position) throws HardwareException + { + miniServoDriver.miniServoEnable(servoMid, 1); + int servoPosition = (int) (abs(position)); + miniServoDriver.miniServoMoveToBlock(servoMid, servoPosition); + log.info("[ {} ] moveTo {}, servo position {} end", servoMid, position, servoPosition); + } + + public void stop(MiniServoMId servoMid) throws HardwareException { + miniServoDriver.miniServoStop(servoMid); + log.info("[ {} ] stop", servoMid.mid.getDescription()); + } +} diff --git a/src/main/java/com/iflytop/colortitration/hardware/service/ServoService.java b/src/main/java/com/iflytop/colortitration/hardware/service/ServoService.java index 045db7a..3ff5815 100644 --- a/src/main/java/com/iflytop/colortitration/hardware/service/ServoService.java +++ b/src/main/java/com/iflytop/colortitration/hardware/service/ServoService.java @@ -2,7 +2,7 @@ package com.iflytop.colortitration.hardware.service; import com.iflytop.colortitration.hardware.drivers.MiniServoDriver.MiniServoDriver; import com.iflytop.colortitration.hardware.exception.HardwareException; -import com.iflytop.colortitration.hardware.type.Servo.DeviceServoId; +import com.iflytop.colortitration.hardware.type.Servo.MiniServoId; import com.iflytop.colortitration.hardware.type.Servo.MiniServoMId; import com.iflytop.colortitration.hardware.type.Servo.MiniServoRegIndex; import com.iflytop.colortitration.hardware.type.error.A8kEcode; @@ -18,12 +18,12 @@ import java.util.Map; public class ServoService { private final MiniServoDriver miniServoDriver; - private void throwDeviceNull(DeviceServoId id) throws HardwareException + private void throwDeviceNull(MiniServoId id) throws HardwareException { throw new HardwareException(new AppError(A8kEcode.PE_PARAM_OUT_OF_RANGE, "StepMotorMId is null for id: " + id)); } - public void enable(DeviceServoId id, Boolean enable) throws HardwareException { + public void enable(MiniServoId id, Boolean enable) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -31,7 +31,7 @@ public class ServoService { miniServoDriver.miniServoEnable(servoMId, enable ? 1 : 0); } - public void stop(DeviceServoId id) throws HardwareException { + public void stop(MiniServoId id) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -39,7 +39,7 @@ public class ServoService { miniServoDriver.moduleStop(servoMId); } - public void moveTo(DeviceServoId id, Integer pos) throws HardwareException { + public void moveTo(MiniServoId id, Integer pos) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -47,7 +47,7 @@ public class ServoService { miniServoDriver.miniServoMoveToBlock(servoMId, pos); } - public void rotate(DeviceServoId id, Integer direction) throws HardwareException { + public void rotate(MiniServoId id, Integer direction) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -55,7 +55,7 @@ public class ServoService { miniServoDriver.miniServoRotate(servoMId, direction); } - public void rotateWithTorque(DeviceServoId id, Integer pos) throws HardwareException { + public void rotateWithTorque(MiniServoId id, Integer pos) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -63,7 +63,7 @@ public class ServoService { miniServoDriver.miniServoRotateWithTorque(servoMId, pos); } - public void setReg(DeviceServoId id, MiniServoRegIndex reg, Integer val) throws HardwareException { + public void setReg(MiniServoId id, MiniServoRegIndex reg, Integer val) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -71,7 +71,7 @@ public class ServoService { miniServoDriver.setReg(servoMId, reg, val); } - public Map getAllReg(DeviceServoId id) { + public Map getAllReg(MiniServoId id) { Map map = new HashMap<>(); MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { @@ -87,7 +87,7 @@ public class ServoService { return map; } - public void setLimitVelocity(DeviceServoId id, Integer val) throws HardwareException { + public void setLimitVelocity(MiniServoId id, Integer val) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -95,7 +95,7 @@ public class ServoService { miniServoDriver.setReg(servoMId, MiniServoRegIndex.kreg_mini_servo_limit_velocity, val); } - public void setLimitTorque(DeviceServoId id, Integer val) throws HardwareException { + public void setLimitTorque(MiniServoId id, Integer val) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -103,7 +103,7 @@ public class ServoService { miniServoDriver.setReg(servoMId, MiniServoRegIndex.kreg_mini_servo_limit_torque, val); } - public void setProtectiveTorque(DeviceServoId id, Integer val) throws HardwareException { + public void setProtectiveTorque(MiniServoId id, Integer val) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -111,7 +111,7 @@ public class ServoService { miniServoDriver.setReg(servoMId, MiniServoRegIndex.kreg_mini_servo_protective_torque, val); } - public Integer readPos(DeviceServoId id) throws HardwareException { + public Integer readPos(MiniServoId id) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); @@ -119,7 +119,7 @@ public class ServoService { return miniServoDriver.miniServoReadPos(servoMId); } - public void moveToZero(DeviceServoId id) throws HardwareException { + public void moveToZero(MiniServoId id) throws HardwareException { MiniServoMId servoMId = id.getServoMId(); if (servoMId == null) { throwDeviceNull(id); diff --git a/src/main/java/com/iflytop/colortitration/hardware/type/Servo/DeviceServoId.java b/src/main/java/com/iflytop/colortitration/hardware/type/Servo/DeviceServoId.java deleted file mode 100644 index 1b630af..0000000 --- a/src/main/java/com/iflytop/colortitration/hardware/type/Servo/DeviceServoId.java +++ /dev/null @@ -1,33 +0,0 @@ -package com.iflytop.colortitration.hardware.type.Servo; - -public enum DeviceServoId { - dual_robot_axis1(MiniServoMId.DUAL_ROBOT_AXIS1_MID, "双轴械臂一轴"), - dual_robot_axis2(MiniServoMId.DUAL_ROBOT_AXIS2_MID, "双轴械臂二轴"), - claw(MiniServoMId.CLAW_MID, "夹爪"), - ; - - private MiniServoMId servoMId; - private String description; - - public MiniServoMId getServoMId() { - return servoMId; - } - - public String getDescription() { - return description; - } - - DeviceServoId(MiniServoMId servoMId, String description) { - this.servoMId = servoMId; - this.description = description; - } - - public static DeviceServoId getById(MiniServoMId servoMId) { - for (DeviceServoId deviceServoId : DeviceServoId.values()) { - if (deviceServoId.getServoMId() == servoMId) { - return deviceServoId; - } - } - return null; - } -} diff --git a/src/main/java/com/iflytop/colortitration/hardware/type/Servo/LeisaiServoMId.java b/src/main/java/com/iflytop/colortitration/hardware/type/Servo/LeisaiServoMId.java index 0c0a912..11b61a2 100644 --- a/src/main/java/com/iflytop/colortitration/hardware/type/Servo/LeisaiServoMId.java +++ b/src/main/java/com/iflytop/colortitration/hardware/type/Servo/LeisaiServoMId.java @@ -4,8 +4,6 @@ import com.iflytop.colortitration.hardware.type.MId; import org.springframework.util.Assert; public enum LeisaiServoMId { - MainXSV(MId.MainXSV), - MainYSV(MId.MainYSV), ; final public MId mid; diff --git a/src/main/java/com/iflytop/colortitration/hardware/type/Servo/LiquidArmMId.java b/src/main/java/com/iflytop/colortitration/hardware/type/Servo/LiquidArmMId.java index 2e865fc..978abfa 100644 --- a/src/main/java/com/iflytop/colortitration/hardware/type/Servo/LiquidArmMId.java +++ b/src/main/java/com/iflytop/colortitration/hardware/type/Servo/LiquidArmMId.java @@ -4,7 +4,6 @@ import com.iflytop.colortitration.hardware.type.MId; import org.springframework.util.Assert; public enum LiquidArmMId { - LiquidDistributionArm(MId.LiquidDistributionArm),// ; final public MId mid; diff --git a/src/main/java/com/iflytop/colortitration/hardware/type/Servo/MiniServoId.java b/src/main/java/com/iflytop/colortitration/hardware/type/Servo/MiniServoId.java new file mode 100644 index 0000000..a818e0d --- /dev/null +++ b/src/main/java/com/iflytop/colortitration/hardware/type/Servo/MiniServoId.java @@ -0,0 +1,31 @@ +package com.iflytop.colortitration.hardware.type.Servo; + +public enum MiniServoId { + claw(MiniServoMId.RobotArmClawSV, "夹爪"), + ; + + private MiniServoMId servoMId; + private String description; + + public MiniServoMId getServoMId() { + return servoMId; + } + + public String getDescription() { + return description; + } + + MiniServoId(MiniServoMId servoMId, String description) { + this.servoMId = servoMId; + this.description = description; + } + + public static MiniServoId getById(MiniServoMId servoMId) { + for (MiniServoId deviceServoId : MiniServoId.values()) { + if (deviceServoId.getServoMId() == servoMId) { + return deviceServoId; + } + } + return null; + } +}