17 changed files with 862 additions and 62 deletions
-
3src/main/java/com/iflytop/gd/hardware/command/handlers/DualRobotHandler.java
-
118src/main/java/com/iflytop/gd/hardware/controller/ServoController.java
-
229src/main/java/com/iflytop/gd/hardware/controller/StepMotorController.java
-
33src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DeviceServoId.java
-
9src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DualRobotDriver.java
-
5src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/MiniServoMId.java
-
4src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/AcidPumpDriver.java
-
51src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DeviceStepMotorId.java
-
4src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DoorDriver.java
-
4src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java
-
4src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HeaterMotorDriver.java
-
2src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java
-
71src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorCtrlDriver.java
-
1src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorRegIndex.java
-
6src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java
-
130src/main/java/com/iflytop/gd/hardware/service/ServoService.java
-
250src/main/java/com/iflytop/gd/hardware/service/StepMotorService.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<String, Integer> getAllReg(@RequestParam DeviceServoId deviceId) { |
|||
return servoService.getAllReg(deviceId); |
|||
} |
|||
} |
|||
@ -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<String, Integer> 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); |
|||
} |
|||
} |
|||
@ -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; |
|||
} |
|||
} |
|||
@ -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; |
|||
} |
|||
} |
|||
@ -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<String, Integer> getAllReg(DeviceServoId id) { |
|||
Map<String, Integer> 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); |
|||
} |
|||
} |
|||
@ -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<String, Integer> readAllRegs(DeviceStepMotorId id) throws HardwareException { |
|||
StepMotorMId stepId = id.getStepMotorMId(); |
|||
if (stepId == null) { |
|||
throwDeviceNull(id); |
|||
} |
|||
return stepMotorCtrlDriver_.getAllReg(stepId); |
|||
} |
|||
} |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue