增加夹爪指令支持

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<Device, MiniServoMId> servoMIdMap_ = Map.ofEntries(
+            Map.entry(Device.CLAW, MiniServoMId.RobotArmClawSV)
+    );
+    private final Map<Device, MId> supportCmdDeviceMIdMap = servoMIdMap_.entrySet().stream().
+            collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+    private final Set<Action> supportActions = Set.of(Action.SET, Action.ORIGIN, Action.MOVE, Action.STOP);
+
+    @Override
+    protected Map<Device, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<Action> 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);
+            }
+        }
+    }
+}

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<String, String> getDeviceList() {
         Map<String, String> 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<String, Integer> getAllReg(@RequestParam DeviceServoId deviceId) {
+    public Map<String, Integer> getAllReg(@RequestParam MiniServoId deviceId) {
         return servoService.getAllReg(deviceId);
     }
 }

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());
+    }
+}

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<String, Integer> getAllReg(DeviceServoId id) {
+    public Map<String, Integer> getAllReg(MiniServoId id) {
         Map<String, Integer> 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);

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;

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;

src/main/java/com/iflytop/colortitration/hardware/type/Servo/DeviceServoId.java → 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;
             }