同步代码

src/main/java/com/iflytop/gd/app/cmd/debug/LiquidArmRotateCommandHandler.java

@@ -49,7 +49,7 @@ public class LiquidArmRotateCommandHandler extends BaseCommandHandler {
                 CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand);
                 futuresList.add(deviceCommandFuture);
             }
-            if (largeArmAngle != null) {
+            if (smallArmAngle != null) {
                 DeviceCommandBundle deviceCommand = DeviceCommandGenerator.dualRobotJoint2Move(smallArmAngle);
                 CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand);
                 futuresList.add(deviceCommandFuture);

src/main/java/com/iflytop/gd/hardware/HardwareService.java

@@ -81,9 +81,11 @@ public class HardwareService {
             String errorStr =  e.getError().toString();
             log.error("Hardware ERROR {}", errorStr);
             response.setSuccess(Boolean.FALSE);
+            response.setErrorMsg(errorStr);
         } catch (Exception e) {
             log.error("指令执行失败: {}", e.getMessage(), e);
             response.setSuccess(Boolean.FALSE);
+            response.setErrorMsg(e.getMessage());
         }
         finally {
             JSONObject jsonResponse = JSONUtil.parseObj(response);

src/main/java/com/iflytop/gd/hardware/command/DeviceResponse.java

@@ -7,5 +7,6 @@ import lombok.Data;
 public class DeviceResponse {
     Integer cmdId;
     Boolean success;
+    String errorMsg;
     JSONObject data;
 }

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

@@ -28,7 +28,7 @@ public class DualRobotHandler extends CommandHandler {
     );
     private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = servoMIdMap_.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.stop);
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop, CmdAction.move_joint);
 
     @Override
     protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()

src/main/java/com/iflytop/gd/hardware/drivers/LeisaiServoDriver.java

@@ -1,12 +1,14 @@
 package com.iflytop.gd.hardware.drivers;
 
 import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
+import com.iflytop.gd.hardware.constants.ActionOvertimeConstant;
 import com.iflytop.gd.hardware.exception.HardwareException;
 import com.iflytop.gd.hardware.type.A8kPacket;
 import com.iflytop.gd.hardware.type.CmdId;
 import com.iflytop.gd.hardware.type.Servo.LeisaiRegIndex;
 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 lombok.RequiredArgsConstructor;
 import lombok.extern.slf4j.Slf4j;
 import org.springframework.stereotype.Component;
@@ -16,6 +18,7 @@ import org.springframework.stereotype.Component;
 @RequiredArgsConstructor
 public class LeisaiServoDriver {
     final private A8kCanBusService canBus;
+    private final ActionOvertimeConstant actionOvertimeConstant;
     static public final Integer          actionOvertime = 10000;
     
     public void moduleStop(LeisaiServoMId id) throws HardwareException {
@@ -60,6 +63,29 @@ public class LeisaiServoDriver {
         canBus.callcmd(id.mid, CmdId.leisai_servo_move_to_zero);
     }
 
+    // ====== ===== ====== ===== ====== ===== Block ====== ===== ====== ===== ====== =====
+    public void moveToBlock(LeisaiServoMId id, LeisaiServoSpeedLevel speedLevel, Integer pos) throws HardwareException {
+        log.info("moveToBlock called with id: {}, speedLevel: {}, pos: {}", id, speedLevel, pos);
+        canBus.callcmd(id.mid, CmdId.leisai_servo_move_to, speedLevel.ordinal(), pos);
+        waitForMod(id, actionOvertimeConstant.get(id.mid, CmdId.leisai_servo_move_to));
+    }
+
+    public void moveByBlock(LeisaiServoMId id, LeisaiServoSpeedLevel speedLevel, Integer dpos) throws HardwareException {
+        log.info("moveByBlock called with id: {}, speedLevel: {}, dpos: {}", id, speedLevel, dpos);
+        canBus.callcmd(id.mid, CmdId.leisai_servo_move_by, speedLevel.ordinal(), dpos);
+        waitForMod(id, actionOvertimeConstant.get(id.mid, CmdId.leisai_servo_move_by));
+    }
+
+    public void moveToZeroBlock(LeisaiServoMId id) throws HardwareException {
+        log.info("moveToZeroBlock called with id: {}", id);
+        canBus.callcmd(id.mid, CmdId.leisai_servo_move_to_zero);
+        waitForMod(id, actionOvertimeConstant.get(id.mid, CmdId.leisai_servo_move_to_zero));
+    }
+
+    public void waitForMod(LeisaiServoMId id, Integer overtime) throws HardwareException {
+        canBus.waitForMod(id.mid, overtime);
+    }
+
     public void setReg(LeisaiServoMId id, LeisaiRegIndex regindex, int val) throws HardwareException {
         log.info("setReg called with id: {}, regindex: {}, val: {}", id, regindex, val);
         canBus.moduleSetReg(id.mid, regindex.regIndex, val);

src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DualRobotDriver.java

@@ -94,14 +94,14 @@ public class DualRobotDriver {
     }
 
     // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
-    private double getSpeed(MiniServoMId sevoMid)
+    private double getSpeed(MiniServoMId servoMid)
     {
         // TODO: 从数据库中获取门的速度
         return speed_;
     }
 
     // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ====
-    public void setSpeed(MiniServoMId sevoMid, double speed) {
+    public void setSpeed(MiniServoMId servoMid, double speed) {
         // 检查速度是否合法
         speed_ = speed;
         // TODO: 写入数据库
@@ -119,23 +119,32 @@ public class DualRobotDriver {
     }
 
     // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
-    public void moveToHome(MiniServoMId sevoMid) throws HardwareException
+    public void servoEnable(MiniServoMId servoMId, boolean enable) throws HardwareException
     {
-        log.info("Servo {} Move To Home", sevoMid.mid.getDescription());
-        miniServoDriver.miniServoMoveToZero(sevoMid);
+        miniServoDriver.miniServoEnable(servoMId, enable ? 1 : 0);
     }
     
-    public void moveJointTo(MiniServoMId sevoMid, double position) throws HardwareException
+    public void moveToHome(MiniServoMId servoMid) throws HardwareException
+    {
+        log.info("Servo {} Move To Home", servoMid.mid.getDescription());
+        this.servoEnable(servoMid, true);
+        miniServoDriver.miniServoMoveToZero(servoMid);
+        this.servoEnable(servoMid, false);
+    }
+
+    public void moveJointTo(MiniServoMId servoMid, double position) throws HardwareException
     {
         // 检查位置是否合法
         Integer servoPosition = toServoPosition(position);
-        log.info("Servo {} Move To position {}, Servo Position {}", sevoMid.mid.getDescription(), position, servoPosition);
-        miniServoDriver.miniServoMoveToBlock(sevoMid, servoPosition);
+        log.info("Servo {} Move To position {}, Servo Position {}", servoMid.mid.getDescription(), position, servoPosition);
+        this.servoEnable(servoMid, true);
+        miniServoDriver.miniServoMoveToBlock(servoMid, servoPosition);
+        this.servoEnable(servoMid, false);
     }
 
-    public void stop(MiniServoMId sevoMid) throws HardwareException {
-        log.info("Servo {} Stop", sevoMid.mid.getDescription());
-        miniServoDriver.miniServoStop(sevoMid);
+    public void stop(MiniServoMId servoMid) throws HardwareException {
+        log.info("Servo {} Stop", servoMid.mid.getDescription());
+        miniServoDriver.miniServoStop(servoMid);
     }
 
     // ==== ==== ==== ==== ==== ==== Application ==== ==== ==== ==== ==== ====