feat:实现虚拟电机功能

src/main/java/com/iflytop/gd/infrastructure/devices/StandardStepMotor.java

@@ -12,6 +12,7 @@ import com.iflytop.gd.system.exceptions.HardwareErrorException;
 import com.iflytop.gd.system.models.DataPacket;
 
 import java.io.IOException;
+import java.util.Map;
 import java.util.concurrent.TimeUnit;
 
 /**
@@ -47,22 +48,27 @@ public class StandardStepMotor implements StepMotor {
     }
 
     @Override
-    public void setCurrentPosition(Integer value) {
+    public void easyMoveToZeroPointQuick() {
 
     }
 
     @Override
-    public void easyMoveToZeroPointQuick() {
+    public void enable() {
 
     }
 
     @Override
-    public void easyMoveToEndPoint() {
+    public void disable() {
 
     }
 
     @Override
-    public void enable() {
+    public void moveForward(RotationDirection direction, Integer distance, DistanceUnit unit) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+
+    }
+
+    @Override
+    public void moveBackward(RotationDirection direction, Integer distance, DistanceUnit unit) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
 
     }
 
@@ -76,86 +82,94 @@ public class StandardStepMotor implements StepMotor {
         return 0;
     }
 
+
     @Override
-    public void easyRotate(RotationDirection direction) {
+    public void stop() {
 
     }
 
     @Override
-    public void easyMoveByBlock() {
+    public void rotateForward() {
 
     }
 
     @Override
-    public void easyMoveToBlock() {
+    public void rotateBackward() {
 
     }
 
-    @Override
-    public void easyMoveToZeroBlock() {
 
+    @Override
+    public Map<String, Boolean> readIOState() {
+        return null;
     }
 
+
     @Override
-    public void readPositionByMoveToZeroBlock() {
+    public void setReg(RegIndex regIndex, Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        DataPacket dataPacket = DataPacket.createCommandDataPacket(moduleId.index, CmdId.module_set_reg.index, value);
+        commandBus.waitForCommandExec(dataPacket, DEFAULT_COMMAND_EXEC_TIMEOUT_SECONDS, TimeUnit.SECONDS);
+    }
 
+    @Override
+    public Integer readReg(RegIndex regIndex) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        DataPacket commandDataPacket = DataPacket.createCommandDataPacket(moduleId.index, CmdId.module_get_reg.index);
+        DataPacket ackDataPacket = commandBus.waitForCommandExec(commandDataPacket, DEFAULT_COMMAND_EXEC_TIMEOUT_SECONDS, TimeUnit.SECONDS);
+        return ackDataPacket.getContentI32(0);
     }
 
     @Override
-    public void easyMoveToZeroPointQuickBlock() {
+    public void setMRes(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
 
     }
 
     @Override
-    public void stop() {
+    public void setIRun(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
 
     }
 
     @Override
-    public void moveByBlock() {
+    public void setIHold(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
 
     }
 
     @Override
-    public void moveTOBlock() {
+    public void setStartAndStopVelocity(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
 
     }
 
     @Override
-    public void rotate() {
+    public void setV1(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
 
     }
 
     @Override
-    public Boolean readIOState() {
-        return null;
+    public void setA1AndD1(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+
     }
 
     @Override
-    public void easyReciprocatingMotion() {
+    public void setAmaxAndDmax(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
 
     }
 
     @Override
-    public void easyReciprocatingMotionBlock() {
+    public void setDefaultVelocity(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
 
     }
 
     @Override
-    public Boolean isReturnToZero() {
-        return null;
+    public void setVelocity(Integer low, Integer mid, Integer high) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+
     }
 
     @Override
-    public void setReg(RegIndex regIndex, Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
-        DataPacket dataPacket = DataPacket.createCommandDataPacket(moduleId.index, CmdId.module_set_reg.index, value);
-        commandBus.waitForCommandExec(dataPacket, DEFAULT_COMMAND_EXEC_TIMEOUT_SECONDS, TimeUnit.SECONDS);
+    public void setOneCyclePulse(Integer pause, Integer denominator) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+
     }
 
     @Override
-    public Integer readReg(RegIndex regIndex) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
-        DataPacket commandDataPacket = DataPacket.createCommandDataPacket(moduleId.index, CmdId.module_get_reg.index);
-        DataPacket ackDataPacket = commandBus.waitForCommandExec(commandDataPacket, DEFAULT_COMMAND_EXEC_TIMEOUT_SECONDS, TimeUnit.SECONDS);
-        return ackDataPacket.getContentI32(0);
+    public void setDZero(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+
     }
 }

src/main/java/com/iflytop/gd/infrastructure/devices/VirtualStepMotor.java

@@ -0,0 +1,170 @@
+package com.iflytop.gd.infrastructure.devices;
+
+import com.iflytop.gd.infrastructure.drivers.RegIndex;
+import com.iflytop.gd.system.constants.DistanceUnit;
+import com.iflytop.gd.system.constants.RotationDirection;
+import com.iflytop.gd.system.devices.StepMotor;
+import com.iflytop.gd.system.exceptions.CommandExecTimeoutException;
+import com.iflytop.gd.system.exceptions.HardwareErrorException;
+import com.iflytop.gd.system.models.MotorStatus;
+
+import java.io.IOException;
+import java.util.HashMap;
+import java.util.Map;
+
+public class VirtualStepMotor implements StepMotor {
+    private MotorStatus motorStatus;
+    private Map<RegIndex, Integer> registers = new HashMap<RegIndex, Integer>();
+
+    @Override
+    public void easyMoveBy(Integer value, DistanceUnit unit) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        int position = this.motorStatus.getCurrentPosition() + unit.toMM(value);
+        this.motorStatus.setCurrentPosition(position);
+        this.motorStatus.setStopped(true);
+    }
+
+    @Override
+    public void easyMoveTo(Integer value, DistanceUnit unit) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        this.motorStatus.setCurrentPosition(unit.toMM(value));
+        this.motorStatus.setZeroPosition(this.motorStatus.getCurrentPosition() == 0);
+        this.motorStatus.setStopped(true);
+    }
+
+    @Override
+    public void easyMoveToZero() throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        this.motorStatus.setCurrentPosition(0);
+        this.motorStatus.setZeroPosition(this.motorStatus.getCurrentPosition() == 0);
+        this.motorStatus.setStopped(true);
+    }
+
+    @Override
+    public void easyMoveToZeroPointQuick() {
+        this.motorStatus.setCurrentPosition(0);
+        this.motorStatus.setZeroPosition(this.motorStatus.getCurrentPosition() == 0);
+        this.motorStatus.setStopped(true);
+    }
+
+    @Override
+    public void enable() {
+        this.motorStatus.setEnabled(true);
+    }
+
+    @Override
+    public void disable() {
+        this.motorStatus.setEnabled(false);
+    }
+
+    @Override
+    public void moveForward(RotationDirection direction, Integer distance, DistanceUnit unit) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+
+    }
+
+    @Override
+    public void moveBackward(RotationDirection direction, Integer distance, DistanceUnit unit) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+
+    }
+
+    @Override
+    public void stop() {
+        this.motorStatus.setStopped(true);
+    }
+
+    @Override
+    public void rotateForward() {
+
+    }
+
+    @Override
+    public void rotateBackward() {
+
+    }
+
+    @Override
+    public Map<String, Boolean> readIOState() {
+        Map<String, Boolean> ioState = new HashMap<>();
+        ioState.put("IO1", true);
+        ioState.put("IO2", false);
+        return ioState;
+    }
+
+    @Override
+    public Integer readPosition() {
+        return this.motorStatus.getCurrentPosition();
+    }
+
+    @Override
+    public Integer readEncoderPosition() {
+        return this.motorStatus.getEncoderPosition();
+    }
+
+    @Override
+    public void setReg(RegIndex regIndex, Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        this.registers.put(regIndex, value);
+    }
+
+    @Override
+    public Integer readReg(RegIndex regIndex) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        return this.registers.get(regIndex);
+    }
+
+    @Override
+    public void setMRes(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_mres, value);
+    }
+
+    @Override
+    public void setIRun(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_irun, value);
+    }
+
+    @Override
+    public void setIHold(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_ihold, value);
+    }
+
+    @Override
+    public void setStartAndStopVelocity(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_vstart, value);
+        setReg(RegIndex.kreg_step_motor_vstop, value);
+    }
+
+    @Override
+    public void setV1(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_v1, value);
+    }
+
+    @Override
+    public void setA1AndD1(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_a1, value);
+        setReg(RegIndex.kreg_step_motor_d1, value);
+    }
+
+    @Override
+    public void setAmaxAndDmax(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_amax, value);
+        setReg(RegIndex.kreg_step_motor_dmax, value);
+    }
+
+    @Override
+    public void setDefaultVelocity(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_default_velocity, value);
+    }
+
+    @Override
+    public void setVelocity(Integer low, Integer mid, Integer high) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_low_velocity, low);
+        setReg(RegIndex.kreg_step_motor_mid_velocity, mid);
+        setReg(RegIndex.kreg_step_motor_high_velocity, high);
+    }
+
+    @Override
+    public void setOneCyclePulse(Integer pause, Integer denominator) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_one_circle_pulse, pause);
+        setReg(RegIndex.kreg_step_motor_one_circle_pulse_denominator, denominator);
+    }
+
+    @Override
+    public void setDZero(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        setReg(RegIndex.kreg_step_motor_dzero_pos, value);
+    }
+}

src/main/java/com/iflytop/gd/system/devices/StepMotor.java

@@ -7,6 +7,7 @@ import com.iflytop.gd.system.exceptions.CommandExecTimeoutException;
 import com.iflytop.gd.system.exceptions.HardwareErrorException;
 
 import java.io.IOException;
+import java.util.Map;
 
 /**
  * 电机驱动类型设备
@@ -14,28 +15,99 @@ import java.io.IOException;
 public interface StepMotor {
 
     /**
-     * 移动指定距离
+     * 相对距离移动
      * @param value
      * @param unit
      */
     void easyMoveBy(Integer value, DistanceUnit unit) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
+
+    /**
+     * 绝对距离移动
+     * @param value
+     * @param unit
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
     void easyMoveTo(Integer value, DistanceUnit unit) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
+    /**
+     * 归零
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
     void easyMoveToZero() throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    void setCurrentPosition(Integer value);
-
+    /**
+     * 快速归零
+     */
     void easyMoveToZeroPointQuick();
 
-    void easyMoveToEndPoint();
-
     /**
      * 使能电机
      */
     void enable();
 
     /**
+     * 失能电机
+     */
+    void disable();
+
+
+    /**
+     * 正向移动
+     * @param direction
+     * @param distance
+     * @param unit
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void moveForward(RotationDirection direction, Integer distance, DistanceUnit unit)
+            throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
+
+    /**
+     * 反向移动
+     * @param direction
+     * @param distance
+     * @param unit
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void moveBackward(RotationDirection direction, Integer distance, DistanceUnit unit)
+            throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
+
+    /**
+     * 停止电机
+     */
+    void stop();
+
+    /**
+     * 正向旋转
+     */
+    void rotateForward();
+
+    /**
+     * 反向旋转
+     */
+    void rotateBackward();
+
+    /**
+     * 读取IO状态
+     *
+     * @return
+     */
+    Map<String, Boolean> readIOState();
+
+
+    /**
      * 读取当前位置
      * @return
      */
@@ -47,54 +119,143 @@ public interface StepMotor {
      */
     Integer readEncoderPosition();
 
-    void easyRotate(RotationDirection direction);
 
-    void easyMoveByBlock();
+    /*************************寄存器相关开始*******************/
 
-    void easyMoveToBlock();
+    /**
+     * 设置模块寄存器值
+     * @param regIndex
+     * @param value
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setReg(RegIndex regIndex, Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    void easyMoveToZeroBlock();
 
-    void readPositionByMoveToZeroBlock();
+    /**
+     * 读取模块寄存器值
+     * @param regIndex
+     * @return
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    Integer readReg(RegIndex regIndex) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    void easyMoveToZeroPointQuickBlock();
+    /**
+     * 设置细分
+     */
+    void setMRes(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    void stop();
 
-    void moveByBlock();
+    /**
+     * 设置电机电流
+     * @param value
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setIRun(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    void moveTOBlock();
 
-    void rotate();
+    /**
+     * 设置电机电流
+     * @param value
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setIHold(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    Boolean readIOState();
+    /**
+     * 设置启动速度
+     * @param value
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setStartAndStopVelocity(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    void easyReciprocatingMotion();
+    /**
+     * 设置V1
+     * @param value
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setV1(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    void easyReciprocatingMotionBlock();
+    /**
+     * 设置加速度
+     * @param value
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setA1AndD1(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    Boolean isReturnToZero();
 
     /**
-     * 设置模块寄存器值
-     * @param regIndex
+     * 设置最大加速度
      * @param value
      * @throws HardwareErrorException
      * @throws CommandExecTimeoutException
      * @throws IOException
      * @throws InterruptedException
      */
-    void setReg(RegIndex regIndex, Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
+    void setAmaxAndDmax(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
+
 
 
     /**
-     * 读取模块寄存器值
-     * @param regIndex
-     * @return
+     * 设置默认速度值
      * @throws HardwareErrorException
      * @throws CommandExecTimeoutException
      * @throws IOException
      * @throws InterruptedException
      */
-    Integer readReg(RegIndex regIndex) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
+    void setDefaultVelocity(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
+
+    /**
+     * 设置速度
+     * @param low
+     * @param mid
+     * @param high
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setVelocity(Integer low, Integer mid, Integer high) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
+
+
+    /**
+     * 设置转动一圈脉冲数
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setOneCyclePulse(Integer pause, Integer denominator) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
+
+    /**
+     * 设置DZero
+     * @param value
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setDZero(Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
+
+
+    /*************************寄存器相关结束*******************/
 }

src/main/java/com/iflytop/gd/system/models/MotorStatus.java

@@ -1,5 +1,16 @@
 package com.iflytop.gd.system.models;
 
+import lombok.Getter;
+import lombok.Setter;
+
+@Getter
+@Setter
 public class MotorStatus {
-    private Integer speed;
+    private boolean stopped = false;
+    private boolean isEnabled = false;
+    private Integer currentSpeed;
+    private Integer currentPosition;
+    private Integer encoderPosition;
+    private boolean isZeroPosition = true;
+    private boolean isLimitPosition = false;
 }