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