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 c952578..0ed6a96 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 @@ -5,7 +5,6 @@ import com.iflytop.gd.common.cmd.DeviceCommandParams; import com.iflytop.gd.common.enums.cmd.CmdAction; import com.iflytop.gd.common.enums.cmd.CmdDevice; import com.iflytop.gd.hardware.command.CommandHandler; -import com.iflytop.gd.hardware.drivers.MiniServoDriver.ClawDriver; import com.iflytop.gd.hardware.drivers.MiniServoDriver.DualRobotDriver; import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoMId; import com.iflytop.gd.hardware.type.MId; @@ -50,7 +49,7 @@ public class DualRobotHandler extends CommandHandler { public void handleCommand(DeviceCommand command) throws Exception { // 发送命令 if (command.getAction() == CmdAction.origin) { - dualRobotDriver_.moveToHome(MiniServoMId.DUAL_ROBOT_AXIS_1_MID); + dualRobotDriver_.moveToHome(MiniServoMId.DUAL_ROBOT_AXIS1_MID); } else if(command.getAction() == CmdAction.stop) { dualRobotDriver_.stop(getJoint(command.getParam())); diff --git a/src/main/java/com/iflytop/gd/hardware/controller/ServoController.java b/src/main/java/com/iflytop/gd/hardware/controller/ServoController.java new file mode 100644 index 0000000..30698a7 --- /dev/null +++ b/src/main/java/com/iflytop/gd/hardware/controller/ServoController.java @@ -0,0 +1,118 @@ +package com.iflytop.gd.hardware.controller; + +import com.iflytop.gd.hardware.drivers.MiniServoDriver.DeviceServoId; +import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoRegIndex; +import com.iflytop.gd.hardware.exception.HardwareException; +import com.iflytop.gd.hardware.service.ServoService; +import io.swagger.v3.oas.annotations.Operation; +import io.swagger.v3.oas.annotations.tags.Tag; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.web.bind.annotation.*; + +import java.util.Map; + +@Tag(name = "伺服电机控制") +@RestController +@RequestMapping("/api/servo") +@RequiredArgsConstructor +@Slf4j +public class ServoController { + private final ServoService servoService; + + // 基础操作 + @PostMapping("/enable") + @Operation(summary = "电源开启") + public void enable(@RequestParam DeviceServoId deviceId) throws HardwareException { + servoService.enable(deviceId, true); + } + + @PostMapping("/disable") + @Operation(summary = "电源关闭") + public void disable(@RequestParam DeviceServoId deviceId) throws HardwareException { + servoService.enable(deviceId, false); + } + + @PostMapping("/stop") + @Operation(summary = "停止") + public void stop(@RequestParam DeviceServoId deviceId) throws HardwareException { + servoService.stop(deviceId); + } + + @PostMapping("/move_to") + @Operation(summary = "移动(绝对)") + public void moveTo(@RequestParam DeviceServoId deviceId, @RequestParam Integer pos) throws HardwareException { + servoService.moveTo(deviceId, pos); + } + + @PostMapping("/rotate_forward") + @Operation(summary = "正转") + public void rotate_forward(@RequestParam DeviceServoId deviceId) throws HardwareException { + servoService.rotate(deviceId, 1); + } + + @PostMapping("/rotate_backward") + @Operation(summary = "反转") + public void rotate_backward(@RequestParam DeviceServoId deviceId) throws HardwareException { + servoService.rotate(deviceId, 0); + } + + @PostMapping("/rotate_with_torque") + @Operation(summary = "旋转(力矩)") + public void rotateWithTorque(@RequestParam DeviceServoId deviceId, @RequestParam Integer pos) throws HardwareException { + servoService.rotateWithTorque(deviceId, pos); + } + + @PostMapping("/move_to_zero") + @Operation(summary = "回Home") + public void moveToZero(@RequestParam DeviceServoId deviceId) throws HardwareException { + servoService.moveToZero(deviceId); + } + + // 寄存器操作 + @PostMapping("/limit_velocity") + @Operation(summary = "设置限速") + public void setLimitVelocity( + @RequestParam DeviceServoId deviceId, + @RequestParam Integer val) throws HardwareException { + servoService.setLimitVelocity(deviceId, val); + } + + @PostMapping("/limit_torque") + @Operation(summary = "设置限力矩") + public void setLimitTorque( + @RequestParam DeviceServoId deviceId, + @RequestParam Integer val) throws HardwareException { + servoService.setLimitTorque(deviceId, val); + } + + @PostMapping("/protective_torque") + @Operation(summary = "设置保护力矩") + public void setProtectiveTorque( + @RequestParam DeviceServoId deviceId, + @RequestParam Integer val) throws HardwareException { + servoService.setProtectiveTorque(deviceId, val); + } + + @PostMapping("/reg") + @Operation(summary = "设置寄存器") + public void setReg( + @RequestParam DeviceServoId deviceId, + @RequestParam MiniServoRegIndex reg, + @RequestParam Integer val) throws HardwareException { + servoService.setReg(deviceId, reg, val); + } + + // 状态查询 + @GetMapping("/position") + @Operation(summary = "读取位置") + public Integer readPos(@RequestParam DeviceServoId deviceId) throws HardwareException { + return servoService.readPos(deviceId); + } + + @GetMapping("/regs") + @Operation(summary = "获取所有寄存器") + public Map getAllReg(@RequestParam DeviceServoId deviceId) { + return servoService.getAllReg(deviceId); + } +} diff --git a/src/main/java/com/iflytop/gd/hardware/controller/StepMotorController.java b/src/main/java/com/iflytop/gd/hardware/controller/StepMotorController.java new file mode 100644 index 0000000..bce3187 --- /dev/null +++ b/src/main/java/com/iflytop/gd/hardware/controller/StepMotorController.java @@ -0,0 +1,229 @@ +package com.iflytop.gd.hardware.controller; + +import com.iflytop.gd.hardware.drivers.StepMotorDriver.*; +import com.iflytop.gd.hardware.exception.HardwareException; +import com.iflytop.gd.hardware.service.StepMotorService; +import io.swagger.v3.oas.annotations.Operation; +import io.swagger.v3.oas.annotations.tags.Tag; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.web.bind.annotation.*; + +import java.util.Map; + +@Tag(name = "步进电机控制") +@RestController +@RequestMapping("/api/step_motor") +@RequiredArgsConstructor +@Slf4j +public class StepMotorController { + private final StepMotorService stepMotorService; + + // 基础设置 + @PostMapping("/speed_level") + @Operation(summary = "设置速度等级") + public void setStepMotorSpeedLevel(@RequestParam StepMotorSpeedLevel speedLevel) { + stepMotorService.setStepMotorSpeedLevel(speedLevel); + } + + // 基础操作 + @PostMapping("/enable") + @Operation(summary = "电源开启") + public void enableMotor(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + stepMotorService.enableMotor(deviceId); + } + + @PostMapping("/disable") + @Operation(summary = "电源关闭") + public void disableMotor(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + stepMotorService.disableMotor(deviceId); + } + + @PostMapping("/move_to_zero") + @Operation(summary = "回Home") + public void stepMotorEasyMoveToZero(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + stepMotorService.stepMotorEasyMoveToZero(deviceId); + } + + @PostMapping("/move_to_zero_quick") + @Operation(summary = "快速回Home") + public void stepMotorEasyMoveToZeroPointQuickBlock(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + stepMotorService.stepMotorEasyMoveToZeroPointQuickBlock(deviceId); + } + + @PostMapping("/stop") + @Operation(summary = "停止") + public void stepMotorStop(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + stepMotorService.stepMotorStop(deviceId); + } + + // 相对移动 + @PostMapping("/move_by_forward") + @Operation(summary = "正向点动") + public void stepMotorEasyMoveByForward( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer dpos) throws HardwareException { + stepMotorService.stepMotorEasyMoveByForward(deviceId, dpos); + } + + @PostMapping("/move_by_backward") + @Operation(summary = "反向点动") + public void stepMotorEasyMoveByBackward( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer dpos) throws HardwareException { + stepMotorService.stepMotorEasyMoveByBackward(deviceId, dpos); + } + + // 绝对移动 + @PostMapping("/move_to") + @Operation(summary = "移动(绝对)") + public void stepMotorEasyMoveTo( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer pos) throws HardwareException { + stepMotorService.stepMotorEasyMoveTo(deviceId, pos); + } + + // 旋转 + @PostMapping("/rotate_forward") + @Operation(summary = "正转") + public void rotateForward(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + stepMotorService.rotateForward(deviceId); + } + + @PostMapping("/rotate_backward") + @Operation(summary = "反转") + public void rotateBackward(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + stepMotorService.rotateBackward(deviceId); + } + + // 寄存器配置 + @PostMapping("/mres") + @Operation(summary = "设置细分") + public void setMres( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer mres) throws HardwareException { + stepMotorService.setMres(deviceId, mres); + } + + @PostMapping("/irun") + @Operation(summary = "设置IRUN") + public void setIRUN( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer irun) throws HardwareException { + stepMotorService.setIRUN(deviceId, irun); + } + + @PostMapping("/ihold") + @Operation(summary = "设置IHOLD") + public void setIHOLD( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer ihold) throws HardwareException { + stepMotorService.setIHOLD(deviceId, ihold); + } + + @PostMapping("/start_stop_vel") + @Operation(summary = "设置起停速度") + public void setStartAndStopVel( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer v) throws HardwareException { + stepMotorService.setStartAndStopVel(deviceId, v); + } + + @PostMapping("/v1") + @Operation(summary = "设置V1") + public void setV1( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer v) throws HardwareException { + stepMotorService.setV1(deviceId, v); + } + + @PostMapping("/acceleration") + @Operation(summary = "设置加速度") + public void setA1AndD1( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer acc) throws HardwareException { + stepMotorService.setA1AndD1(deviceId, acc); + } + + @PostMapping("/max_acceleration") + @Operation(summary = "设置最大加速度") + public void setAmaxAndDmax( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer acc) throws HardwareException { + stepMotorService.setAmaxAndDmax(deviceId, acc); + } + + @PostMapping("/default_velocity") + @Operation(summary = "设置默认速度") + public void setDefaultVel( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer v) throws HardwareException { + stepMotorService.setDefaultVel(deviceId, v); + } + + @PostMapping("/speed") + @Operation(summary = "设置速度") + public void setSpeed( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer low, + @RequestParam Integer mid, + @RequestParam Integer high) throws HardwareException { + stepMotorService.setSpeed(deviceId, low, mid, high); + } + + @PostMapping("/one_circle_pulse") + @Operation(summary = "设置一圈脉冲数") + public void setOneCirclePulse( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer pulse, + @RequestParam Integer denominator) throws HardwareException { + stepMotorService.setOneCirclePulse(deviceId, pulse, denominator); + } + + @PostMapping("/dzero") + @Operation(summary = "设置DZero") + public void setDZero( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam Integer dzero) throws HardwareException { + stepMotorService.setDZero(deviceId, dzero); + } + + @GetMapping("/reg") + @Operation(summary = "读取寄存器值") + public Integer readReg( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam StepMotorRegIndex reg) { + Integer val = 5; + return val; +// return stepMotorService.readReg(deviceId, reg); + + } + + @PostMapping("/reg") + @Operation(summary = "设置寄存器") + public void setReg( + @RequestParam DeviceStepMotorId deviceId, + @RequestParam StepMotorRegIndex reg, + @RequestParam Integer val) throws HardwareException { + stepMotorService.setReg(deviceId, reg, val); + } + + // 状态查询 + @GetMapping("/regs") + @Operation(summary = "读取所有寄存器") + public Map readAllRegs(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + return stepMotorService.readAllRegs(deviceId); + } + + @GetMapping("/position") + @Operation(summary = "读取位置") + public Integer readPos(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + return stepMotorService.readPos(deviceId); + } + + @GetMapping("/encoder_position") + @Operation(summary = "读取编码器位置") + public Integer readEncPos(@RequestParam DeviceStepMotorId deviceId) throws HardwareException { + return stepMotorService.readEncPos(deviceId); + } +} diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DeviceServoId.java b/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DeviceServoId.java new file mode 100644 index 0000000..c3a3dd5 --- /dev/null +++ b/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DeviceServoId.java @@ -0,0 +1,33 @@ +package com.iflytop.gd.hardware.drivers.MiniServoDriver; + +public enum DeviceServoId { + dual_robot_axis1(MiniServoMId.DUAL_ROBOT_AXIS1_MID, "双轴械臂一轴"), + dual_robot_axis2(MiniServoMId.DUAL_ROBOT_AXIS2_MID, "双轴械臂二轴"), + claw(MiniServoMId.CLAW_MID, "夹爪"), + ; + + private MiniServoMId servoMId; + private String description; + + public MiniServoMId getServoMId() { + return servoMId; + } + + public String getDescription() { + return description; + } + + DeviceServoId(MiniServoMId servoMId, String description) { + this.servoMId = servoMId; + this.description = description; + } + + public static DeviceServoId getById(MiniServoMId servoMId) { + for (DeviceServoId deviceServoId : DeviceServoId.values()) { + if (deviceServoId.getServoMId() == servoMId) { + return deviceServoId; + } + } + return null; + } +} 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 b897a41..d00db27 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 @@ -1,7 +1,6 @@ package com.iflytop.gd.hardware.drivers.MiniServoDriver; import com.iflytop.gd.hardware.exception.HardwareException; -import com.iflytop.gd.hardware.type.MId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -60,9 +59,9 @@ public class DualRobotDriver { public MiniServoMId getMiniServoMId(Joint joint) { switch (joint) { case JOINT_1: - return MiniServoMId.DUAL_ROBOT_AXIS_1_MID; + return MiniServoMId.DUAL_ROBOT_AXIS1_MID; case JOINT_2: - return MiniServoMId.DUAL_ROBOT_AXIS_2_MID; + return MiniServoMId.DUAL_ROBOT_AXIS2_MID; default: return null; } @@ -100,8 +99,8 @@ public class DualRobotDriver { public void moveToPoint(double pointX, double pointY) throws HardwareException { Point2D point = getServoPosition(new Point2D.Double(pointX, pointY)); - miniServoDriver.miniServoMoveToBlock(MiniServoMId.DUAL_ROBOT_AXIS_1_MID, (int)point.getX()); - miniServoDriver.miniServoMoveToBlock(MiniServoMId.DUAL_ROBOT_AXIS_2_MID, (int)point.getY()); + miniServoDriver.miniServoMoveToBlock(MiniServoMId.DUAL_ROBOT_AXIS1_MID, (int)point.getX()); + miniServoDriver.miniServoMoveToBlock(MiniServoMId.DUAL_ROBOT_AXIS2_MID, (int)point.getY()); } public void moveToHome(Joint joint) throws HardwareException { diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/MiniServoMId.java b/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/MiniServoMId.java index 48802b3..34f5af1 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/MiniServoMId.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/MiniServoMId.java @@ -1,12 +1,11 @@ package com.iflytop.gd.hardware.drivers.MiniServoDriver; import com.iflytop.gd.hardware.type.MId; -import org.springframework.util.Assert; public enum MiniServoMId { NotSet(MId.NotSet),// - DUAL_ROBOT_AXIS_1_MID(MId.DualRobotAxis1M),// - DUAL_ROBOT_AXIS_2_MID(MId.DualRobotAxis2M), + DUAL_ROBOT_AXIS1_MID(MId.DualRobotAxis1M),// + DUAL_ROBOT_AXIS2_MID(MId.DualRobotAxis2M), CLAW_MID(MId.HBotClawS) ; diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/AcidPumpDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/AcidPumpDriver.java index c9a3ab3..e509079 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/AcidPumpDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/AcidPumpDriver.java @@ -51,7 +51,7 @@ public class AcidPumpDriver { { double speed = getSpeed(stepMotorMId); int motorSpeed = getMotorSpeed(speed); - stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); +// stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); } public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception @@ -61,7 +61,7 @@ public class AcidPumpDriver { throw new InvalidParameterException("Position is out of range"); } this.setSpeed(stepMotorMId, speed); - stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); +// stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); } public void stop(StepMotorMId stepMotorMId) throws Exception { diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DeviceStepMotorId.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DeviceStepMotorId.java new file mode 100644 index 0000000..ad56364 --- /dev/null +++ b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DeviceStepMotorId.java @@ -0,0 +1,51 @@ +package com.iflytop.gd.hardware.drivers.StepMotorDriver; + + +public enum DeviceStepMotorId { + door(StepMotorMId.DOOR_MOTOR_MID, "门"), + shake_motor(StepMotorMId.SHAKE_MOTOR_MID, "摇匀电机"), + tray_motor(StepMotorMId.TRAY_MOTOR_MID, "托盘电机"), + gantry_x(StepMotorMId.HBOT_X_MOTOR_MID, "龙门架X轴"), + gantry_y(StepMotorMId.HBOT_Y_MOTOR_MID, "龙门架Y轴"), + gantry_z(StepMotorMId.HBOT_Z_MOTOR_MID, "龙门架Z轴"), + heater_motor_1(StepMotorMId.HEATER_1_MOTOR_MID, "加热位顶升电机1"), + heater_motor_2(StepMotorMId.HEATER_2_MOTOR_MID, "加热位顶升电机2"), + heater_motor_3(StepMotorMId.HEATER_3_MOTOR_MID, "加热位顶升电机3"), + heater_motor_4(StepMotorMId.HEATER_4_MOTOR_MID, "加热位顶升电机4"), + heater_motor_5(StepMotorMId.HEATER_5_MOTOR_MID, "加热位顶升电机5"), + heater_motor_6(StepMotorMId.HEATER_6_MOTOR_MID, "加热位顶升电机6"), + acid_pump_1(StepMotorMId.ACID_PUMP_1_MOTOR_MID, "加酸泵电机1"), + acid_pump_2(StepMotorMId.ACID_PUMP_2_MOTOR_MID, "加酸泵电机2"), + acid_pump_3(StepMotorMId.ACID_PUMP_3_MOTOR_MID, "加酸泵电机3"), + acid_pump_4(StepMotorMId.ACID_PUMP_4_MOTOR_MID, "加酸泵电机4"), + acid_pump_5(StepMotorMId.ACID_PUMP_5_MOTOR_MID, "加酸泵电机5"), + acid_pump_6(StepMotorMId.ACID_PUMP_6_MOTOR_MID, "加酸泵电机6"), + acid_pump_7(StepMotorMId.ACID_PUMP_7_MOTOR_MID, "加酸泵电机7"), + acid_pump_8(StepMotorMId.ACID_PUMP_8_MOTOR_MID, "加酸泵电机8"), + ; + + private StepMotorMId stepMotorMId; + private String description; + + public StepMotorMId getStepMotorMId() { + return stepMotorMId; + } + + public String getDescription() { + return description; + } + + DeviceStepMotorId(StepMotorMId stepMotorMId, String description) { + this.stepMotorMId = stepMotorMId; + this.description = description; + } + + public static DeviceStepMotorId getById(StepMotorMId stepMotorMId) { + for (DeviceStepMotorId deviceStepMotorId : DeviceStepMotorId.values()) { + if (deviceStepMotorId.getStepMotorMId() == stepMotorMId) { + return deviceStepMotorId; + } + } + return null; + } +} diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DoorDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DoorDriver.java index e0a5d54..6231df0 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DoorDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DoorDriver.java @@ -51,7 +51,7 @@ public class DoorDriver{ { double speed = getSpeed(stepMotorMId); int motorSpeed = getMotorSpeed(speed); - stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); +// stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); } public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception @@ -61,7 +61,7 @@ public class DoorDriver{ throw new InvalidParameterException("Position is out of range"); } this.setSpeed(stepMotorMId, speed); - stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); +// stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); } public void stop(StepMotorMId stepMotorMId) throws Exception { diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java index c32e40f..5506d4d 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java @@ -51,7 +51,7 @@ public class HBotDriver { { double speed = getSpeed(stepMotorMId); int motorSpeed = getMotorSpeed(speed); - stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); +// stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); } public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception @@ -61,7 +61,7 @@ public class HBotDriver { throw new InvalidParameterException("Position is out of range"); } this.setSpeed(stepMotorMId, speed); - stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); +// stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); } public void stop(StepMotorMId stepMotorMId) throws Exception { diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HeaterMotorDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HeaterMotorDriver.java index 1a5304b..a873c51 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HeaterMotorDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HeaterMotorDriver.java @@ -51,7 +51,7 @@ public class HeaterMotorDriver { { double speed = getSpeed(stepMotorMId); int motorSpeed = getMotorSpeed(speed); - stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); +// stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); } public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception @@ -61,7 +61,7 @@ public class HeaterMotorDriver { throw new InvalidParameterException("Position is out of range"); } this.setSpeed(stepMotorMId, speed); - stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); +// stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); } public void stop(StepMotorMId stepMotorMId) throws Exception { diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java index a6c9375..83fa2bf 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java @@ -46,7 +46,7 @@ public class ShakeMotorDriver { double speed = getSpeed(stepMotorMId); int motorSpeed = getMotorSpeed(speed); int direction = 1; - stepMotorCtrlDriver_.rotate(stepMotorMId, direction, motorSpeed); +// stepMotorCtrlDriver_.rotate(stepMotorMId, direction, motorSpeed); } public void stop(StepMotorMId stepMotorMId) throws Exception { diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorCtrlDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorCtrlDriver.java index a76758e..3bcdb40 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorCtrlDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorCtrlDriver.java @@ -8,6 +8,9 @@ import lombok.RequiredArgsConstructor; import org.slf4j.Logger; import org.springframework.stereotype.Component; +import java.util.HashMap; +import java.util.Map; + @Component @RequiredArgsConstructor public class StepMotorCtrlDriver { @@ -105,24 +108,25 @@ public class StepMotorCtrlDriver { // step_motor_move_to_zero_point_quick(0x021f, "STEP_MOTOR_MOVE_TO_ZERO_POINT_QUICK"), // (speedlevel)->null speedlevel=0,1,2,3(default,low,mid,high) // step_motor_rotate(0x0220, "STEP_MOTOR_ROTATE"), // -// public void moveByBlock(StepMotorMId id, Integer dpos, StepMotorSpeedLevel speedLevel) throws HardwareException { -// canBus.callcmd(id.mid, CmdId.step_motor_move_by, dpos, speedLevel.ordinal()); -// waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_by)); -// } -// -// public void moveToBlock(StepMotorMId id, Integer pos, StepMotorSpeedLevel speedLevel) throws HardwareException { -// canBus.callcmd(id.mid, CmdId.step_motor_move_to, pos, speedLevel.ordinal()); -// waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to)); -// } -// -// public void moveToZeroQuickBlock(StepMotorMId id, StepMotorSpeedLevel speedLevel) throws HardwareException { -// canBus.callcmd(id.mid, CmdId.step_motor_move_to_zero_point_quick, speedLevel.ordinal()); -// waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to_zero_point_quick)); -// } -// -// public void rotate(StepMotorMId id, Integer direction, StepMotorSpeedLevel speedLevel) throws HardwareException { -// canBus.callcmd(id.mid, CmdId.step_motor_rotate, direction, speedLevel.ordinal()); -// } + public void moveByBlock(StepMotorMId id, Integer dpos, StepMotorSpeedLevel speedLevel) throws HardwareException { + canBus.callcmd(id.mid, CmdId.step_motor_move_by, dpos, speedLevel.ordinal()); + waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_by)); + } + + public void moveToBlock(StepMotorMId id, Integer pos, StepMotorSpeedLevel speedLevel) throws HardwareException { + canBus.callcmd(id.mid, CmdId.step_motor_move_to, pos, speedLevel.ordinal()); + waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to)); + } + + public void moveToZeroQuickBlock(StepMotorMId id, StepMotorSpeedLevel speedLevel) throws HardwareException { + canBus.callcmd(id.mid, CmdId.step_motor_move_to_zero_point_quick, speedLevel.ordinal()); + waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to_zero_point_quick)); + } + + public void rotate(StepMotorMId id, Integer direction, StepMotorSpeedLevel speedLevel) throws HardwareException { + canBus.callcmd(id.mid, CmdId.step_motor_rotate, direction, speedLevel.ordinal()); + + } public Boolean stepMotorReadIoState(StepMotorMId id, Integer ioindex) throws HardwareException { var packet = canBus.callcmd(id.mid, CmdId.step_motor_read_io_state, ioindex); @@ -143,27 +147,6 @@ public class StepMotorCtrlDriver { canBus.waitForMod(id.mid, actionOvertimeConstant.get(id, CmdId.step_motor_easy_reciprocating_motion)); } - // ==== ==== ==== ==== ==== ==== raw ctrl ==== ==== ==== ==== ==== ==== - public void rotate(StepMotorMId id, Integer direction, Integer speed) throws HardwareException { - canBus.callcmd(id.mid, CmdId.step_motor_rotate, direction, speed); - } - - public void moveToBlock(StepMotorMId id, Integer pos, Integer speed) throws HardwareException { - canBus.callcmd(id.mid, CmdId.step_motor_move_to, pos, speed); - waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to)); - } - - public void moveByBlock(StepMotorMId id, Integer pos, Integer speed) throws HardwareException { - canBus.callcmd(id.mid, CmdId.step_motor_move_by, pos, speed); - waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_by)); - } - - public void moveToZeroQuickBlock(StepMotorMId id, Integer speed) throws HardwareException { - canBus.callcmd(id.mid, CmdId.step_motor_move_to_zero_point_quick, speed); - waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to_zero_point_quick)); - } - - public Boolean isHasMoveToZero(StepMotorMId id) throws HardwareException { return getReg(id, StepMotorRegIndex.kreg_step_motor_has_move_zero) != 0; } @@ -189,4 +172,14 @@ public class StepMotorCtrlDriver { // } // return node; // } + + public Map getAllReg(StepMotorMId id) throws HardwareException { + Map map = new HashMap(); + for (StepMotorRegIndex regIndex : StepMotorRegIndex.values()) { + Integer regVal = getReg(id, regIndex); + logger.debug("read reg {} -> {}", regIndex, regVal); + map.put(regIndex.name(), regVal); + } + return map; + } } diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorRegIndex.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorRegIndex.java index 98fe9bc..5338084 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorRegIndex.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorRegIndex.java @@ -14,7 +14,6 @@ public enum StepMotorRegIndex { kreg_step_motor_is_enable(RegIndex.kreg_step_motor_is_enable), // 是否使能 kreg_step_motor_dpos(RegIndex.kreg_step_motor_dpos), // 执行完上一条指令后的相对位移 kreg_step_motor_has_move_zero(RegIndex.kreg_step_motor_has_move_zero), // 是否已经移动到零点 - // kreg_step_motor_shift(RegIndex.kreg_step_motor_shift), // x偏移 kreg_step_motor_shaft(RegIndex.kreg_step_motor_shaft), // x轴是否反转 kreg_step_motor_one_circle_pulse(RegIndex.kreg_step_motor_one_circle_pulse), // x轴一圈脉冲数 kreg_step_motor_one_circle_pulse_denominator(RegIndex.kreg_step_motor_one_circle_pulse_denominator), // 设置一圈脉冲数的分母 diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java index 9bc5ebe..5a7785d 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java @@ -60,7 +60,7 @@ public class TrayMotorDriver { { double speed = getSpeed(stepMotorMId); int motorSpeed = getMotorSpeed(speed); - stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); +// stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed); } public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception @@ -70,7 +70,7 @@ public class TrayMotorDriver { throw new InvalidParameterException("Position is out of range"); } this.setSpeed(stepMotorMId, speed); - stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); +// stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed)); } public void stop(StepMotorMId stepMotorMId) throws Exception { @@ -85,6 +85,6 @@ public class TrayMotorDriver { int motorPosition = getMotorPosition(position); int motorSpeed = getMotorSpeed(speed); - stepMotorCtrlDriver_.moveToBlock(stepMotorMId, motorPosition, motorSpeed); +// stepMotorCtrlDriver_.moveToBlock(stepMotorMId, motorPosition, motorSpeed); } } diff --git a/src/main/java/com/iflytop/gd/hardware/service/ServoService.java b/src/main/java/com/iflytop/gd/hardware/service/ServoService.java new file mode 100644 index 0000000..38cab18 --- /dev/null +++ b/src/main/java/com/iflytop/gd/hardware/service/ServoService.java @@ -0,0 +1,130 @@ +package com.iflytop.gd.hardware.service; + +import com.iflytop.gd.hardware.drivers.MiniServoDriver.DeviceServoId; +import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoDriver; +import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoMId; +import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoRegIndex; +import com.iflytop.gd.hardware.drivers.StepMotorDriver.DeviceStepMotorId; +import com.iflytop.gd.hardware.exception.HardwareException; +import com.iflytop.gd.hardware.type.A8kEcode; +import com.iflytop.gd.hardware.type.AppError; +import lombok.RequiredArgsConstructor; +import org.springframework.stereotype.Service; + +import java.util.HashMap; +import java.util.Map; + +@Service +@RequiredArgsConstructor +public class ServoService { + private final MiniServoDriver miniServoDriver; + + private void throwDeviceNull(DeviceServoId id) throws HardwareException + { + throw new HardwareException(new AppError(A8kEcode.PE_PARAM_OUT_OF_RANGE, "StepMotorMId is null for id: " + id)); + } + + public void enable(DeviceServoId id, Boolean enable) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.miniServoEnable(servoMId, enable ? 1 : 0); + } + + public void stop(DeviceServoId id) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.moduleStop(servoMId); + } + + public void moveTo(DeviceServoId id, Integer pos) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.miniServoMoveToBlock(servoMId, pos); + } + + public void rotate(DeviceServoId id, Integer direction) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.miniServoRotate(servoMId, direction); + } + + public void rotateWithTorque(DeviceServoId id, Integer pos) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.miniServoRotateWithTorque(servoMId, pos); + } + + public void setReg(DeviceServoId id, MiniServoRegIndex reg, Integer val) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.setReg(servoMId, reg, val); + } + + public Map getAllReg(DeviceServoId id) { + Map map = new HashMap<>(); + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + return map; + } + + for (MiniServoRegIndex reg : MiniServoRegIndex.values()) { + try { + map.put(reg.name(), miniServoDriver.getReg(servoMId, reg)); + } catch (HardwareException e) { + } + } + return map; + } + + public void setLimitVelocity(DeviceServoId id, Integer val) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.setReg(servoMId, MiniServoRegIndex.kreg_mini_servo_limit_velocity, val); + } + + public void setLimitTorque(DeviceServoId id, Integer val) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.setReg(servoMId, MiniServoRegIndex.kreg_mini_servo_limit_torque, val); + } + + public void setProtectiveTorque(DeviceServoId id, Integer val) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.setReg(servoMId, MiniServoRegIndex.kreg_mini_servo_protective_torque, val); + } + + public Integer readPos(DeviceServoId id) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + return miniServoDriver.miniServoReadPos(servoMId); + } + + public void moveToZero(DeviceServoId id) throws HardwareException { + MiniServoMId servoMId = id.getServoMId(); + if (servoMId == null) { + throwDeviceNull(id); + } + miniServoDriver.miniServoMoveToZeroBlock(servoMId); + } +} diff --git a/src/main/java/com/iflytop/gd/hardware/service/StepMotorService.java b/src/main/java/com/iflytop/gd/hardware/service/StepMotorService.java new file mode 100644 index 0000000..b3c0a4c --- /dev/null +++ b/src/main/java/com/iflytop/gd/hardware/service/StepMotorService.java @@ -0,0 +1,250 @@ +package com.iflytop.gd.hardware.service; + +import com.iflytop.gd.hardware.drivers.StepMotorDriver.*; +import com.iflytop.gd.hardware.exception.HardwareException; +import com.iflytop.gd.hardware.type.A8kEcode; +import com.iflytop.gd.hardware.type.AppError; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Service; + +import java.util.Map; + +@Slf4j +@Service +@RequiredArgsConstructor +public class StepMotorService { + private final StepMotorCtrlDriver stepMotorCtrlDriver_; + + StepMotorSpeedLevel speedLevel_ = StepMotorSpeedLevel.DEFAULT; + + private void throwDeviceNull(DeviceStepMotorId id) throws HardwareException + { + throw new HardwareException(new AppError(A8kEcode.PE_PARAM_OUT_OF_RANGE, "StepMotorMId is null for id: " + id)); + } + + public void setStepMotorSpeedLevel(StepMotorSpeedLevel speedLevel) { + this.speedLevel_ = speedLevel; + } + + public void enableMotor(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.stepMotorEnable(stepId, 1); + } + + public void disableMotor(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.stepMotorEnable(stepId, 0); + } + + public void stepMotorEasyMoveToZero(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.stepMotorEasyMoveToZero(stepId); + } + + public void stepMotorEasyMoveToZeroPointQuickBlock(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.stepMotorEasyMoveToZeroPointQuickBlock(stepId); + } + + public void stepMotorStop(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.stepMotorStop(stepId); + } + + public void stepMotorEasyMoveByForward(DeviceStepMotorId id, Integer dpos) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + log.info("stepMotorEasyMoveForward: {} {}", id, dpos); + stepMotorCtrlDriver_.moveByBlock(stepId, dpos, speedLevel_); + } + + public void stepMotorEasyMoveByBackward(DeviceStepMotorId id, Integer dpos) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + log.info("stepMotorEasyMoveBackward: {} {}", id, -dpos); + stepMotorCtrlDriver_.moveByBlock(stepId, -dpos, speedLevel_); + } + + public void stepMotorEasyMoveTo(DeviceStepMotorId id, Integer pos) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + log.info("stepMotorEasyMoveTo: {} {}", id, pos); + stepMotorCtrlDriver_.moveToBlock(stepId, pos, speedLevel_); + } + + public void rotateForward(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.rotate(stepId, 1, speedLevel_); + } + + public void rotateBackward(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.rotate(stepId, -1, speedLevel_); + } + + public Integer readEncPos(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + return stepMotorCtrlDriver_.stepMotorReadEncPos(stepId); + } + + public Integer readPos(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + return stepMotorCtrlDriver_.stepMotorReadPos(stepId); + } + + public void setReg(DeviceStepMotorId id, StepMotorRegIndex reg, Integer val) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, reg, val); + } + + public void setStartAndStopVel(DeviceStepMotorId id, Integer v) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_vstart, v); + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_vstop, v); + } + + public void setV1(DeviceStepMotorId id, Integer v) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_v1, v); + } + + public void setA1AndD1(DeviceStepMotorId id, Integer acc) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_a1, acc); + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_d1, acc); + } + + public void setAmaxAndDmax(DeviceStepMotorId id, Integer acc) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_amax, acc); + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_dmax, acc); + } + + public void setDefaultVel(DeviceStepMotorId id, Integer v) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_default_velocity, v); + } + + public void setSpeed(DeviceStepMotorId id, Integer low_speed, Integer mid_speed, Integer high_speed) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_low_velocity, low_speed); + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_mid_velocity, mid_speed); + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_high_velocity, high_speed); + } + + public void setMres(DeviceStepMotorId id, Integer mres) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_mres, mres); + } + + public void setIRUN(DeviceStepMotorId id, Integer irun) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_irun, irun); + } + + public void setIHOLD(DeviceStepMotorId id, Integer ihold) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_ihold, ihold); + } + + public void setOneCirclePulse(DeviceStepMotorId id, Integer pulse, Integer denominator) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_one_circle_pulse, pulse); + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_one_circle_pulse_denominator, denominator); + } + + public void setDZero(DeviceStepMotorId id, Integer dzero) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + stepMotorCtrlDriver_.setReg(stepId, StepMotorRegIndex.kreg_step_motor_dzero_pos, dzero); + } + + public Integer readReg(DeviceStepMotorId id, StepMotorRegIndex reg) { + try { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + return stepMotorCtrlDriver_.getReg(stepId, reg); + } catch (HardwareException e) { + return 0; + } + } + + public Map readAllRegs(DeviceStepMotorId id) throws HardwareException { + StepMotorMId stepId = id.getStepMotorMId(); + if (stepId == null) { + throwDeviceNull(id); + } + return stepMotorCtrlDriver_.getAllReg(stepId); + } +}