add:增加状态

src/main/java/com/iflytop/gd/hardware/drivers/DIDriver/InputDetectDriver.java

@@ -1,44 +1,38 @@
-//package com.iflytop.gd.hardware.drivers.DIDriver;
-//
-//import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
-//import com.iflytop.gd.hardware.exception.HardwareException;
-//import com.iflytop.gd.hardware.type.ModuleType;
-//import lombok.RequiredArgsConstructor;
-//import lombok.extern.slf4j.Slf4j;
-//import org.springframework.stereotype.Component;
-//
-//@Slf4j
-//@Component
-//@RequiredArgsConstructor
-//public class InputDetectDriver {
-//
-//	private final A8kCanBusService a8kCanBusService;
-//
-//	public Boolean getIOState(InputIOId ioid) throws HardwareException {
-//		while (true) {
-//			Boolean firstReadIO  = priGetIOState(ioid);
-//			Boolean secondReadIO = priGetIOState(ioid);
-//			if (firstReadIO == secondReadIO) {
-//				if (ioid.mirror) {
-//					return !firstReadIO;
-//				}
-//				return firstReadIO;
-//			} else {
-//				log.warn("getIOState {} not match, retry", ioid);
-//			}
-//		}
-//	}
-//
-//
-//	private Boolean priGetIOState(InputIOId ioid) throws HardwareException {
-//		if (ioid.mtype == ModuleType.Board) {
-//			return a8kCanBusService.callcmd(ioid.mid, CmdId.extboard_read_inio, ioid.ioIndex).getContentI32(0) != 0;
-//		} else if (ioid.mtype == ModuleType.TMCStepMotor) {
-//			return a8kCanBusService.callcmd(ioid.mid, CmdId.step_motor_read_io_state, ioid.ioIndex).getContentI32(0) != 0;
-//		} else if (ioid.mtype == ModuleType.MiniServo) {
-//			return a8kCanBusService.callcmd(ioid.mid, CmdId.mini_servo_read_io_state, ioid.ioIndex).getContentI32(0) != 0;
-//		} else {
-//			throw new HardwareException(new AECodeError(String.format("IOID MODULE TYPE %s NOT SUPPORT", ioid.mtype)));
-//		}
-//	}
-//}
+package com.iflytop.gd.hardware.drivers.DIDriver;
+
+import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
+import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.CmdId;
+import com.iflytop.gd.hardware.type.IO.InputIOMId;
+import com.iflytop.gd.hardware.type.ModuleType;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class InputDetectDriver {
+
+	private final A8kCanBusService canBus;
+
+	public Boolean getIOState(InputIOMId ioid) throws HardwareException {
+		while (true) {
+			Boolean firstReadIO  = priGetIOState(ioid);
+			Boolean secondReadIO = priGetIOState(ioid);
+			if (firstReadIO == secondReadIO) {
+				if (ioid.mirror) {
+					return !firstReadIO;
+				}
+				return firstReadIO;
+			} else {
+				log.warn("getIOState {} not match, retry", ioid);
+			}
+		}
+	}
+
+
+	private Boolean priGetIOState(InputIOMId ioid) throws HardwareException {
+         return canBus.callcmd(ioid.mid, CmdId.write_out_io, ioid.ioIndex).getContentI32(0) != 0;
+	}
+}

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

@@ -4,6 +4,7 @@ import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
 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 lombok.RequiredArgsConstructor;
@@ -33,10 +34,10 @@ public class LeisaiServoDriver {
         return packet.getContentI32(0);
     }
 
-    public Integer readIoState(LeisaiServoMId id, Integer io_index) throws HardwareException {
+    public Boolean readIoState(LeisaiServoMId id, Integer io_index) throws HardwareException {
         log.info("readIoState called with id: {}, io_index: {}", id, io_index);
         A8kPacket packet = canBus.callcmd(id.mid, CmdId.leisai_servo_read_io_state, io_index);
-        return packet.getContentI32(0);
+        return packet.getContentI32(0)  != 0;
     }
 
     public void moveTo(LeisaiServoMId id, LeisaiServoSpeedLevel speedLevel, Integer pos) throws HardwareException {
@@ -58,4 +59,15 @@ public class LeisaiServoDriver {
         log.info("moveToZero called with id: {}", id);
         canBus.callcmd(id.mid, CmdId.leisai_servo_move_to_zero);
     }
+
+    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);
+    }
+
+
+    public Integer getReg(LeisaiServoMId id, LeisaiRegIndex regindex) throws HardwareException {
+        log.info("getReg called with id: {}, regindex: {}", id, regindex);
+        return canBus.moduleGetReg(id.mid, regindex.regIndex);
+    }
 }

src/main/java/com/iflytop/gd/hardware/service/GDDeviceStatusService.java

@@ -0,0 +1,90 @@
+package com.iflytop.gd.hardware.service;
+
+import com.iflytop.gd.hardware.drivers.DIDriver.InputDetectDriver;
+import com.iflytop.gd.hardware.drivers.HeaterRodDriver;
+import com.iflytop.gd.hardware.drivers.LeisaiServoDriver;
+import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoDriver;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorCtrlDriver;
+import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.IO.InputIOMId;
+import com.iflytop.gd.hardware.type.Servo.LeisaiServoMId;
+import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId;
+import com.iflytop.gd.hardware.type.driver.HeaterRodSlavedId;
+import com.iflytop.gd.hardware.utils.Math.StepMotorConverter;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class GDDeviceStatusService {
+    private final InputDetectDriver inputDetectDriver;
+    private final HeaterRodDriver heaterRodDriver;
+    private final StepMotorCtrlDriver stepMotorCtrlDriver;
+    private final MiniServoDriver miniServoDriver;
+    private final LeisaiServoDriver leisaiServoDriver;
+
+    /**
+     * 获取步进电机的原点状态
+     * @param stepMotorMId
+     * @return true 在原点 false 不在原点
+     */
+    public Boolean stepMotorIsOrigin(StepMotorMId stepMotorMId) throws HardwareException {
+        return stepMotorCtrlDriver.stepMotorReadIoState(stepMotorMId, 0);
+    }
+
+    /**
+     * 获取XY 伺服的原点状态
+     * @param mid
+     * @return
+     * @throws HardwareException
+     */
+    public Boolean getXYServoIsOrigin(LeisaiServoMId mid) throws HardwareException
+    {
+        return leisaiServoDriver.readIoState(mid, 0);
+    }
+
+    /**
+     * 获取 拍子 试管架拍子 急停 到位信号
+     * @param inputIOMId
+     * @return
+     * @throws HardwareException
+     */
+    public Boolean getInputState(InputIOMId inputIOMId) throws HardwareException {
+        return inputDetectDriver.getIOState(inputIOMId);
+    }
+
+    /**
+     * 获取加热棒温度
+     * @param heaterRodSlavedId
+     * @return
+     * @throws Exception
+     */
+    public Double getHeaterRodTemperature(HeaterRodSlavedId heaterRodSlavedId) throws Exception {
+        return heaterRodDriver.getTemperature(heaterRodSlavedId);
+    }
+
+    /**
+     * 获取电机位置
+     * @param stepMotorMId
+     * @return
+     * @throws HardwareException
+     */
+    public Double getStepMotorPostion(StepMotorMId stepMotorMId) throws HardwareException {
+        Integer motorPosition = stepMotorCtrlDriver.stepMotorReadPos(stepMotorMId);
+        Double realPosition = StepMotorConverter.toUserPosition(motorPosition);
+        return realPosition;
+    }
+
+    /**
+     * 获取伺服类电机位置
+     * @param mid
+     * @return
+     * @throws HardwareException
+     */
+    public Double getXYServoPosition(LeisaiServoMId mid) throws HardwareException {
+        int servoPosition = leisaiServoDriver.readPosition(mid);
+        return (double)servoPosition;
+    }
+}

src/main/java/com/iflytop/gd/hardware/type/IO/InputIOMId.java

@@ -0,0 +1,39 @@
+package com.iflytop.gd.hardware.type.IO;
+
+import com.iflytop.gd.hardware.type.MId;
+
+public enum InputIOMId {
+    Heater_CAP1_EXSIT("Heater_CAP1_EXSIT [ 加热位拍子1到位 ]", MId.IO1_IO, 0,false),
+    Heater_CAP2_EXSIT("Heater_CAP2_EXSIT [ 加热位拍子2到位 ]", MId.IO1_IO, 1,false),
+    Heater_CAP3_EXSIT("Heater_CAP3_EXSIT [ 加热位拍子3到位 ]", MId.IO1_IO, 2,false),
+    Heater_CAP4_EXSIT("Heater_CAP4_EXSIT [ 加热位拍子4到位 ]", MId.IO1_IO, 3,false),
+    Heater_CAP5_EXSIT("Heater_CAP5_EXSIT [ 加热位拍子5到位 ]", MId.IO1_IO, 4,false),
+    Heater_CAP6_EXSIT("Heater_CAP6_EXSIT [ 加热位拍子6到位 ]", MId.IO1_IO, 5,false),
+    Heater_TUBE1_EXSIT("Heater_TUBE1_EXSIT [ 加热位试管架1到位 ]", MId.IO1_IO, 6,false),
+    Heater_TUBE2_EXSIT("Heater_TUBE2_EXSIT [ 加热位试管架2到位 ]", MId.IO1_IO, 7,false),
+    Heater_TUBE3_EXSIT("Heater_TUBE3_EXSIT [ 加热位试管架3到位 ]", MId.IO1_IO, 8,false),
+    Heater_TUBE4_EXSIT("Heater_TUBE4_EXSIT [ 加热位试管架4到位 ]", MId.IO1_IO, 9,false),
+    Heater_TUBE5_EXSIT("Heater_TUBE5_EXSIT [ 加热位试管架5到位 ]", MId.IO1_IO, 10,false),
+    Heater_TUBE6_EXSIT("Heater_TUBE6_EXSIT [ 加热位试管架6到位 ]", MId.IO1_IO, 11,false),
+    TRAY_CAP1_EXSIT("TRAY_CAP1_EXSIT [ 拍子存放位拍子1到位 ]", MId.IO1_IO, 12,false),
+    TRAY_CAP2_EXSIT("TRAY_CAP2_EXSIT [ 拍子存放位拍子2到位 ]", MId.IO1_IO, 13,false),
+    TRAY_CAP3_EXSIT("TRAY_CAP3_EXSIT [ 拍子存放位拍子3到位 ]", MId.IO1_IO, 14,false),
+    TRAY_CAP4_EXSIT("TRAY_CAP4_EXSIT [ 拍子存放位拍子4到位 ]", MId.IO1_IO, 15,false),
+    TRAY_CAP5_EXSIT("TRAY_CAP5_EXSIT [ 拍子存放位拍子5到位 ]", MId.IO1_IO, 16,false),
+    TRAY_CAP6_EXSIT("TRAY_CAP6_EXSIT [ 拍子存放位拍子6到位 ]", MId.IO1_IO, 17,false),
+    E_STOP("E_STOP [ 急停 ]", MId.IO1_IO, 18,false),
+    ;
+
+    final public String description;
+    final public MId        mid;
+    final public int        ioIndex;
+    final public boolean    mirror;
+
+    InputIOMId(String description, MId mid, int ioIndex, boolean mirror) {
+        this.description = description;
+        this.mid     = mid;
+        this.ioIndex = ioIndex;
+        this.mirror  = mirror;
+
+    }
+}

src/main/java/com/iflytop/gd/hardware/type/MId.java

@@ -8,14 +8,14 @@ public enum MId {
     IO1Board(5, "台面 IO 板模块"),
     PWMLight(6, "PWM 灯"),
     TriColorLight(7, "三色灯"),
-    HBotClawS(8, "夹爪舵机"),//
-    DualRobotAxis1S(9, "双轴机械臂1舵机"),
-    DualRobotAxis2S(10, "双轴机械臂2舵机"),
+    HBotClawSV(8, "夹爪舵机"),//
+    DualRobotAxis1SV(9, "双轴机械臂1舵机"),
+    DualRobotAxis2SV(10, "双轴机械臂2舵机"),
     IO1_IO(11, "台面 IO 板模块"),
     IO1_ADC(12, "台面 ADC 板模块"),
     LiquidDistributionArm(13, "加液臂"),
-    HBotXLeisaiSV(14, "X轴Leisai舵机"),//
-    HBotYLeisaiSV(15, "Y轴Leisai舵机"),//
+    MainXSV(14, "X轴Leisai舵机"),//
+    MainYSV(15, "Y轴Leisai舵机"),//
 
 //	IO2Board(25, "台下 IO 板模块"),
 

src/main/java/com/iflytop/gd/hardware/type/RegIndex.java

@@ -145,8 +145,14 @@ public enum RegIndex {
     kreg_liquid_distribution_arm_pos15_d0(10640),  // 位置15 舵机0位置
     kreg_liquid_distribution_arm_pos15_d1(10641),  // 位置15 舵机1位置
     kreg_liquid_distribution_arm_pos16_d0(10642),  // 位置16 舵机0位置
-    kreg_liquid_distribution_arm_pos16_d1(10643);  // 位置16 舵机1位置
-
+    kreg_liquid_distribution_arm_pos16_d1(10643),  // 位置16 舵机1位置
+
+    kreg_leisai_servo_is_enable(10702), // 是否使能
+    kreg_leisai_servo_has_move_zero(10703), // 是否回零
+    kreg_leisai_servo_default_velocity(10704), // 默认速度
+    kreg_leisai_servo_low_velocity(10705), // 低速
+    kreg_leisai_servo_mid_velocity(10706), // 中速
+    kreg_leisai_servo_high_velocity(10707); // 高速
     ;
 
     public final int     index;
@@ -172,3 +178,4 @@ public enum RegIndex {
         return null;
     }
 }
+

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

@@ -3,7 +3,12 @@ package com.iflytop.gd.hardware.type.Servo;
 import com.iflytop.gd.hardware.type.RegIndex;
 
 public enum LeisaiRegIndex {
-    kreg_mini_servo_firmware_main_version(RegIndex.kreg_mini_servo_firmware_main_version),
+    kreg_leisai_servo_is_enable(RegIndex.kreg_leisai_servo_is_enable),
+    kreg_leisai_servo_has_move_zero(RegIndex.kreg_leisai_servo_has_move_zero),
+    kreg_leisai_servo_default_velocity(RegIndex.kreg_leisai_servo_default_velocity),
+    kreg_leisai_servo_low_velocity(RegIndex.kreg_leisai_servo_low_velocity),
+    kreg_leisai_servo_mid_velocity(RegIndex.kreg_leisai_servo_mid_velocity),
+    kreg_leisai_servo_high_velocity(RegIndex.kreg_leisai_servo_high_velocity),
     ;
 
     public final RegIndex regIndex;

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.HBotXLeisaiSV),
-    HBotYLeisaiSV(MId.HBotYLeisaiSV),
+    HBotXLeisaiSV(MId.MainXSV),
+    HBotYLeisaiSV(MId.MainYSV),
     ;
 
     final public MId mid;

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

@@ -4,9 +4,9 @@ import com.iflytop.gd.hardware.type.MId;
 
 public enum MiniServoMId {
 	NotSet(MId.NotSet),//
-	DUAL_ROBOT_AXIS1_MID(MId.DualRobotAxis1S),//
-	DUAL_ROBOT_AXIS2_MID(MId.DualRobotAxis2S),
-	CLAW_MID(MId.HBotClawS)
+	DUAL_ROBOT_AXIS1_MID(MId.DualRobotAxis1SV),//
+	DUAL_ROBOT_AXIS2_MID(MId.DualRobotAxis2SV),
+	CLAW_MID(MId.HBotClawSV)
 	;
 
 	final public MId mid;