@EventSubscribe
@MQTTPath(group = MessageGroup.COMMANDS, device = "servos", label = "position")
public void convert(MQTTMessage mqttMessage) {
LOG.debug(
"Executing Servo command from topic: {}", mqttMessage.getMessage(), mqttMessage.getTopic());
BasicCommand basicCommand = mapFromJson(mqttMessage.getMessage(), BasicCommandImpl.class);
Robot robot = robotRegistry.getRobot(basicCommand.getControllerId());
ServoDriver servoDriver = robot.getServoDriver();
String servoPosition = basicCommand.getProperty("position");
String servoId = basicCommand.getProperty("servoId");
String speed = basicCommand.getProperty("speed");
if (StringUtils.hasText(servoPosition) && StringUtils.hasText(servoId)) {
LOG.info("Setting servo: {} to position: {}", servoId, servoPosition);
if (StringUtils.hasText(speed)) {
servoDriver.setPositionAndSpeed(
servoId, IntUtils.toInt(speed, DEFAULT_SPEED), IntUtils.toSafeInt(servoPosition));
} else {
servoDriver.setTargetPosition(servoId, IntUtils.toSafeInt(servoPosition));
}
} else {
LOG.warn("Received servo command, but no servoId or Position specified");
}
}
@EventSubscribe
@MQTTPath(group = MessageGroup.COMMANDS, device = "servos", label = "torgue")
public void torgue(MQTTMessage mqttMessage) {
LOG.debug(
"Executing Servo command from topic: {}", mqttMessage.getMessage(), mqttMessage.getTopic());
BasicCommand basicCommand = mapFromJson(mqttMessage.getMessage(), BasicCommandImpl.class);
Robot robot = robotRegistry.getRobot(basicCommand.getControllerId());
ServoDriver servoDriver = robot.getServoDriver();
String servoId = basicCommand.getProperty("servoId");
boolean torgueEnabled = Boolean.parseBoolean(basicCommand.getProperty("torgue"));
Optional<Integer> tl = IntUtils.toInt(basicCommand.getProperty("torgueLimit"));
if (tl.isPresent()) {
servoDriver.setTorgue(servoId, tl.get());
} else {
servoDriver.setTorgue(servoId, torgueEnabled);
}
}
