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/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/DeviceServoId.java b/src/main/java/com/iflytop/colortitration/hardware/type/Servo/MiniServoId.java similarity index 54% rename from src/main/java/com/iflytop/colortitration/hardware/type/Servo/DeviceServoId.java rename to src/main/java/com/iflytop/colortitration/hardware/type/Servo/MiniServoId.java index 1b630af..a818e0d 100644 --- a/src/main/java/com/iflytop/colortitration/hardware/type/Servo/DeviceServoId.java +++ b/src/main/java/com/iflytop/colortitration/hardware/type/Servo/MiniServoId.java @@ -1,9 +1,7 @@ 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, "夹爪"), +public enum MiniServoId { + claw(MiniServoMId.RobotArmClawSV, "夹爪"), ; private MiniServoMId servoMId; @@ -17,13 +15,13 @@ public enum DeviceServoId { return description; } - DeviceServoId(MiniServoMId servoMId, String description) { + MiniServoId(MiniServoMId servoMId, String description) { this.servoMId = servoMId; this.description = description; } - public static DeviceServoId getById(MiniServoMId servoMId) { - for (DeviceServoId deviceServoId : DeviceServoId.values()) { + public static MiniServoId getById(MiniServoMId servoMId) { + for (MiniServoId deviceServoId : MiniServoId.values()) { if (deviceServoId.getServoMId() == servoMId) { return deviceServoId; }