diff --git a/src/test/java/ru/r2cloud/rotator/allview/AllviewClient.java b/src/test/java/ru/r2cloud/rotator/allview/AllviewClient.java new file mode 100644 index 00000000..3ed0aa0b --- /dev/null +++ b/src/test/java/ru/r2cloud/rotator/allview/AllviewClient.java @@ -0,0 +1,250 @@ +package ru.r2cloud.rotator.allview; + +import java.io.BufferedReader; +import java.io.BufferedWriter; +import java.io.IOException; +import java.io.InputStreamReader; +import java.io.OutputStreamWriter; +import java.nio.charset.StandardCharsets; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import com.fazecast.jSerialComm.SerialPort; +import com.fazecast.jSerialComm.SerialPortInvalidPortException; + +import ru.r2cloud.lora.loraat.SerialInterface; +import ru.r2cloud.lora.loraat.SerialPortInterface; +import ru.r2cloud.rotctrld.Position; + +public class AllviewClient { + + private static final Logger LOG = LoggerFactory.getLogger(AllviewClient.class); + private static final int CHECK_STATUS_DELAY = 100; + + private final String portDescriptor; + private final int timeout; + private final SerialInterface serial; + + private SerialPortInterface port; + private BufferedReader is; + private BufferedWriter os; + + private AllviewMotor az; + private AllviewMotor el; + + public AllviewClient(String portDescriptor, int timeout, SerialInterface serial) { + this.portDescriptor = portDescriptor; + this.timeout = timeout; + this.serial = serial; + } + + public void slew(Position nextPosition, Position previousPosition) throws IOException { + double currentAzimuth; + if (az.isFastMode()) { + try { + // check if position overrun. i.e. + currentAzimuth = az.getPosition(); + } catch (IOException e) { + currentAzimuth = previousPosition.getAzimuth(); + } + } else { + currentAzimuth = previousPosition.getAzimuth(); + } + double azimuthDelta = nextPosition.getAzimuth() - currentAzimuth; + // counter clock wise + if (azimuthDelta > 180.0) { + azimuthDelta = 0 - currentAzimuth - (360.0 - nextPosition.getAzimuth()); + } + // clock wise + if (azimuthDelta < -180.0) { + azimuthDelta = (360.0 - currentAzimuth) + nextPosition.getAzimuth(); + } + double elevationDelta; + if (el.isFastMode()) { + try { + elevationDelta = nextPosition.getElevation() - el.getPosition(); + } catch (IOException e) { + elevationDelta = nextPosition.getElevation() - previousPosition.getElevation(); + } + } else { + elevationDelta = nextPosition.getElevation() - previousPosition.getElevation(); + } + double azimuthDeltaAbs = Math.abs(azimuthDelta); + double elevationDeltaAbs = Math.abs(elevationDelta); + + boolean azStopRequired = az.isStopRequired(azimuthDelta); + boolean elStopRequired = el.isStopRequired(elevationDelta); + if (!azStopRequired && !elStopRequired) { + if (azimuthDeltaAbs > elevationDeltaAbs) { + az.slew(azimuthDelta); + el.slew(elevationDelta); + } else { + el.slew(elevationDelta); + az.slew(azimuthDelta); + } + } else if (azStopRequired && !elStopRequired) { + el.slew(elevationDelta); + az.stop(); + while (!az.waitMotorStop()) { + try { + Thread.sleep(CHECK_STATUS_DELAY); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + return; + } + } + az.slew(azimuthDelta); + } else if (!azStopRequired && elStopRequired) { + az.slew(azimuthDelta); + el.stop(); + while (!el.waitMotorStop()) { + try { + Thread.sleep(CHECK_STATUS_DELAY); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + return; + } + } + el.slew(elevationDelta); + } else { + az.stop(); + el.stop(); + while (!az.waitMotorStop() || !el.waitMotorStop()) { + try { + Thread.sleep(CHECK_STATUS_DELAY); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + return; + } + } + if (azimuthDeltaAbs > elevationDeltaAbs) { + az.slew(azimuthDelta); + el.slew(elevationDelta); + } else { + el.slew(elevationDelta); + az.slew(azimuthDelta); + } + } + } + + public synchronized void start() throws Exception { + port = serial.getCommPort(portDescriptor); + port.setComPortTimeouts(SerialPort.TIMEOUT_READ_SEMI_BLOCKING, timeout, timeout); + port.setBaudRate(9600); + port.setParity(SerialPort.NO_PARITY); + port.setNumStopBits(SerialPort.ONE_STOP_BIT); + if (!port.openPort()) { + throw new SerialPortInvalidPortException("can't open port"); + } + is = new BufferedReader(new InputStreamReader(port.getInputStream(), StandardCharsets.US_ASCII)); + os = new BufferedWriter(new OutputStreamWriter(port.getOutputStream(), StandardCharsets.US_ASCII)); + + az = new AllviewMotor(1, this); + el = new AllviewMotor(2, this); + } + + public synchronized void stop() { + if (az != null) { + try { + az.stop(); + } catch (IOException e) { + LOG.info("can't stop azimuth", e); + } + } + if (el != null) { + try { + el.stop(); + } catch (IOException e) { + LOG.info("can't stop elevation", e); + } + } + if (port != null) { + port.closePort(); + } + } + + public synchronized String sendMessage(String message) throws IOException { + os.append(message); + os.flush(); + char curChar = '\r'; + while ((curChar = (char) is.read()) != '\r') { + // skip + } + StringBuilder str = new StringBuilder(); + while ((curChar = (char) is.read()) != '\r') { + str.append(curChar); + } + String result = str.toString().trim(); + if (result.startsWith("!")) { + throw new IOException("invalid response: " + convertErrorCode(Integer.valueOf(result.substring(1))) + " request: " + message); + } + // ignore "=" + result = result.substring(1); + return result; + } + + private static String convertErrorCode(int code) { + switch (code) { + case 0: { + return "Unknown Command"; + } + case 1: { + return "Command Length Error"; + } + case 2: { + return "Motor not Stopped"; + } + case 3: { + return "Invalid Character"; + } + case 4: { + return "Not Initialized"; + } + case 5: { + return "Driver Sleeping"; + } + case 7: { + return "PEC Training is running"; + } + case 8: { + return "No Valid PEC data"; + } + default: + return "unknown error code: " + code; + } + } + + public Position getPosition() throws IOException { + Position result = new Position(); + result.setAzimuth(az.getPosition()); + result.setElevation(el.getPosition()); + return result; + } + + public void setPosition(Position position) throws IOException { + az.setPosition(position.getAzimuth()); + el.setPosition(position.getElevation()); + } + + public void stopMotors() throws IOException { + az.stop(); + el.stop(); + } + + public void waitMotorStop() throws IOException { + while (!az.waitMotorStop() || !el.waitMotorStop()) { + try { + Thread.sleep(CHECK_STATUS_DELAY); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + break; + } + } + } + + public String getMotorBoardVersion() throws IOException { + return sendMessage(":e1\r"); + } + +} diff --git a/src/test/java/ru/r2cloud/rotator/allview/AllviewMotor.java b/src/test/java/ru/r2cloud/rotator/allview/AllviewMotor.java new file mode 100644 index 00000000..1243c769 --- /dev/null +++ b/src/test/java/ru/r2cloud/rotator/allview/AllviewMotor.java @@ -0,0 +1,162 @@ +package ru.r2cloud.rotator.allview; + +import java.io.IOException; + +public class AllviewMotor { + + private boolean fastMode; + private int index; + private AllviewClient client; + private boolean stopped = false; + private int currentSpeed = 0; + private double currentDegree; + private int cpr; + private int timerInterruptFreq; + + public AllviewMotor(int index, AllviewClient client) { + this.index = index; + this.client = client; + } + + public void setPosition(double degrees) throws IOException { + client.sendMessage(":G" + index + "00\r"); + if (cpr == 0) { + cpr = convertInteger(client.sendMessage(":a" + index + "\r")); + } + int number = (int) ((cpr / 360.0) * degrees) | 0x800000; + String command = String.format(":S%d%02X%02X%02X\r", index, (number & 0xFF), ((number >> 8) & 0xFF), ((number >> 16) & 0xFF)); + client.sendMessage(command); + client.sendMessage(":J" + index + "\r"); + stopped = false; + fastMode = true; + currentSpeed = 0; + } + + public double getPosition() throws IOException { + return convertAngle(client.sendMessage(":j" + index + "\r")); + } + + private static int convertInteger(String inputStr) { + int input = Integer.valueOf(inputStr, 16); + return ((input & 0x0000FF) << 16) | (input & 0x00FF00) | ((input & 0xFF0000) >> 16); + } + + public static double convertAngle(String inputStr) { + int converted = convertInteger(inputStr); + boolean positive = true; + if (converted > 0x800000) { + converted = converted - 0x800000; + } else { + converted = 0x800000 - converted; + positive = false; + } + double ticks_per_angle = (double) 865050 / 360.0; + double result = converted / ticks_per_angle; + if (!positive) { + return 360.0 - result; + } + return result; + } + + public void stop() throws IOException { + if (stopped) { + return; + } + client.sendMessage(":K" + index + "\r"); + } + + public boolean waitMotorStop() throws IOException { + if (stopped) { + return stopped; + } + String status = client.sendMessage(":f" + index + "\r"); + int num = Integer.valueOf(status); + if (((num >> 1) & 0x1) == 0) { + stopped = true; + currentSpeed = 0; + } + return stopped; + } + + public boolean isStopped() { + return stopped; + } + + public boolean isFastMode() { + return fastMode; + } + + public void slew(double degreePerSec) throws IOException { + boolean changeDirectionToCw = (degreePerSec > 0 && currentDegree < 0); + boolean changeDirectionToCcw = (degreePerSec < 0 && currentDegree > 0); + boolean changeDirectionNeeded = (changeDirectionToCw || changeDirectionToCcw); + double degreeAbs = Math.abs(degreePerSec); + int number = (int) ((getTimerInterruptFreq() * 360.0f) / degreeAbs / 865050.0f); + if (currentSpeed != 0 && currentSpeed == number && !changeDirectionNeeded) { + return; + } + currentDegree = degreePerSec; + currentSpeed = number; + if (degreeAbs > 0.37037037037037) { + // slow mode to fast mode + if (stopped) { + int direction = degreePerSec > 0 ? 0 : 1; + client.sendMessage(":G" + index + "3" + direction + "\r"); + } + number = 32 * number; + String command = String.format(":I%d%02X%02X%02X\r", index, (number & 0xFF), ((number >> 8) & 0xFF), ((number >> 16) & 0xFF)); + client.sendMessage(command); + client.sendMessage(":J" + index + "\r"); + fastMode = true; + } else { + if (stopped) { + int direction = degreePerSec > 0 ? 0 : 1; + client.sendMessage(":G" + index + "1" + direction + "\r"); + } + String command = String.format(":I%d%02X%02X%02X\r", index, (number & 0xFF), ((number >> 8) & 0xFF), ((number >> 16) & 0xFF)); + client.sendMessage(command); + if (stopped) { + // switch from fast to slow mode require full stop. thus start motor command + client.sendMessage(":J" + index + "\r"); + } + fastMode = false; + } + stopped = false; + } + + public boolean isStopRequired(double degree) throws IOException { + if (stopped) { + return false; + } + // transition from slow to fast + if (!fastMode && Math.abs(degree) > 0.37037037037037) { + return true; + } + if (fastMode) { + // transition from fast to slow + if (Math.abs(degree) < 0.37037037037037) { + return true; + } + // only when required speed is actually changed + int number = (int) ((getTimerInterruptFreq() * 360.0f) / Math.abs(degree) / 865050.0f); + if (currentSpeed != number) { + return true; + } + } + boolean changeDirectionToCw = (degree > 0 && currentDegree < 0); + boolean changeDirectionToCcw = (degree < 0 && currentDegree > 0); + if (changeDirectionToCw || changeDirectionToCcw) { + return true; + } + + return false; + } + + private int getTimerInterruptFreq() throws IOException { + if (timerInterruptFreq == 0) { + timerInterruptFreq = convertInteger(client.sendMessage(":b1\r")); + } + return timerInterruptFreq; + } + +}