diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/LiquidArmRotateCommandHandler.java b/src/main/java/com/iflytop/gd/app/cmd/debug/LiquidArmRotateCommandHandler.java index e249c2b..80c7702 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/LiquidArmRotateCommandHandler.java +++ b/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); diff --git a/src/main/java/com/iflytop/gd/hardware/HardwareService.java b/src/main/java/com/iflytop/gd/hardware/HardwareService.java index eb358c0..275b7d7 100644 --- a/src/main/java/com/iflytop/gd/hardware/HardwareService.java +++ b/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); diff --git a/src/main/java/com/iflytop/gd/hardware/command/DeviceResponse.java b/src/main/java/com/iflytop/gd/hardware/command/DeviceResponse.java index dd38291..da49955 100644 --- a/src/main/java/com/iflytop/gd/hardware/command/DeviceResponse.java +++ b/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; } diff --git a/src/main/java/com/iflytop/gd/hardware/command/handlers/DualRobotHandler.java b/src/main/java/com/iflytop/gd/hardware/command/handlers/DualRobotHandler.java index e1ac17c..05e48de 100644 --- a/src/main/java/com/iflytop/gd/hardware/command/handlers/DualRobotHandler.java +++ b/src/main/java/com/iflytop/gd/hardware/command/handlers/DualRobotHandler.java @@ -28,7 +28,7 @@ public class DualRobotHandler extends CommandHandler { ); private final Map supportCmdDeviceMIdMap = servoMIdMap_.entrySet().stream(). collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid)); - private final Set supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop); + private final Set supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop, CmdAction.move_joint); @Override protected Map getSupportCmdDeviceMIdMap() diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/LeisaiServoDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/LeisaiServoDriver.java index b4023c0..94101d4 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/LeisaiServoDriver.java +++ b/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); diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DualRobotDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DualRobotDriver.java index 41757f0..b63b30e 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DualRobotDriver.java +++ b/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 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 sevoMid, double position) throws HardwareException + 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 ==== ==== ==== ==== ==== ====