diff --git a/src/main/java/com/iflytop/gd/infrastructure/devices/StandardStepMotor.java b/src/main/java/com/iflytop/gd/infrastructure/devices/StandardStepMotor.java index aa4091e..90f2337 100644 --- a/src/main/java/com/iflytop/gd/infrastructure/devices/StandardStepMotor.java +++ b/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 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 { + } } diff --git a/src/main/java/com/iflytop/gd/infrastructure/devices/VirtualStepMotor.java b/src/main/java/com/iflytop/gd/infrastructure/devices/VirtualStepMotor.java new file mode 100644 index 0000000..d008fd5 --- /dev/null +++ b/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 registers = new HashMap(); + + @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 readIOState() { + Map 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); + } +} diff --git a/src/main/java/com/iflytop/gd/system/devices/StepMotor.java b/src/main/java/com/iflytop/gd/system/devices/StepMotor.java index 00c4685..eba216c 100644 --- a/src/main/java/com/iflytop/gd/system/devices/StepMotor.java +++ b/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 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; + + + /*************************寄存器相关结束*******************/ } diff --git a/src/main/java/com/iflytop/gd/system/models/MotorStatus.java b/src/main/java/com/iflytop/gd/system/models/MotorStatus.java index b045ba1..d4a882a 100644 --- a/src/main/java/com/iflytop/gd/system/models/MotorStatus.java +++ b/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; }