fixed: 修改 HBOt

src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java

@@ -3,12 +3,15 @@ package com.iflytop.gd.hardware.command.handlers;
 import com.iflytop.gd.common.cmd.DeviceCommand;
 import com.iflytop.gd.common.enums.cmd.CmdAction;
 import com.iflytop.gd.common.enums.cmd.CmdDevice;
-import com.iflytop.gd.common.enums.cmd.CmdDirection;
 import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.LeisaiServoDriver;
 import com.iflytop.gd.hardware.drivers.StepMotorDriver.HBotDriver;
 import com.iflytop.gd.hardware.type.IO.OutputIOMId;
+import com.iflytop.gd.hardware.type.Servo.LeisaiServoMId;
+import com.iflytop.gd.hardware.type.Servo.LeisaiServoSpeedLevel;
 import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId;
 import com.iflytop.gd.hardware.type.MId;
+import com.iflytop.gd.hardware.utils.Math.StepMotorConverter;
 import lombok.RequiredArgsConstructor;
 import lombok.extern.slf4j.Slf4j;
 import org.springframework.stereotype.Component;
@@ -22,6 +25,7 @@ import java.util.stream.Collectors;
 @RequiredArgsConstructor
 public class HBotHandler extends CommandHandler {
     private final HBotDriver driver_;
+    private final LeisaiServoDriver servoDriver_;
 
     private final Map<CmdDevice, OutputIOMId>  supportCmdDeviceIOOutputMap = Map.ofEntries(
             Map.entry(CmdDevice.gantry_z, OutputIOMId.DO_HBOTZ_MOTOR_CLAMP)
@@ -32,6 +36,13 @@ public class HBotHandler extends CommandHandler {
             Map.entry(CmdDevice.gantry_y, StepMotorMId.HBOT_Y_MOTOR_MID),
             Map.entry(CmdDevice.gantry_z, StepMotorMId.HBOT_Z_MOTOR_MID)
     );
+
+
+
+    private final Map<CmdDevice, LeisaiServoMId> leisaiServoMIdMap = Map.ofEntries(
+            Map.entry(CmdDevice.gantry_y, LeisaiServoMId.MainXSV),
+            Map.entry(CmdDevice.gantry_z, LeisaiServoMId.MainYSV)
+    );
     private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream().
             collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
     private final Set<CmdAction> supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.move_by, CmdAction.stop);
@@ -51,26 +62,70 @@ public class HBotHandler extends CommandHandler {
     public void handleCommand(DeviceCommand command) throws Exception {
         // 发送命令
         StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
+        LeisaiServoMId servoMId = leisaiServoMIdMap.get(command.getDevice());
+
+
+        boolean isServo = command.getDevice() != CmdDevice.gantry_z;
 
         OutputIOMId clampMId = supportCmdDeviceIOOutputMap.get(command.getDevice());
 
         if (command.getAction() == CmdAction.origin) {
-            driver_.moveToHome(stepMotorMId);
+            driver_.openClamp(clampMId);
+            if(isServo)
+            {
+                log.info("HBotHandler origin servo");
+                servoDriver_.moveToZero(servoMId);
+            }
+            else
+            {
+                driver_.moveToHome(stepMotorMId);
+
+            }
+            driver_.closeClamp(clampMId);
+
         }
         else if(command.getAction() == CmdAction.stop) {
-            driver_.stop(stepMotorMId);
+            driver_.openClamp(clampMId);
+            if(isServo)
+            {
+                log.info("HBotHandler stop servo");
+                servoDriver_.moduleStop(servoMId);
+            }
+            else
+            {
+                driver_.stop(stepMotorMId);
+            }
+            driver_.closeClamp(clampMId);
         }
         else if(command.getAction() == CmdAction.move) {
             driver_.openClamp(clampMId);
-            double position = command.getParam().getPosition();
-            driver_.moveTo(stepMotorMId, position);
+            if(isServo)
+            {
+                double userPos = command.getParam().getPosition();
+                int motor_Pos = StepMotorConverter.toMotorPosition(userPos);
+                log.info("HBotHandler move to userPos: {}, motor_Pos: {}", userPos, motor_Pos);
+                servoDriver_.moveTo(servoMId, LeisaiServoSpeedLevel.DEFAULT, motor_Pos);
+            }
+            else
+            {
+                driver_.moveTo(stepMotorMId, command.getParam().getPosition());
+            }
             driver_.closeClamp(clampMId);
         }
         else if(command.getAction() == CmdAction.move_by) {
             driver_.openClamp(clampMId);
             double distance = command.getParam().getPosition();
 
-            driver_.moveBy(stepMotorMId, distance);
+            if(isServo)
+            {
+                int motor_Pos = StepMotorConverter.toMotorPosition(distance);
+                log.info("HBotHandler move by distance: {}, motor_Pos: {}", distance, motor_Pos);
+                servoDriver_.moveBy(servoMId, LeisaiServoSpeedLevel.DEFAULT, motor_Pos);
+            }
+            else
+            {
+                driver_.moveBy(stepMotorMId, distance);
+            }
             driver_.closeClamp(clampMId);
         }
         else if(command.getAction() == CmdAction.set) {

src/main/java/com/iflytop/gd/hardware/type/Servo/LeisaiServoMId.java

@@ -4,8 +4,8 @@ import com.iflytop.gd.hardware.type.MId;
 import org.springframework.util.Assert;
 
 public enum LeisaiServoMId {
-    HBotXLeisaiSV(MId.MainXSV),
-    HBotYLeisaiSV(MId.MainYSV),
+    MainXSV(MId.MainXSV),
+    MainYSV(MId.MainYSV),
     ;
 
     final public MId mid;

src/main/resources/application-dev.yml

@@ -36,7 +36,7 @@ command_bus:
 device.enableCanBus: true
 
 iflytophald:
-  ip: 192.168.1.140
+  ip: 192.168.8.168
   cmdch.port: 19004
   datach.port: 19005