From 9e6fc08f488822e5f92806884cfd952c8782a84e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=99=BD=E5=87=A4=E5=90=89?= Date: Sun, 6 Apr 2025 14:10:37 +0800 Subject: [PATCH] =?UTF-8?q?=E5=90=91=E8=93=9D=E7=89=99=E8=AE=BE=E5=A4=87?= =?UTF-8?q?=E5=8F=91=E9=80=81=E6=8C=87=E4=BB=A4=EF=BC=8C=E8=BF=81=E7=A7=BB?= =?UTF-8?q?=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../com/iflytop/profilometer/api/auth/AuthApi.java | 8 +- .../com/iflytop/profilometer/api/ble/BleApi.java | 6 +- .../profilometer/api/measure/MeasureApi.java | 21 ++ .../profilometer/api/measure/MeasureRoutes.kt | 4 + .../api/ws/DeviceStateWebsocketManager.java | 5 +- .../core/bluetooth/BleDeviceDriver.java | 43 ++++ .../core/bluetooth/BleDeviceListener.java | 4 +- .../profilometer/core/bluetooth/BleManager.java | 14 +- .../core/bluetooth/BlePingManager.java | 56 +++++ .../migration/channel/BleDeviceUartChannel.java | 9 +- .../core/migration/measure/AppConstantConfig.java | 28 +++ .../migration/measure/EncoderSensorRawData.java | 17 ++ .../core/migration/measure/ZLockList.java | 70 ++++++ .../migration/measure/drawer/AngleConverter.java | 28 +++ .../measure/drawer/CoordinateCalculator.java | 240 ++++++++++++++++++ .../measure/drawer/PulleyPointDrawer.java | 80 ++++++ .../measure/drawer/RailProfileDrawer.java | 272 +++++++++++++++++++++ .../drawer/TwoLinkArmInverseKinematics.java | 72 ++++++ .../migration/measure/drawer/type/MeasureSide.java | 18 ++ .../drawer/type/RailProfileDrawerListener.java | 34 +++ .../drawer/type/RailProfileDrawerState.java | 42 ++++ .../drawer/type/RailProfileMeasureTaskStatus.java | 32 +++ .../migration/measure/drawer/type/TrackPoint.java | 44 ++++ .../migration/measure/drawer/type/TwoObject.java | 16 ++ .../migration/measure/drawer/type/XYPoint.java | 47 ++++ .../profilometer/core/migration/type/Version.java | 29 +++ .../migration/type/enums/SubDeviceChannel.java | 6 + .../migration/type/protocol/TPMIBasicReport.java | 5 +- .../core/migration/type/protocol/TPMIPacket.java | 8 +- .../migration/type/protocol/TPMIPosReport.java | 5 +- .../profilometer/core/system/SystemService.java | 51 ++++ .../profilometer/core/system/SystemState.java | 36 --- .../profilometer/measure/AppConstantConfig.java | 28 --- .../profilometer/measure/EncoderSensorRawData.java | 17 -- .../iflytop/profilometer/measure/ZLockList.java | 70 ------ .../measure/drawer/AngleConverter.java | 28 --- .../measure/drawer/CoordinateCalculator.java | 240 ------------------ .../measure/drawer/PulleyPointDrawer.java | 80 ------ .../measure/drawer/RailProfileDrawer.java | 272 --------------------- .../drawer/TwoLinkArmInverseKinematics.java | 72 ------ .../measure/drawer/type/MeasureSide.java | 18 -- .../drawer/type/RailProfileDrawerListener.java | 34 --- .../drawer/type/RailProfileDrawerState.java | 42 ---- .../drawer/type/RailProfileMeasureTaskStatus.java | 32 --- .../measure/drawer/type/TrackPoint.java | 44 ---- .../measure/drawer/type/TwoObject.java | 16 -- .../profilometer/measure/drawer/type/XYPoint.java | 47 ---- .../model/bo/RailProfileMeasureTaskState.java | 4 +- .../service/RailProfileMeasureTaskCtrlService.java | 8 +- 49 files changed, 1295 insertions(+), 1107 deletions(-) create mode 100644 app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleDeviceDriver.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/bluetooth/BlePingManager.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/AppConstantConfig.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/EncoderSensorRawData.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/ZLockList.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/AngleConverter.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/CoordinateCalculator.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/PulleyPointDrawer.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/RailProfileDrawer.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/TwoLinkArmInverseKinematics.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/MeasureSide.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileDrawerListener.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileDrawerState.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileMeasureTaskStatus.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/TrackPoint.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/TwoObject.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/XYPoint.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/type/Version.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/migration/type/enums/SubDeviceChannel.java create mode 100644 app/src/main/java/com/iflytop/profilometer/core/system/SystemService.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/core/system/SystemState.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/AppConstantConfig.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/EncoderSensorRawData.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/ZLockList.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/AngleConverter.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/CoordinateCalculator.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/PulleyPointDrawer.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/RailProfileDrawer.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/TwoLinkArmInverseKinematics.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/type/MeasureSide.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileDrawerListener.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileDrawerState.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileMeasureTaskStatus.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/type/TrackPoint.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/type/TwoObject.java delete mode 100644 app/src/main/java/com/iflytop/profilometer/measure/drawer/type/XYPoint.java diff --git a/app/src/main/java/com/iflytop/profilometer/api/auth/AuthApi.java b/app/src/main/java/com/iflytop/profilometer/api/auth/AuthApi.java index d8953e8..6f7693f 100644 --- a/app/src/main/java/com/iflytop/profilometer/api/auth/AuthApi.java +++ b/app/src/main/java/com/iflytop/profilometer/api/auth/AuthApi.java @@ -3,7 +3,7 @@ package com.iflytop.profilometer.api.auth; import android.content.Context; import com.iflytop.profilometer.common.result.Result; -import com.iflytop.profilometer.core.system.SystemState; +import com.iflytop.profilometer.core.system.SystemService; import com.iflytop.profilometer.dao.UserDao; import com.iflytop.profilometer.model.entity.AppUser; @@ -24,7 +24,7 @@ public class AuthApi { UserDao userDao = new UserDao(context); AppUser user = userDao.login(username, password); if (user != null) { - SystemState.getInstance().setCurrentUser(user); + SystemService.getInstance().setCurrentUser(user); return Result.success(user); } else { return Result.failed(); @@ -35,7 +35,7 @@ public class AuthApi { * 登出 */ public String logout() { - SystemState.getInstance().setCurrentUser(null); + SystemService.getInstance().setCurrentUser(null); return Result.success(); } @@ -43,7 +43,7 @@ public class AuthApi { * 获取当前登录用户 */ public String current() { - AppUser user = SystemState.getInstance().getCurrentUser(); + AppUser user = SystemService.getInstance().getCurrentUser(); return Result.success(user); } } diff --git a/app/src/main/java/com/iflytop/profilometer/api/ble/BleApi.java b/app/src/main/java/com/iflytop/profilometer/api/ble/BleApi.java index 09964de..e3520d2 100644 --- a/app/src/main/java/com/iflytop/profilometer/api/ble/BleApi.java +++ b/app/src/main/java/com/iflytop/profilometer/api/ble/BleApi.java @@ -6,9 +6,8 @@ import android.util.Log; import com.iflytop.profilometer.api.ws.BleWebsocketManager; import com.iflytop.profilometer.api.ws.DeviceStateWebsocketManager; import com.iflytop.profilometer.common.result.Result; -import com.iflytop.profilometer.core.bluetooth.BleDeviceListener; import com.iflytop.profilometer.core.bluetooth.BleManager; -import com.iflytop.profilometer.core.migration.channel.BleDeviceUartChannel; +import com.iflytop.profilometer.core.bluetooth.BlePingManager; /** * 蓝牙接口 @@ -56,6 +55,8 @@ public class BleApi { try { BleManager.getInstance().connectToDevice(mac); DeviceStateWebsocketManager.getInstance().startWsPush(); + + BlePingManager.getInstance().startPing(); return Result.success(); } catch (Exception e) { Log.e(TAG, "链接蓝牙设备失败", e); @@ -69,6 +70,7 @@ public class BleApi { public String disconnect() { try { BleManager.getInstance().disconnect(); + DeviceStateWebsocketManager.getInstance().stopWsPush(); return Result.success(); } catch (Exception e) { Log.e(TAG, "断开蓝牙设备链接失败", e); diff --git a/app/src/main/java/com/iflytop/profilometer/api/measure/MeasureApi.java b/app/src/main/java/com/iflytop/profilometer/api/measure/MeasureApi.java index 1960183..a1856cd 100644 --- a/app/src/main/java/com/iflytop/profilometer/api/measure/MeasureApi.java +++ b/app/src/main/java/com/iflytop/profilometer/api/measure/MeasureApi.java @@ -2,6 +2,10 @@ package com.iflytop.profilometer.api.measure; import android.content.Context; +import com.iflytop.profilometer.common.result.Result; +import com.iflytop.profilometer.core.bluetooth.BleDeviceDriver; +import com.iflytop.profilometer.core.system.SystemService; + /** * 测量接口 */ @@ -12,5 +16,22 @@ public class MeasureApi { this.context = context.getApplicationContext(); } + /** + * 开始测量 + */ + public String start() { + BleDeviceDriver bleDeviceDriver = SystemService.getInstance().getBleDeviceDriver(); + bleDeviceDriver.startSampling(); + return Result.success(); + } + + /** + * 停止测量 + */ + public String stop() { + BleDeviceDriver bleDeviceDriver = SystemService.getInstance().getBleDeviceDriver(); + bleDeviceDriver.stopSampling(); + return Result.success(); + } } diff --git a/app/src/main/java/com/iflytop/profilometer/api/measure/MeasureRoutes.kt b/app/src/main/java/com/iflytop/profilometer/api/measure/MeasureRoutes.kt index 3f59f31..687c795 100644 --- a/app/src/main/java/com/iflytop/profilometer/api/measure/MeasureRoutes.kt +++ b/app/src/main/java/com/iflytop/profilometer/api/measure/MeasureRoutes.kt @@ -15,12 +15,16 @@ fun Routing.measureRoutes(context: Context) { * 开始测量 */ post("/api/measure/start") { + val jsonResponse = api.start() + call.respondText(jsonResponse, ContentType.Application.Json) } /** * 停止测量 */ post("/api/measure/stop") { + val jsonResponse = api.stop() + call.respondText(jsonResponse, ContentType.Application.Json) } /** diff --git a/app/src/main/java/com/iflytop/profilometer/api/ws/DeviceStateWebsocketManager.java b/app/src/main/java/com/iflytop/profilometer/api/ws/DeviceStateWebsocketManager.java index 8d13322..bd697d6 100644 --- a/app/src/main/java/com/iflytop/profilometer/api/ws/DeviceStateWebsocketManager.java +++ b/app/src/main/java/com/iflytop/profilometer/api/ws/DeviceStateWebsocketManager.java @@ -1,10 +1,9 @@ package com.iflytop.profilometer.api.ws; import android.annotation.SuppressLint; -import android.content.Context; import com.iflytop.profilometer.core.system.DeviceState; -import com.iflytop.profilometer.core.system.SystemState; +import com.iflytop.profilometer.core.system.SystemService; import com.iflytop.profilometer.core.websocket.WebSocketManager; import java.util.HashMap; @@ -38,7 +37,7 @@ public class DeviceStateWebsocketManager { public void startWsPush() { stopWsPush(); scheduledTask = scheduler.scheduleWithFixedDelay(() -> { - DeviceState deviceState = SystemState.getInstance().getDeviceState(); + DeviceState deviceState = SystemService.getInstance().getDeviceState(); Map map = new HashMap<>(); map.put("type", "peripheral-status"); map.put("data", deviceState); diff --git a/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleDeviceDriver.java b/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleDeviceDriver.java new file mode 100644 index 0000000..6f9486d --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleDeviceDriver.java @@ -0,0 +1,43 @@ +package com.iflytop.profilometer.core.bluetooth; + +import com.iflytop.profilometer.core.migration.channel.BleDeviceUartChannel; +import com.iflytop.profilometer.core.migration.factory.TPMIPacketFactory; +import com.iflytop.profilometer.core.migration.measure.AppConstantConfig; +import com.iflytop.profilometer.core.migration.type.protocol.TPMIPacket; +import com.iflytop.profilometer.model.bo.RailProfileMeasureTaskState; + +public class BleDeviceDriver { + + private final BleDeviceUartChannel uartChannel; + + private RailProfileMeasureTaskState taskState = new RailProfileMeasureTaskState(); + + public BleDeviceDriver(BleDeviceUartChannel channel) { + this.uartChannel = channel; + } + + /** + * 发送PING指令（检查设备是否在线） + */ + public void ping() { + TPMIPacket packet = TPMIPacketFactory.buildPingCmd(); + uartChannel.sendCommand(packet); + } + + /** + * 启动位移采样（启动测量） + */ + public void startSampling() { + TPMIPacket packet = TPMIPacketFactory.buildStartPosSampleCmd(AppConstantConfig.POINT_REPORT_PERIOD); + uartChannel.sendCommand(packet); + } + + /**BleWebsocketManager + * 停止位移采样（结束测量） + */ + public void stopSampling() { + TPMIPacket packet = TPMIPacketFactory.buildStopPosSampleCmd(); + uartChannel.sendCommand(packet); + } + +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleDeviceListener.java b/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleDeviceListener.java index ad06a19..62bff05 100644 --- a/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleDeviceListener.java +++ b/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleDeviceListener.java @@ -6,7 +6,7 @@ import com.iflytop.profilometer.core.migration.type.protocol.TPMIBasicReport; import com.iflytop.profilometer.core.migration.type.protocol.TPMIPacket; import com.iflytop.profilometer.core.migration.type.protocol.TPMIPosReport; import com.iflytop.profilometer.core.system.DeviceState; -import com.iflytop.profilometer.core.system.SystemState; +import com.iflytop.profilometer.core.system.SystemService; public class BleDeviceListener implements BleDeviceOnReportListener { private static final String TAG = "MyBleDeviceListener"; @@ -14,7 +14,7 @@ public class BleDeviceListener implements BleDeviceOnReportListener { @Override public void onBasicReport(TPMIBasicReport report) { Log.i(TAG, "基础上报包:" + report); - DeviceState deviceState = SystemState.getInstance().getDeviceState(); + DeviceState deviceState = SystemService.getInstance().getDeviceState(); deviceState.setTemperature(report.getTemperature()); deviceState.setInclinatorX(report.getInclinatorX()); deviceState.setInclinatorY(report.getInclinatorY()); diff --git a/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleManager.java b/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleManager.java index 6864892..1735ba7 100644 --- a/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleManager.java +++ b/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BleManager.java @@ -28,6 +28,7 @@ import androidx.core.app.ActivityCompat; import androidx.core.content.ContextCompat; import com.iflytop.profilometer.core.migration.channel.BleDeviceUartChannel; +import com.iflytop.profilometer.core.system.SystemService; import java.util.ArrayList; import java.util.HashMap; @@ -54,7 +55,7 @@ public class BleManager { //【通信部分】请根据你的设备替换下列 UUID public static final UUID SERVICE_UUID = UUID.fromString("6e400001-b5a3-f393-e0a9-e50e24dcca9e"); - public static final UUID WRITE_CHARACTERISTIC_UUID = UUID.fromString("0000fff1-0000-1000-8000-00805f9b34fb"); + public static final UUID WRITE_CHARACTERISTIC_UUID = UUID.fromString("6e400002-b5a3-f393-e0a9-e50e24dcca9e"); public static final UUID NOTIFY_CHARACTERISTIC_UUID = UUID.fromString("6e400003-b5a3-f393-e0a9-e50e24dcca9e"); public static final UUID CLIENT_CHARACTERISTIC_CONFIG_UUID = UUID.fromString("00002902-0000-1000-8000-00805f9b34fb"); @@ -149,7 +150,7 @@ public class BleManager { // 收到通知数据后，将数据通过回调接口传递出去 if (NOTIFY_CHARACTERISTIC_UUID.equals(characteristic.getUuid())) { byte[] data = characteristic.getValue(); - Log.d(TAG, "收到通知数据: " + new String(data)); +// Log.d(TAG, "收到通知数据: " + new String(data)); if (bleDataListener != null) { bleDataListener.onDataReceived(data); } @@ -191,6 +192,7 @@ public class BleManager { } return instance; } + public static synchronized BleManager getInstance() { return instance; } @@ -351,7 +353,7 @@ public class BleManager { BluetoothDevice device = bluetoothAdapter.getRemoteDevice(macAddress); bluetoothGatt = device.connectGatt(context, false, gattCallback); - BleDeviceUartChannel channel = new BleDeviceUartChannel(); + BleDeviceUartChannel channel = SystemService.getInstance().getChannel(); channel.init(); channel.setDeviceOnReportListener(new BleDeviceListener()); @@ -362,11 +364,9 @@ public class BleManager { /** * 向当前已连接的 BLE 设备发送数据。 - * - * @param message 要发送的字符串数据（根据设备协议，可转换为其他格式） */ @SuppressLint("MissingPermission") - public void sendMessage(String message) { + public void sendMessage(byte[] rawpacket) { if (bluetoothGatt == null) { Log.e(TAG, "没有连接的设备，无法发送消息"); return; @@ -381,7 +381,7 @@ public class BleManager { Log.e(TAG, "未发现写特征，请确认写特征 UUID 是否正确"); return; } - writeCharacteristic.setValue(message.getBytes()); + writeCharacteristic.setValue(rawpacket); boolean result = bluetoothGatt.writeCharacteristic(writeCharacteristic); if (!result) { Log.e(TAG, "调用 writeCharacteristic 失败"); diff --git a/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BlePingManager.java b/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BlePingManager.java new file mode 100644 index 0000000..06534af --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/bluetooth/BlePingManager.java @@ -0,0 +1,56 @@ +package com.iflytop.profilometer.core.bluetooth; + +import android.annotation.SuppressLint; +import android.bluetooth.BluetoothDevice; + +import com.iflytop.profilometer.core.system.SystemService; +import com.iflytop.profilometer.core.websocket.WebSocketManager; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.ScheduledFuture; +import java.util.concurrent.TimeUnit; + +import cn.hutool.json.JSONUtil; + +public class BlePingManager { + private static BlePingManager instance; + private final ScheduledExecutorService scheduler = Executors.newScheduledThreadPool(1); + private ScheduledFuture scheduledTask; + + private BlePingManager() { + } + + public static synchronized BlePingManager getInstance() { + if (instance == null) { + instance = new BlePingManager(); + } + return instance; + } + + + /** + * 开始定时任务 + */ + @SuppressLint("MissingPermission") + public void startPing() { + stopPing(); + scheduledTask = scheduler.scheduleWithFixedDelay(() -> { + BleDeviceDriver bleDeviceDriver = SystemService.getInstance().getBleDeviceDriver(); + bleDeviceDriver.ping(); + }, 0, 5000, TimeUnit.MILLISECONDS); + } + + /** + * 结束定时任务 + */ + public void stopPing() { + if (scheduledTask != null && !scheduledTask.isCancelled()) { + scheduledTask.cancel(false); + } + } + +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/channel/BleDeviceUartChannel.java b/app/src/main/java/com/iflytop/profilometer/core/migration/channel/BleDeviceUartChannel.java index 440cf5e..cfa3583 100644 --- a/app/src/main/java/com/iflytop/profilometer/core/migration/channel/BleDeviceUartChannel.java +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/channel/BleDeviceUartChannel.java @@ -34,12 +34,13 @@ public class BleDeviceUartChannel { this.deviceOnReportListener = listener; } - public void sendCommand(byte[] command) { - BleManager.getInstance().sendMessage(new String(command)); + public void sendCommand(TPMIPacket tx) { + Log.e("BLE", "TX PACKET :" + tx); + BleManager.getInstance().sendMessage(tx.rawpacket); } private void handleReceivedData(byte[] data) { - Log.d("BLE", "收到原始数据: " + Arrays.toString(data)); +// Log.d("BLE", "收到原始数据: " + Arrays.toString(data)); // 追加到缓冲区 if (bufferLen + data.length > buffer.length) { @@ -53,7 +54,7 @@ public class BleDeviceUartChannel { while (offset < bufferLen) { var report = TPMIPacketFactory.analysisRawRxData(buffer, bufferLen, offset); if (report.result.equals(TPMIPacketFactory.PacketAnalysisResult.SUC)) { - Log.d("BLE", "BLE协议包解析成功: " + report.packet); +// Log.d("BLE", "BLE协议包解析成功: " + report.packet); if (reportListener != null) { reportListener.onReportReceived(report.packet); } diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/AppConstantConfig.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/AppConstantConfig.java new file mode 100644 index 0000000..1ad4da6 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/AppConstantConfig.java @@ -0,0 +1,28 @@ +package com.iflytop.profilometer.core.migration.measure; + +public class AppConstantConfig { + public static final int HEARTBEAT_INTERVAL = 10000; + + //100ms上报一次点位 + public static final int POINT_REPORT_PERIOD = 5; + + /* + * 轮廓绘制，启动信号判断相关参数 + * 逻辑: + * 滑轮在指定区域静止超过一定时间 + * + * 参数: + * 区域判断阈值（圆心附近的长方形） + * START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_HIGHT 高度阈值 + * START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_WEIGHT 宽度阈值 + * 时间判断阈值（毫秒） + * OUTLINE_START_RECORD_SIG_JUDGE_TIME_THRESHOLD 毫秒 + * 静止判断位置阈值(毫米) + * STILL_JUDGE_THRESHOLD + */ + + public static final int OUTLINE_START_RECORD_SIG_JUDGE_TIME_THRESHOLD = 1000; + public static final int START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_HIGHT = 30; + public static final int START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_WEIGHT = 50; + public static final double STILL_JUDGE_THRESHOLD = 40; +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/EncoderSensorRawData.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/EncoderSensorRawData.java new file mode 100644 index 0000000..9dfea9b --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/EncoderSensorRawData.java @@ -0,0 +1,17 @@ +package com.iflytop.profilometer.core.migration.measure; + +public class EncoderSensorRawData { + public double arm1Angle = 0; //0->360 + public double arm2Angle = 0; //0->360 + + public EncoderSensorRawData(double arm1Angle, double arm2Angle) { + this.arm1Angle = arm1Angle; + this.arm2Angle = arm2Angle; + } + + public EncoderSensorRawData() {} + + public String toString() { + return String.format("(%+.5f, %+.5f)", arm1Angle, arm2Angle); + } +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/ZLockList.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/ZLockList.java new file mode 100644 index 0000000..fbe2567 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/ZLockList.java @@ -0,0 +1,70 @@ +package com.iflytop.profilometer.core.migration.measure; + +import java.util.ArrayList; +import java.util.List; + +public class ZLockList { + private List list; + private Integer maxCount = 0; + + public ZLockList() { + this.list = new ArrayList<>(); + } + + public ZLockList(Integer maxCount) { + this.list = new ArrayList<>(); + this.maxCount = maxCount; + } + + synchronized public void add(T t) { + list.add(t); + if (maxCount > 0 && list.size() > maxCount) { + list.removeFirst(); + } + } + + synchronized public void clear() { + list.clear(); + } + + synchronized public void remove(T t) { + list.remove(t); + } + + synchronized public T getLast(Integer index) { + if (list.size() > index) { + return list.get(list.size() - 1 - index); + } + return null; + } + + synchronized public List getList() { + return new ArrayList<>(list); + } + + synchronized public void removeTheFirst() { + list.removeFirst(); + } + + synchronized public T get(Integer index) { + if (list.size() > index) { + return list.get(index); + } + return null; + } + + synchronized public void pop(int num) { + for (int i = 0; i < num; i++) { + list.removeFirst(); + } + } + + + synchronized public boolean isEmpty() { + return list.isEmpty(); + } + + synchronized public int size() { + return list.size(); + } +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/AngleConverter.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/AngleConverter.java new file mode 100644 index 0000000..526dc58 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/AngleConverter.java @@ -0,0 +1,28 @@ +package com.iflytop.profilometer.core.migration.measure.drawer; + +public class AngleConverter { + + public static Integer angleToInt32(double angle) { + angle = limitAngle(angle); + return (int) (angle / 180.0 * Integer.MAX_VALUE); + } + + public static double int32ToAngle(Integer angle) { + double val = angle / (double) Integer.MAX_VALUE * 180; + return limitAngle(val); + } + + public static double limitAngle(double angle) { + while (!(angle >= -180) || !(angle < 180)) { + if (angle > 180) { + angle -= 360; + } + if (angle <= -180) { + angle += 360; + } + } + return angle; + } + + +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/CoordinateCalculator.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/CoordinateCalculator.java new file mode 100644 index 0000000..dd03dc7 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/CoordinateCalculator.java @@ -0,0 +1,240 @@ +package com.iflytop.profilometer.core.migration.measure.drawer; + + +import static java.lang.Math.PI; +import static java.lang.Math.abs; + +import com.iflytop.profilometer.core.migration.measure.drawer.type.TrackPoint; +import com.iflytop.profilometer.core.migration.measure.drawer.type.TwoObject; +import com.iflytop.profilometer.core.migration.measure.drawer.type.XYPoint; + +public class CoordinateCalculator { + + + /** + * 两轴机械臂坐标转换为XY坐标 + * @param arm0Length 机械臂0长度 + * @param arm0Rad 机械臂0弧度 + * @param arm1Length 机械臂1长度 + * @param arm1Rad 机械臂1弧度 + * @return XY坐标 + */ + static public XYPoint twoAxisArmCoordTransformToXYPointV1(Double arm0Length, Double arm0Rad, Double arm1Length, Double arm1Rad) { + XYPoint xyPoint = new XYPoint(); + double lval = calculateThirdSide(arm0Length, arm1Length, Math.PI - arm1Rad); + + if (lval < 0.000001) { + xyPoint.x = 0.0; xyPoint.y = 0.0; + } else { + double rad0 = calculateAngle(arm0Length, lval, arm1Length); + double lrad = 0.0; + if (arm1Rad > 0) { + lrad = arm0Rad + rad0; + } else { + lrad = arm0Rad - rad0; + } + xyPoint.x = lval * Math.sin(lrad); + xyPoint.y = lval * Math.cos(lrad); + } return xyPoint; + } + + static public XYPoint twoAxisArmCoordTransformToXYPointV2(Double arm0Length, Double arm0Rad, Double arm1Length, Double arm1Rad) { + XYPoint xyPoint = new XYPoint(); + xyPoint.x = arm0Length * Math.sin(arm0Rad) + arm1Length * Math.sin(arm0Rad + arm1Rad); + xyPoint.y = arm0Length * Math.cos(arm0Rad) + arm1Length * Math.cos(arm0Rad + arm1Rad); + return xyPoint; + } + + static public XYPoint twoAxisArmCoordTransformToXYPoint(Double arm0Length, Double arm0Rad, Double arm1Length, Double arm1Rad) { + return twoAxisArmCoordTransformToXYPointV2(arm0Length, arm0Rad, arm1Length, arm1Rad); + } + + + /** + * 计算切点坐标 + * @param point0 零点 + * @param refpoint 曲线参考点 + * @param radius 半径 + * @return 经过point0且垂直于point0-refpoint的曲线上距离零点为radius的点的坐标 + * + * + * target1 + * | + * | + * | radius + * | + * | + * ----------ref----point0------------- + * | + * | + * | radius + * | + * | + * target2 + * + */ + static public TwoObject calculateTangentPointCoordinates(XYPoint point0, XYPoint refpoint, double radius, double mindistance) { + if (point0 == null || refpoint == null) { + return new TwoObject<>(null, null); + } + + if (distance(point0, refpoint) < mindistance) { + return new TwoObject<>(null, null); + } + + double dx = refpoint.x - point0.x; + double dy = refpoint.y - point0.y; + + + double x = Math.sqrt(radius * radius * dx * dx / (dx * dx + dy * dy)); + double y = Math.sqrt(radius * radius * dy * dy / (dx * dx + dy * dy)); + + y = dy > 0 ? y : -y; + x = dx > 0 ? x : -x; + + //顺时针旋转90度 + double new1x = y; + double new1y = -x; + + //逆时针旋转90度 + double new2x = -y; + double new2y = x; + + XYPoint t1 = new XYPoint(point0.x + new1x, point0.y + new1y); + XYPoint t2 = new XYPoint(point0.x + new2x, point0.y + new2y); + + return new TwoObject<>(t1, t2); + } + + static public TwoObject calculateTangentPointCoordinates(TrackPoint point0, TrackPoint refpoint, double radius) { + XYPoint xypoint0 = point0.toXYPoint(); + XYPoint xyRefpoint = refpoint.toXYPoint(); + var ret = calculateTangentPointCoordinates(xypoint0, xyRefpoint, radius, 0); + return new TwoObject<>(new TrackPoint(ret.r, point0.date), new TrackPoint(ret.l, point0.date)); + } + + + // + // PRIVATE + // + static double calculateThirdSide(double a, double b, double angleInRadians) { + // 计算第三条边的平方 + double cSquared = a * a + b * b - 2 * a * b * Math.cos(angleInRadians); + + // 返回第三条边的长度 + return Math.sqrt(cSquared); + } + + static double calculateAngle(double side1, double side2, double oppositeSide) { + //防止由于double精度问题导致的结果出现NaN + if (oppositeSide > side1 + side2) { + return PI; + } + + //防止由于double精度问题导致的结果出现NaN + if (oppositeSide < Math.abs(side1 - side2)) { + return 0; + } + + // 使用余弦定理计算夹角的余弦值 + double cosTheta = (side1 * side1 + side2 * side2 - oppositeSide * oppositeSide) / (2 * side1 * side2); + + // 使用反余弦函数求出夹角的弧度 + var ret = abs(Math.acos(cosTheta)); if (Double.isNaN(ret)) { + throw new RuntimeException(String.format("calculateAngle ret NaN: side1=%+.4f,side2=%+.4f,oppositeSide=%+.4f", side1, side2, oppositeSide)); + } return ret; + } + + static public double distance(XYPoint p0, XYPoint p1) { + if (p0 == null || p1 == null) { + return 0; + } + return Math.sqrt(Math.pow(p0.x - p1.x, 2) + Math.pow(p0.y - p1.y, 2)); + } + + // + // TEST + // + static void Assert(boolean condition, String message) { + if (!condition) { + throw new RuntimeException(message); + } + } + + static void testCalculateThirdSide(double a, double b, double angleInRadians, double expected) { + double c = calculateThirdSide(a, b, angleInRadians); + Assert(abs(c - expected) < 0.0001, String.format("testCalculateThirdSide: a=%+.4f,b=%+.4f,angleInRadians=%+.4f --> c=%+.4f", a, b, angleInRadians, c)); + } + + static void testcalculateAngle(double side1, double side2, double oppositeSide, double expected) { + double angle = calculateAngle(side1, side2, oppositeSide); + Assert(abs(angle - expected) < 0.0001, + String.format("testcalculateAngle: side1=%+.4f,side2=%+.4f,oppositeSide=%+.4f --> angle=%+.4f(%+.4f)", side1, side2, oppositeSide, angle, Math.toDegrees(angle))); + } + + static void testTwoAxisArmCoordTransformToXYPoint(double arm0Length, double arm0Rad, double arm1Length, double arm1Rad, double expectedX, double expectedY) { + XYPoint xyPoint = twoAxisArmCoordTransformToXYPoint(arm0Length, arm0Rad, arm1Length, arm1Rad); + Assert(abs(xyPoint.x - expectedX) < 0.0001 && abs(xyPoint.y - expectedY) < 0.0001, + String.format("testCalculator: arm0Length=%+.4f,arm0Rad=%+.4f,arm1Length=%+.4f,arm1Rad=%+.4f --> x=%+.4f,y=%+.4f", arm0Length, arm0Rad, arm1Length, arm1Rad, xyPoint.x, xyPoint.y)); + } + + static public void testCalculateTangentPointCoordinates(XYPoint point0, XYPoint refpoint, double radius, XYPoint expected0, XYPoint expected1) { + var ret = calculateTangentPointCoordinates(point0, refpoint, radius,0); + Assert(!(expected0 == null || expected1 == null || !expected0.equals(ret.r) || !expected1.equals(ret.l)), + String.format("check testCalculateTangentPointCoordinates(%s,%s,%s) fail\n%s", point0, refpoint, radius, + String.format("%.4f\t%.4f\n%.4f\t%.4f\n%.4f\t%.4f\n%.4f\t%.4f", point0.x, point0.y, refpoint.x, refpoint.y, ret.r.x, ret.r.y, ret.l.x, ret.l.y))); + } + + public static void main(String[] args) { + + // log.info("{}", + // twoAxisArmCoordTransformToXYPoint(53.0, 18.6326 / 180 * Math.PI, 96.0, -91.8046 / 180 * Math.PI)); + // + // CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(AppConstantConfig.ARM1_LENGTH_MM, 0.0, AppConstantConfig.ARM2_LENGTH_MM, 0.0, 0.0, 2.0); + + + CoordinateCalculator.testCalculateThirdSide(1, 1, 0, 0.0); + CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI / 4, 0.7654); + CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI / 2, 1.4142); + CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI * 3 / 4, 1.8478); + CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI, 2.0); + CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI * 1 / 4 + Math.PI, 1.8478); + + CoordinateCalculator.testcalculateAngle(1, 1, 1, Math.PI / 3); + CoordinateCalculator.testcalculateAngle(1, 2, 3, Math.PI); + CoordinateCalculator.testcalculateAngle(1, 2, 3.0000001, Math.PI); + CoordinateCalculator.testcalculateAngle(1, 2, 0.99999999, 0); + CoordinateCalculator.testcalculateAngle(1, 1, Math.sqrt(2), Math.PI / 2); + + CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, 0.0, 1.0, 0.0, 0.0, 2.0); + CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, 0.0, 1.0, Math.PI, 0.0, 0.0); + CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, PI / 4, 1.0, 0, 1.4142, 1.4142); + CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, -PI / 4, 1.0, 0, -1.4142, 1.4142); + // CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, PI / 4, 1.0, PI * 5 / 4, 0.7071, -0.2929); + CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, PI / 4, 1.0, -PI * 1 / 2, 0.0, 1.4142); + CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, PI / 4, 1.0, PI * 1 / 2, 1.4142, 0.0); + CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, -PI / 4, 1.0, -PI * 1 / 2, -1.4142, 0.0); + CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, -PI / 4, 1.0, PI * 1 / 2, 0.0, 1.4142); + + CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(1, 0), 1, + new XYPoint(0, -1), new XYPoint(0, 1)); + CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(2, 2), Math.sqrt(2), + new XYPoint(1, -1), new XYPoint(-1, 1)); + CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(1, 1), new XYPoint(2, 2), Math.sqrt(2), + new XYPoint(2, 0), new XYPoint(0, 2)); + CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(-2, -2), Math.sqrt(2), + new XYPoint(-1, 1), new XYPoint(1, -1)); + CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(1, 1), new XYPoint(-2, -2), Math.sqrt(2), + new XYPoint(0, 2), new XYPoint(2, 0)); + CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(-1, 1), new XYPoint(-2, 2), Math.sqrt(2), + new XYPoint(0, 2), new XYPoint(-2, 0)); + CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(1, -1), new XYPoint(-2, 2), Math.sqrt(2), + new XYPoint(2, 0), new XYPoint(0, -2)); + CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(1, 2), Math.sqrt(5), + new XYPoint(2, -1), new XYPoint(-2, 1)); + CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(4, 6), Math.sqrt(52), + new XYPoint(6, -4), new XYPoint(-6, 4)); + + } + +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/PulleyPointDrawer.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/PulleyPointDrawer.java new file mode 100644 index 0000000..c14f445 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/PulleyPointDrawer.java @@ -0,0 +1,80 @@ +package com.iflytop.profilometer.core.migration.measure.drawer; + + +import com.iflytop.profilometer.core.migration.measure.EncoderSensorRawData; +import com.iflytop.profilometer.core.migration.measure.drawer.type.XYPoint; + +public class PulleyPointDrawer { + static class Config { + public double arm1Length = 0; + public double arm2Length = 0; + public double pulleyRadius = 0; + public double profilometerHight = 0; + } + + Config cfg = new Config(); // 配置 + XYPoint lastPulleyPoint = null; + + public PulleyPointDrawer() { + + } + + synchronized public void startDraw(double arm1Length, double arm2Length, double pulleyRadius, + double profilometerHight) { +// log.info("startDraw arm1Length:{} arm2Length:{} pulleyRadius:{} profilometerHight:{}", arm1Length, arm2Length, +// pulleyRadius, profilometerHight); + this.cfg.arm1Length = arm1Length; + this.cfg.arm2Length = arm2Length; + this.cfg.pulleyRadius = pulleyRadius; + this.cfg.profilometerHight = profilometerHight; + } + + synchronized public void updateConfig(double arm1Length, double arm2Length, double pulleyRadius, + double profilometerHight) { +// log.info("updateConfig arm1Length:{} arm2Length:{} pulleyRadius:{} profilometerHight:{}", arm1Length, arm2Length, +// pulleyRadius, profilometerHight); + this.cfg.arm1Length = arm1Length; + this.cfg.arm2Length = arm2Length; + this.cfg.pulleyRadius = pulleyRadius; + this.cfg.profilometerHight = profilometerHight; + } + + synchronized public void stopDraw() { + ; + } + + + public EncoderSensorRawData convertToEncoderSensorRawData(Integer encoder1, Integer encoder2) { + EncoderSensorRawData rawData = new EncoderSensorRawData(); + rawData.arm1Angle = encoder1 * 1.0 / 2147483647 * 180; + rawData.arm2Angle = encoder2 * 1.0 / 2147483647 * 180; + return rawData; + } + + /** + * 将编码器数值转换为滑轮坐标，并自动过滤掉相同位置的点 + * @param angle1 编码器0的点 + * @param angle2 编码器1的点 + * @return 滑轮坐标 + */ + public XYPoint convertToPulleyPoint(double angle1, double angle2) { + //编码器数值转弧度 + double arm1rad = angle1 / 180.0 * Math.PI; + double arm2rad = angle2 / 180.0 * Math.PI; + XYPoint newPulleyPoint = CoordinateCalculator.twoAxisArmCoordTransformToXYPointV2(cfg.arm1Length, arm1rad, cfg.arm2Length, arm2rad); + newPulleyPoint.y = newPulleyPoint.y - cfg.profilometerHight + cfg.pulleyRadius; + // if (isTheSamePoint(lastPulleyPoint, newPulleyPoint)) { + // return null; + // } + lastPulleyPoint = newPulleyPoint; + return newPulleyPoint; + } + + static private boolean isTheSamePoint(XYPoint p0, XYPoint p1) { + if (p0 == null || p1 == null) { + return false; + } + return CoordinateCalculator.distance(p0, p1) < 0.003; + } + +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/RailProfileDrawer.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/RailProfileDrawer.java new file mode 100644 index 0000000..db83571 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/RailProfileDrawer.java @@ -0,0 +1,272 @@ +package com.iflytop.profilometer.core.migration.measure.drawer; + +import static com.iflytop.profilometer.core.migration.measure.AppConstantConfig.STILL_JUDGE_THRESHOLD; + +import com.iflytop.profilometer.core.migration.measure.AppConstantConfig; +import com.iflytop.profilometer.core.migration.measure.drawer.type.MeasureSide; +import com.iflytop.profilometer.core.migration.measure.drawer.type.RailProfileDrawerListener; +import com.iflytop.profilometer.core.migration.measure.drawer.type.RailProfileDrawerState; +import com.iflytop.profilometer.core.migration.measure.drawer.type.RailProfileMeasureTaskStatus; +import com.iflytop.profilometer.core.migration.measure.drawer.type.XYPoint; + +public class RailProfileDrawer { + public static final double PRECISION = 0.1;// mm 满足13cm，1500个点 + + public static class Config { + public double arm1Length = 0; + public double arm2Length = 0; + public double pulleyRadius = 0; + public double profilometerHight = 0; + public int startSigJudgeThreshold = 100; + } + + Config cfg = new Config(); // 配置 + RailProfileDrawerState state = new RailProfileDrawerState(); + RailProfileDrawerListener railProfileDrawerListener; + MeasureSide startSide = MeasureSide.LEFT; + + public RailProfileDrawer(RailProfileDrawerListener railProfileDrawerListener) { + this.railProfileDrawerListener = railProfileDrawerListener; + } + + + synchronized public void startDraw(double arm1Length, double arm2Length, double pulleyRadius, + double profilometerHight) { +// log.info("startDraw arm1Length:{} arm2Length:{} pulleyRadius:{} profilometerHight:{}", arm1Length, arm2Length, +// pulleyRadius, profilometerHight); + this.cfg.arm1Length = arm1Length; + this.cfg.arm2Length = arm2Length; + this.cfg.pulleyRadius = pulleyRadius; + this.cfg.profilometerHight = profilometerHight; + this.cfg.startSigJudgeThreshold = AppConstantConfig.OUTLINE_START_RECORD_SIG_JUDGE_TIME_THRESHOLD / AppConstantConfig.POINT_REPORT_PERIOD; + state.resetRecordContext(); + state.state = RailProfileMeasureTaskStatus.WAITING_FOR_MEASURING; + } + + synchronized public void stopDraw() { + state.state = RailProfileMeasureTaskStatus.IDLE; + } + + synchronized public RailProfileDrawerState getState() { + return state; + } + + synchronized public Config getCfg() { + return cfg; + } + + boolean isInState(RailProfileMeasureTaskStatus... state) { + for (RailProfileMeasureTaskStatus s : state) { + if (this.state.state.equals(s)) { + return true; + } + } + return false; + } + + + // public XYPoint convertToPulleyPoint(Integer encoder0, Integer encoder1) { + // //编码器数值转弧度 + // double arm1rad = encoder0 * 1.0 / AppConstantConfig.ENCODER_ACCURACY * Math.PI; + // double arm2rad = encoder1 * 1.0 / AppConstantConfig.ENCODER_ACCURACY * Math.PI; + // XYPoint newPulleyPoint = twoAxisArmCoordTransformToXYPointV2(cfg.arm1Length, arm1rad, cfg.arm2Length, arm2rad); + // } + + synchronized public void processRawEncoderData(Integer data1, Integer data2) { + data1 = data1 & 0xFFFFF000; + data2 = data2 & 0xFFFFF000; + double arm1rad = data1 * 1.0 / 2147483647 * Math.PI; + double arm2rad = data2 * 1.0 / 2147483647 * Math.PI; + processEncoderData(arm1rad, arm2rad); + } + + synchronized private void processEncoderData(double rad1, double rad2) { + if (isInState(RailProfileMeasureTaskStatus.IDLE, RailProfileMeasureTaskStatus.FINISHED)) { + return; + } + + //角度转换成坐标 + XYPoint newPulleyPoint = twoAxisArmCoordTransformToXYPointV2(cfg.arm1Length, rad1, cfg.arm2Length, rad2); + railProfileDrawerListener.onNewPulleyCentroidPos(newPulleyPoint);// 新的质心点 + newPulleyPoint.y = newPulleyPoint.y - cfg.profilometerHight; + boolean isTheSamePoint = isTheSamePoint(state.lastPulleyPoint, newPulleyPoint); + XYPoint lastPulleyPoint = state.lastPulleyPoint; + if (!isTheSamePoint) { + state.lastPulleyPoint = newPulleyPoint; + } + + //位置判断 + state.isInStartPosArea = isInTheStartPos(newPulleyPoint); + + //计算速度 + double distance = distance(lastPulleyPoint, newPulleyPoint); + int speedNowCompute = (int) (distance / PRECISION * 100); + if (isInState(RailProfileMeasureTaskStatus.MEASURING)) { + this.state.speedNow = speedNowCompute; + if (speedNowCompute > 100) { + if (!state.errorSpeedDetected) + railProfileDrawerListener.onErrorSpeedDetected();// 检测到速度异常 + state.errorSpeedDetected = true; + } else { + state.errorSpeedDetected = false; + } + } else { + this.state.speedNow = 0; + state.errorSpeedDetected = false; + } + + //静止判断 + state.isStill = speedNowCompute < STILL_JUDGE_THRESHOLD; + + //启动信号判断 + if (isInState(RailProfileMeasureTaskStatus.WAITING_FOR_MEASURING)) { + //左负数，右正 + MeasureSide side = rad1 < 0 ? MeasureSide.LEFT : MeasureSide.RIGHT; + + if (state.isInStartPosArea && state.isStill) { + state.stayStillDurationCnt++; + } else { + state.stayStillDurationCnt = 0; + } + if (state.stayStillDurationCnt >= cfg.startSigJudgeThreshold) { + MeasureSide expectSide = null; + if (state.measureSide == null) { + expectSide = startSide; + } else { + expectSide = MeasureSide.getOpposite(state.measureSide); + } + + if (!expectSide.equals(side)) { +// log.warn("用户启动记录方向错误"); + railProfileDrawerListener.onErrorStartRecordProfileSig(); + state.stayStillDurationCnt = 0; + } else { + /* + * 启动录制,并且发送启动录制信号 + */ + state.state = RailProfileMeasureTaskStatus.MEASURING; + state.measureSide = expectSide; + state.stayStillDurationCnt = 0; + state.profilePoints.clear(); + state.pulleyPointsRecordForTangentCalculation.clear(); + railProfileDrawerListener.onStartRecordProfileSig(expectSide); // 启动信号 + } + } + } + + if (isInState(RailProfileMeasureTaskStatus.MEASURING)) { + //停止判断 + if (!isTheSamePoint) { + if (speedNowCompute > 1200) { + // 测量停止 + if (state.measureSide.equals(MeasureSide.LEFT)) { + state.leftSideFinished = true; + } + if (state.measureSide.equals(MeasureSide.RIGHT)) { + state.rightSideFinished = true; + } + + if (state.rightSideFinished && state.leftSideFinished) { + state.state = RailProfileMeasureTaskStatus.FINISHED; + railProfileDrawerListener.onEndRecordProfileSig(state.measureSide, true); // 停止信号 + state.speedNow = 0; + state.measureSide = null; + state.errorSpeedDetected = false; + } else { + state.state = RailProfileMeasureTaskStatus.WAITING_FOR_MEASURING; + railProfileDrawerListener.onEndRecordProfileSig(state.measureSide, false); // 停止信号 + } + return; + } + } + + + //轮廓计算 + // 1. 当前的点，找到上一个距离他至少0.05距离以上的点作为参考点 + // + // + state.pulleyPointsRecordForTangentCalculation.add(newPulleyPoint); + int rmnum = 0; + for (int i = 0; i < state.pulleyPointsRecordForTangentCalculation.size(); i++) { + if (distance(newPulleyPoint, state.pulleyPointsRecordForTangentCalculation.get(i + 1)) >= 0.05) { + rmnum++; + } else { + break; + } + } + state.pulleyPointsRecordForTangentCalculation.pop(rmnum); + + XYPoint point1 = null; + XYPoint point2 = null; + if (distance(newPulleyPoint, state.pulleyPointsRecordForTangentCalculation.get(0)) >= 0.05) { + var candidatePoints = CoordinateCalculator.calculateTangentPointCoordinates(newPulleyPoint, + state.pulleyPointsRecordForTangentCalculation.get(0), cfg.pulleyRadius, 0); + point1 = candidatePoints.r; + point2 = candidatePoints.l; + } + // + // 新的轮廓点: + // 考虑掉切线方向上有两个备选点，下面根据不同情况选择一个 + // 1. 第一个轮廓点，假设轮廓点在滑轮质心点的正下方 + // 2. 后续轮廓点，取离上一个轮廓点近的点 + // + if (point1 != null && point2 != null) { + if (state.profilePoints.getLast(0) == null) { + var newPoint = point1.y > point2.y ? point1 : point2; // y轴朝下为正 + state.profilePoints.add(newPoint); + railProfileDrawerListener.onProfilePointPos(newPoint);// 回调 + } else if (state.profilePoints.getLast(0) != null) { + var newPoint = nearPoint(state.profilePoints.getLast(0), point1, point2); + state.profilePoints.add(newPoint); + if (state.profilePoints.getLast(30) != null) {//丢弃掉结束时的点 + railProfileDrawerListener.onProfilePointPos(state.profilePoints.getLast(30));// 回调 + } + } + } + } + } + + + + // + // Utils + // + + static private XYPoint twoAxisArmCoordTransformToXYPointV2(Double arm0Length, Double arm0Rad, Double arm1Length, + Double arm1Rad) { + double x = arm0Length * Math.sin(arm0Rad) + arm1Length * Math.sin(arm0Rad + arm1Rad); + double y = arm0Length * Math.cos(arm0Rad) + arm1Length * Math.cos(arm0Rad + arm1Rad); + return new XYPoint(x, y); + } + + static private boolean isTheSamePoint(XYPoint p0, XYPoint p1) { + if (p0 == null || p1 == null) { + return false; + } + return Math.abs(p0.x - p1.x) < 0.005 && Math.abs(p0.y - p1.y) < 0.005; + // return distance(p0, p1) < 0.001; + } + + static private double distance(XYPoint p0, XYPoint p1) { + return CoordinateCalculator.distance(p0, p1); + } + + + static private boolean isInTheStartPos(XYPoint pos) { + int hight = AppConstantConfig.START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_HIGHT; + int weight = AppConstantConfig.START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_WEIGHT; + + if (pos.x <= hight / 2.0 && pos.x >= -hight / 2.0 + && pos.y <= weight / 2.0 && pos.y >= -weight / 2.0) { + return true; + } + return false; + } + + static public XYPoint nearPoint(XYPoint refpoint, XYPoint p1, XYPoint p2) { + double d1 = distance(refpoint, p1); + double d2 = distance(refpoint, p2); + return d1 < d2 ? p1 : p2; + } + +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/TwoLinkArmInverseKinematics.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/TwoLinkArmInverseKinematics.java new file mode 100644 index 0000000..ec65ec0 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/TwoLinkArmInverseKinematics.java @@ -0,0 +1,72 @@ +package com.iflytop.profilometer.core.migration.measure.drawer; + +public class TwoLinkArmInverseKinematics { + + public static void main(String[] args) { + // 机械臂参数 + double L1 = 52; // 第一段臂长 + double L2 = 96; // 第二段臂长 + + // 计算逆运动学 + + int pointNum = 0; + for (double i = -60; i < 60; ) { + double[][] angles = inverseKinematics(i, 80, L1, L2); + if (angles == null) { + System.out.println("无解"); + continue; + } + System.out.println(String.format("%.4f %.4f %.4f %.4f %.4f %.4f", i, 0.0, angles[0][0], angles[0][1], angles[1][0], angles[1][1])); + pointNum++; + i = i + 0.2; + if (pointNum >= 500) { + break; + } + + } + + + } + + /** + * 二轴机械臂逆运动学求解 + * + * @param x 目标位置的 x 坐标 + * @param y 目标位置的 y 坐标 + * @param L1 第一段臂长 + * @param L2 第二段臂长 + * @return 返回两个关节的角度 [theta1, theta2]，单位为弧度。如果无解，返回 null。 + */ + public static double[][] inverseKinematics(double x, double y, double L1, double L2) { + // 计算目标点到原点的距离 + double distanceSquared = y * y + x * x; + double distance = Math.sqrt(distanceSquared); + + // 检查目标点是否在工作空间内 + if (distance > L1 + L2 || distance < Math.abs(L1 - L2)) { + return null; // 无解 + } + + // 计算 theta2（肘部向上和肘部向下两种解） + double cosTheta2 = (distanceSquared - L1 * L1 - L2 * L2) / (2 * L1 * L2); + double sinTheta2 = Math.sqrt(1 - cosTheta2 * cosTheta2); + + // 肘部向上构型 + double theta2_up = Math.atan2(sinTheta2, cosTheta2); + double k1_up = L1 + L2 * Math.cos(theta2_up); + double k2_up = L2 * Math.sin(theta2_up); + double theta1_up = Math.atan2(x, y) - Math.atan2(k2_up, k1_up); + + // 肘部向下构型 + double theta2_down = -Math.atan2(sinTheta2, cosTheta2); + double k1_down = L1 + L2 * Math.cos(theta2_down); + double k2_down = L2 * Math.sin(theta2_down); + double theta1_down = Math.atan2(x, y) - Math.atan2(k2_down, k1_down); + + // 返回两组角度 + return new double[][]{ + {Math.toDegrees(theta1_up), Math.toDegrees(theta2_up)}, // 肘部向上 + {Math.toDegrees(theta1_down), Math.toDegrees(theta2_down)} // 肘部向下 + }; + } +} \ No newline at end of file diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/MeasureSide.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/MeasureSide.java new file mode 100644 index 0000000..6b058a7 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/MeasureSide.java @@ -0,0 +1,18 @@ +package com.iflytop.profilometer.core.migration.measure.drawer.type; + +public enum MeasureSide { + LEFT, RIGHT; + + MeasureSide() { + } + + static public MeasureSide getOpposite(MeasureSide side) { + if (side == LEFT) { + return RIGHT; + } else { + return LEFT; + } + } + + +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileDrawerListener.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileDrawerListener.java new file mode 100644 index 0000000..9af4e6d --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileDrawerListener.java @@ -0,0 +1,34 @@ +package com.iflytop.profilometer.core.migration.measure.drawer.type; + +public interface RailProfileDrawerListener { + /** + * 新的质心位置 + */ + void onNewPulleyCentroidPos(XYPoint xyPoint); + + /** + * 新的轮廓点位置 + */ + void onProfilePointPos(XYPoint xyPoint); + + /** + * 开始记录轮廓点信号 + */ + void onStartRecordProfileSig(MeasureSide side); + + /** + * 用户启动记录方向错误 + */ + void onErrorStartRecordProfileSig(); + + /** + * 结束记录轮廓点信号 + */ + void onEndRecordProfileSig(MeasureSide side, Boolean finished); + + /** + * 移动超速，图像可能会发生失真 + */ + void onErrorSpeedDetected(); + +} \ No newline at end of file diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileDrawerState.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileDrawerState.java new file mode 100644 index 0000000..8c7de72 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileDrawerState.java @@ -0,0 +1,42 @@ +package com.iflytop.profilometer.core.migration.measure.drawer.type; + +import com.iflytop.profilometer.core.migration.measure.ZLockList; + +public class RailProfileDrawerState { + public RailProfileMeasureTaskStatus state = RailProfileMeasureTaskStatus.IDLE; //状态 + + public boolean rightSideFinished = false; // 右侧是否测量完成 + public boolean leftSideFinished = false; // 左侧是否测量完成 + + // StartRecordJudgeContext + public int stayStillDurationCnt = 0; // 停留时间计数 + + // RecordContext + public int speedNow = 0; // 0->100(+) ，用户移动滑轮的速度，速度为接近100时，图像可能会发生失真，需要警告用户,大于100时，需要停止测量 + public boolean errorSpeedDetected = false; // 用户移动滑轮的速度过快 + // public ZLockList pulleyPoints = new ZLockList<>(10); + public ZLockList pulleyPointsRecordForTangentCalculation = new ZLockList<>(10); + public ZLockList profilePoints = new ZLockList<>(); + public XYPoint lastPulleyPoint = new XYPoint(); + + public boolean isInStartPosArea = false; + public boolean isStill = false; + + public MeasureSide measureSide = null; + + + public void resetRecordContext() { + speedNow = 0; + errorSpeedDetected = false; + stayStillDurationCnt = 0; + rightSideFinished = false; + leftSideFinished = false; + isInStartPosArea = false; + isStill = false; + measureSide = null; + // pulleyPoints.clear(); + lastPulleyPoint = null; + profilePoints.clear(); + pulleyPointsRecordForTangentCalculation.clear(); + } +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileMeasureTaskStatus.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileMeasureTaskStatus.java new file mode 100644 index 0000000..af2582f --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/RailProfileMeasureTaskStatus.java @@ -0,0 +1,32 @@ +package com.iflytop.profilometer.core.migration.measure.drawer.type; + +public enum RailProfileMeasureTaskStatus { + /** + * 空闲 + */ + IDLE, + /** + * 正在测量数据 + */ + MEASURING, + /** + * 等待测量 + */ + WAITING_FOR_MEASURING, + /** + * 测量完成 + */ + FINISHED, + + ; + + public String toCHString() { + return switch (this) { + case IDLE -> "空闲"; + case MEASURING -> "正在测量数据"; + case WAITING_FOR_MEASURING -> "等待测量"; + case FINISHED -> "测量完成"; + default -> "未知状态"; + }; + } +} \ No newline at end of file diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/TrackPoint.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/TrackPoint.java new file mode 100644 index 0000000..6f397ba --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/TrackPoint.java @@ -0,0 +1,44 @@ +package com.iflytop.profilometer.core.migration.measure.drawer.type; + +public class TrackPoint { + public double x; + public double y; + public long date; + + public TrackPoint() { + this.x = 0.0; + this.y = 0.0; + this.date = 0; + } + + public TrackPoint(double x, double y, long date) { + this.x = x; + this.y = y; + this.date = date; + } + + public TrackPoint(XYPoint xyPoint, long date) { + this.x = xyPoint.x; + this.y = xyPoint.y; + this.date = date; + } + + public boolean equals(TrackPoint other, double epsilon) { + if (other == null) { + return false; + } + return Math.abs(this.x - other.x) < epsilon && Math.abs(this.y - other.y) < epsilon; + } + + + + public XYPoint toXYPoint() { + return new XYPoint(x, y); + } + + public String toString() { + return String.format("[%.2f,%.2f]", x, y); + } + + +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/TwoObject.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/TwoObject.java new file mode 100644 index 0000000..e289275 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/TwoObject.java @@ -0,0 +1,16 @@ +package com.iflytop.profilometer.core.migration.measure.drawer.type; + +public class TwoObject { + public R r; + public L l; + + public TwoObject(R r, L l) { + this.r = r; + this.l = l; + } + + + + + +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/XYPoint.java b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/XYPoint.java new file mode 100644 index 0000000..3ef412d --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/measure/drawer/type/XYPoint.java @@ -0,0 +1,47 @@ +package com.iflytop.profilometer.core.migration.measure.drawer.type; + + +public class XYPoint { + public double x; + public double y; + + public XYPoint() { + this.x = 0.0; + this.y = 0.0; + } + + public XYPoint(double x, double y) { + this.x = x; + this.y = y; + } + + public String toString() { + return String.format("(%+.5f, %+.5f)", x, y); + } + + public Boolean equals(XYPoint other, double epsilon) { + if (other == null) { + return false; + } + return Math.abs(this.x - other.x) < epsilon && Math.abs(this.y - other.y) < epsilon; + } + + synchronized public double getX() { + return x; + } + + synchronized public double getY() { + return y; + } + + @Override + public boolean equals(Object other) { + if (other == null) { + return false; + } + if (other instanceof XYPoint) { + return equals((XYPoint) other, 0.0001); + } + return false; + } +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/type/Version.java b/app/src/main/java/com/iflytop/profilometer/core/migration/type/Version.java new file mode 100644 index 0000000..1c6ac96 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/type/Version.java @@ -0,0 +1,29 @@ +package com.iflytop.profilometer.core.migration.type; + +public class Version { + public Integer main; + public Integer sub; + public Integer fix; + + public String toString() { + return String.format("%d.%d.%d", main, sub, fix); + } + + public Version() { + main = 0; + sub = 0; + fix = 0; + } + + public Version(Integer main, Integer sub, Integer fix) { + this.main = main; + this.sub = sub; + this.fix = fix; + } + + public Version(Integer in32val) { + this.main = (in32val >> 16) & 0xFF; + this.sub = (in32val >> 8) & 0xFF; + this.fix = in32val & 0xFF; + } +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/type/enums/SubDeviceChannel.java b/app/src/main/java/com/iflytop/profilometer/core/migration/type/enums/SubDeviceChannel.java new file mode 100644 index 0000000..5afea2c --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/type/enums/SubDeviceChannel.java @@ -0,0 +1,6 @@ +package com.iflytop.profilometer.core.migration.type.enums; + +public enum SubDeviceChannel { + BLE_CHANNEL, + UART_CHANNEL, +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIBasicReport.java b/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIBasicReport.java index d5eeea6..6b763e3 100644 --- a/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIBasicReport.java +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIBasicReport.java @@ -1,6 +1,8 @@ package com.iflytop.profilometer.core.migration.type.protocol; +import androidx.annotation.NonNull; + import cn.hutool.core.lang.Assert; //report: <-(power,state,flag,inclinatorX(0.001°), inclinatorY(0.001°), temperature(0.01℃)) 1s @@ -34,7 +36,8 @@ public class TPMIBasicReport extends TPMIPacket { return this.getDataAsInt(5) / 1000.0; } - public String toString() { + @NonNull + public String toString() { return String.format("Power:%d State:%d Flag:0x%08x InclinatorX:%.3f InclinatorY:%.3f Temperature:%.2f", getPower(), getState(), getFlag(), getInclinatorX(), getInclinatorY(), getTemperature()); } diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIPacket.java b/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIPacket.java index d38a824..053b783 100644 --- a/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIPacket.java +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIPacket.java @@ -1,6 +1,10 @@ package com.iflytop.profilometer.core.migration.type.protocol; +import android.annotation.SuppressLint; + +import androidx.annotation.NonNull; + import com.iflytop.profilometer.core.migration.factory.TPMIPacketFactory; import com.iflytop.profilometer.core.migration.utils.ZByteUtils; @@ -244,7 +248,9 @@ public class TPMIPacket { } - public String toString() { + @NonNull + @SuppressLint("DefaultLocale") + public String toString() { return String.format("[INDEX:%03d CMD: %-35s,%s]", getIndex(), getCommand(), ZByteUtils.bytesToHexString(rawpacket, rawpacket.length)); // return String.format("H:%02X L:%02X I:%02X T:%02X C:%02X DT:%02X %s C:%02X END:%02X", // rawpacket[P_HEADER_OFF], rawpacket[P_LEN_OFF], rawpacket[P_INDEX_OFF], rawpacket[P_TYPE_OFF], rawpacket[P_CMD_OFF], rawpacket[P_DATA_TYPE_OFF], diff --git a/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIPosReport.java b/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIPosReport.java index 0193c60..968d69e 100644 --- a/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIPosReport.java +++ b/app/src/main/java/com/iflytop/profilometer/core/migration/type/protocol/TPMIPosReport.java @@ -1,6 +1,8 @@ package com.iflytop.profilometer.core.migration.type.protocol; +import androidx.annotation.NonNull; + import cn.hutool.core.lang.Assert; public class TPMIPosReport extends TPMIPacket { @@ -21,7 +23,8 @@ public class TPMIPosReport extends TPMIPacket { } - public String toString() { + @NonNull + public String toString() { return String.format("ARM0Angle:%d ARM1Angle:%d ", getArm1Angle(), getArm2Angle()); } } diff --git a/app/src/main/java/com/iflytop/profilometer/core/system/SystemService.java b/app/src/main/java/com/iflytop/profilometer/core/system/SystemService.java new file mode 100644 index 0000000..6c61928 --- /dev/null +++ b/app/src/main/java/com/iflytop/profilometer/core/system/SystemService.java @@ -0,0 +1,51 @@ +package com.iflytop.profilometer.core.system; + +import com.iflytop.profilometer.core.bluetooth.BleDeviceDriver; +import com.iflytop.profilometer.core.migration.channel.BleDeviceUartChannel; +import com.iflytop.profilometer.model.entity.AppUser; + +/** + * 全局系统服务 + */ +public class SystemService { + private final DeviceState deviceState = new DeviceState(); + + private final BleDeviceUartChannel channel = new BleDeviceUartChannel(); + + private final BleDeviceDriver bleDeviceDriver = new BleDeviceDriver(channel); + + private SystemService() { + } + + private static final SystemService instance = new SystemService(); + + // 提供获取单例实例的全局访问点 + public static SystemService getInstance() { + return instance; + } + + /** + * 系统当前登录用户 + */ + private AppUser currentUser; + + public synchronized AppUser getCurrentUser() { + return currentUser; + } + + public synchronized void setCurrentUser(AppUser currentUser) { + this.currentUser = currentUser; + } + + public DeviceState getDeviceState() { + return deviceState; + } + + public BleDeviceUartChannel getChannel() { + return channel; + } + + public BleDeviceDriver getBleDeviceDriver() { + return bleDeviceDriver; + } +} diff --git a/app/src/main/java/com/iflytop/profilometer/core/system/SystemState.java b/app/src/main/java/com/iflytop/profilometer/core/system/SystemState.java deleted file mode 100644 index 49c57b1..0000000 --- a/app/src/main/java/com/iflytop/profilometer/core/system/SystemState.java +++ /dev/null @@ -1,36 +0,0 @@ -package com.iflytop.profilometer.core.system; - -import com.iflytop.profilometer.model.entity.AppUser; - -/** - * 全局系统状态 - */ -public class SystemState { - private final DeviceState deviceState = new DeviceState(); - private SystemState() { - } - - private static final SystemState instance = new SystemState(); - - // 提供获取单例实例的全局访问点 - public static SystemState getInstance() { - return instance; - } - - /** - * 系统当前登录用户 - */ - private AppUser currentUser; - - public synchronized AppUser getCurrentUser() { - return currentUser; - } - - public synchronized void setCurrentUser(AppUser currentUser) { - this.currentUser = currentUser; - } - - public DeviceState getDeviceState() { - return deviceState; - } -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/AppConstantConfig.java b/app/src/main/java/com/iflytop/profilometer/measure/AppConstantConfig.java deleted file mode 100644 index d6e83d6..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/AppConstantConfig.java +++ /dev/null @@ -1,28 +0,0 @@ -package com.iflytop.profilometer.measure; - -public class AppConstantConfig { - public static final int HEARTBEAT_INTERVAL = 10000; - - //100ms上报一次点位 - public static final int POINT_REPORT_PERIOD = 5; - - /* - * 轮廓绘制，启动信号判断相关参数 - * 逻辑: - * 滑轮在指定区域静止超过一定时间 - * - * 参数: - * 区域判断阈值（圆心附近的长方形） - * START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_HIGHT 高度阈值 - * START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_WEIGHT 宽度阈值 - * 时间判断阈值（毫秒） - * OUTLINE_START_RECORD_SIG_JUDGE_TIME_THRESHOLD 毫秒 - * 静止判断位置阈值(毫米) - * STILL_JUDGE_THRESHOLD - */ - - public static final int OUTLINE_START_RECORD_SIG_JUDGE_TIME_THRESHOLD = 1000; - public static final int START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_HIGHT = 30; - public static final int START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_WEIGHT = 50; - public static final double STILL_JUDGE_THRESHOLD = 40; -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/EncoderSensorRawData.java b/app/src/main/java/com/iflytop/profilometer/measure/EncoderSensorRawData.java deleted file mode 100644 index 3341bc0..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/EncoderSensorRawData.java +++ /dev/null @@ -1,17 +0,0 @@ -package com.iflytop.profilometer.measure; - -public class EncoderSensorRawData { - public double arm1Angle = 0; //0->360 - public double arm2Angle = 0; //0->360 - - public EncoderSensorRawData(double arm1Angle, double arm2Angle) { - this.arm1Angle = arm1Angle; - this.arm2Angle = arm2Angle; - } - - public EncoderSensorRawData() {} - - public String toString() { - return String.format("(%+.5f, %+.5f)", arm1Angle, arm2Angle); - } -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/ZLockList.java b/app/src/main/java/com/iflytop/profilometer/measure/ZLockList.java deleted file mode 100644 index d848a7a..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/ZLockList.java +++ /dev/null @@ -1,70 +0,0 @@ -package com.iflytop.profilometer.measure; - -import java.util.ArrayList; -import java.util.List; - -public class ZLockList { - private List list; - private Integer maxCount = 0; - - public ZLockList() { - this.list = new ArrayList<>(); - } - - public ZLockList(Integer maxCount) { - this.list = new ArrayList<>(); - this.maxCount = maxCount; - } - - synchronized public void add(T t) { - list.add(t); - if (maxCount > 0 && list.size() > maxCount) { - list.removeFirst(); - } - } - - synchronized public void clear() { - list.clear(); - } - - synchronized public void remove(T t) { - list.remove(t); - } - - synchronized public T getLast(Integer index) { - if (list.size() > index) { - return list.get(list.size() - 1 - index); - } - return null; - } - - synchronized public List getList() { - return new ArrayList<>(list); - } - - synchronized public void removeTheFirst() { - list.removeFirst(); - } - - synchronized public T get(Integer index) { - if (list.size() > index) { - return list.get(index); - } - return null; - } - - synchronized public void pop(int num) { - for (int i = 0; i < num; i++) { - list.removeFirst(); - } - } - - - synchronized public boolean isEmpty() { - return list.isEmpty(); - } - - synchronized public int size() { - return list.size(); - } -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/AngleConverter.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/AngleConverter.java deleted file mode 100644 index da1c3a4..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/AngleConverter.java +++ /dev/null @@ -1,28 +0,0 @@ -package com.iflytop.profilometer.measure.drawer; - -public class AngleConverter { - - public static Integer angleToInt32(double angle) { - angle = limitAngle(angle); - return (int) (angle / 180.0 * Integer.MAX_VALUE); - } - - public static double int32ToAngle(Integer angle) { - double val = angle / (double) Integer.MAX_VALUE * 180; - return limitAngle(val); - } - - public static double limitAngle(double angle) { - while (!(angle >= -180) || !(angle < 180)) { - if (angle > 180) { - angle -= 360; - } - if (angle <= -180) { - angle += 360; - } - } - return angle; - } - - -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/CoordinateCalculator.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/CoordinateCalculator.java deleted file mode 100644 index e36f8a8..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/CoordinateCalculator.java +++ /dev/null @@ -1,240 +0,0 @@ -package com.iflytop.profilometer.measure.drawer; - - -import static java.lang.Math.PI; -import static java.lang.Math.abs; - -import com.iflytop.profilometer.measure.drawer.type.TrackPoint; -import com.iflytop.profilometer.measure.drawer.type.TwoObject; -import com.iflytop.profilometer.measure.drawer.type.XYPoint; - -public class CoordinateCalculator { - - - /** - * 两轴机械臂坐标转换为XY坐标 - * @param arm0Length 机械臂0长度 - * @param arm0Rad 机械臂0弧度 - * @param arm1Length 机械臂1长度 - * @param arm1Rad 机械臂1弧度 - * @return XY坐标 - */ - static public XYPoint twoAxisArmCoordTransformToXYPointV1(Double arm0Length, Double arm0Rad, Double arm1Length, Double arm1Rad) { - XYPoint xyPoint = new XYPoint(); - double lval = calculateThirdSide(arm0Length, arm1Length, Math.PI - arm1Rad); - - if (lval < 0.000001) { - xyPoint.x = 0.0; xyPoint.y = 0.0; - } else { - double rad0 = calculateAngle(arm0Length, lval, arm1Length); - double lrad = 0.0; - if (arm1Rad > 0) { - lrad = arm0Rad + rad0; - } else { - lrad = arm0Rad - rad0; - } - xyPoint.x = lval * Math.sin(lrad); - xyPoint.y = lval * Math.cos(lrad); - } return xyPoint; - } - - static public XYPoint twoAxisArmCoordTransformToXYPointV2(Double arm0Length, Double arm0Rad, Double arm1Length, Double arm1Rad) { - XYPoint xyPoint = new XYPoint(); - xyPoint.x = arm0Length * Math.sin(arm0Rad) + arm1Length * Math.sin(arm0Rad + arm1Rad); - xyPoint.y = arm0Length * Math.cos(arm0Rad) + arm1Length * Math.cos(arm0Rad + arm1Rad); - return xyPoint; - } - - static public XYPoint twoAxisArmCoordTransformToXYPoint(Double arm0Length, Double arm0Rad, Double arm1Length, Double arm1Rad) { - return twoAxisArmCoordTransformToXYPointV2(arm0Length, arm0Rad, arm1Length, arm1Rad); - } - - - /** - * 计算切点坐标 - * @param point0 零点 - * @param refpoint 曲线参考点 - * @param radius 半径 - * @return 经过point0且垂直于point0-refpoint的曲线上距离零点为radius的点的坐标 - * - * - * target1 - * | - * | - * | radius - * | - * | - * ----------ref----point0------------- - * | - * | - * | radius - * | - * | - * target2 - * - */ - static public TwoObject calculateTangentPointCoordinates(XYPoint point0, XYPoint refpoint, double radius, double mindistance) { - if (point0 == null || refpoint == null) { - return new TwoObject<>(null, null); - } - - if (distance(point0, refpoint) < mindistance) { - return new TwoObject<>(null, null); - } - - double dx = refpoint.x - point0.x; - double dy = refpoint.y - point0.y; - - - double x = Math.sqrt(radius * radius * dx * dx / (dx * dx + dy * dy)); - double y = Math.sqrt(radius * radius * dy * dy / (dx * dx + dy * dy)); - - y = dy > 0 ? y : -y; - x = dx > 0 ? x : -x; - - //顺时针旋转90度 - double new1x = y; - double new1y = -x; - - //逆时针旋转90度 - double new2x = -y; - double new2y = x; - - XYPoint t1 = new XYPoint(point0.x + new1x, point0.y + new1y); - XYPoint t2 = new XYPoint(point0.x + new2x, point0.y + new2y); - - return new TwoObject<>(t1, t2); - } - - static public TwoObject calculateTangentPointCoordinates(TrackPoint point0, TrackPoint refpoint, double radius) { - XYPoint xypoint0 = point0.toXYPoint(); - XYPoint xyRefpoint = refpoint.toXYPoint(); - var ret = calculateTangentPointCoordinates(xypoint0, xyRefpoint, radius, 0); - return new TwoObject<>(new TrackPoint(ret.r, point0.date), new TrackPoint(ret.l, point0.date)); - } - - - // - // PRIVATE - // - static double calculateThirdSide(double a, double b, double angleInRadians) { - // 计算第三条边的平方 - double cSquared = a * a + b * b - 2 * a * b * Math.cos(angleInRadians); - - // 返回第三条边的长度 - return Math.sqrt(cSquared); - } - - static double calculateAngle(double side1, double side2, double oppositeSide) { - //防止由于double精度问题导致的结果出现NaN - if (oppositeSide > side1 + side2) { - return PI; - } - - //防止由于double精度问题导致的结果出现NaN - if (oppositeSide < Math.abs(side1 - side2)) { - return 0; - } - - // 使用余弦定理计算夹角的余弦值 - double cosTheta = (side1 * side1 + side2 * side2 - oppositeSide * oppositeSide) / (2 * side1 * side2); - - // 使用反余弦函数求出夹角的弧度 - var ret = abs(Math.acos(cosTheta)); if (Double.isNaN(ret)) { - throw new RuntimeException(String.format("calculateAngle ret NaN: side1=%+.4f,side2=%+.4f,oppositeSide=%+.4f", side1, side2, oppositeSide)); - } return ret; - } - - static public double distance(XYPoint p0, XYPoint p1) { - if (p0 == null || p1 == null) { - return 0; - } - return Math.sqrt(Math.pow(p0.x - p1.x, 2) + Math.pow(p0.y - p1.y, 2)); - } - - // - // TEST - // - static void Assert(boolean condition, String message) { - if (!condition) { - throw new RuntimeException(message); - } - } - - static void testCalculateThirdSide(double a, double b, double angleInRadians, double expected) { - double c = calculateThirdSide(a, b, angleInRadians); - Assert(abs(c - expected) < 0.0001, String.format("testCalculateThirdSide: a=%+.4f,b=%+.4f,angleInRadians=%+.4f --> c=%+.4f", a, b, angleInRadians, c)); - } - - static void testcalculateAngle(double side1, double side2, double oppositeSide, double expected) { - double angle = calculateAngle(side1, side2, oppositeSide); - Assert(abs(angle - expected) < 0.0001, - String.format("testcalculateAngle: side1=%+.4f,side2=%+.4f,oppositeSide=%+.4f --> angle=%+.4f(%+.4f)", side1, side2, oppositeSide, angle, Math.toDegrees(angle))); - } - - static void testTwoAxisArmCoordTransformToXYPoint(double arm0Length, double arm0Rad, double arm1Length, double arm1Rad, double expectedX, double expectedY) { - XYPoint xyPoint = twoAxisArmCoordTransformToXYPoint(arm0Length, arm0Rad, arm1Length, arm1Rad); - Assert(abs(xyPoint.x - expectedX) < 0.0001 && abs(xyPoint.y - expectedY) < 0.0001, - String.format("testCalculator: arm0Length=%+.4f,arm0Rad=%+.4f,arm1Length=%+.4f,arm1Rad=%+.4f --> x=%+.4f,y=%+.4f", arm0Length, arm0Rad, arm1Length, arm1Rad, xyPoint.x, xyPoint.y)); - } - - static public void testCalculateTangentPointCoordinates(XYPoint point0, XYPoint refpoint, double radius, XYPoint expected0, XYPoint expected1) { - var ret = calculateTangentPointCoordinates(point0, refpoint, radius,0); - Assert(!(expected0 == null || expected1 == null || !expected0.equals(ret.r) || !expected1.equals(ret.l)), - String.format("check testCalculateTangentPointCoordinates(%s,%s,%s) fail\n%s", point0, refpoint, radius, - String.format("%.4f\t%.4f\n%.4f\t%.4f\n%.4f\t%.4f\n%.4f\t%.4f", point0.x, point0.y, refpoint.x, refpoint.y, ret.r.x, ret.r.y, ret.l.x, ret.l.y))); - } - - public static void main(String[] args) { - - // log.info("{}", - // twoAxisArmCoordTransformToXYPoint(53.0, 18.6326 / 180 * Math.PI, 96.0, -91.8046 / 180 * Math.PI)); - // - // CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(AppConstantConfig.ARM1_LENGTH_MM, 0.0, AppConstantConfig.ARM2_LENGTH_MM, 0.0, 0.0, 2.0); - - - CoordinateCalculator.testCalculateThirdSide(1, 1, 0, 0.0); - CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI / 4, 0.7654); - CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI / 2, 1.4142); - CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI * 3 / 4, 1.8478); - CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI, 2.0); - CoordinateCalculator.testCalculateThirdSide(1, 1, Math.PI * 1 / 4 + Math.PI, 1.8478); - - CoordinateCalculator.testcalculateAngle(1, 1, 1, Math.PI / 3); - CoordinateCalculator.testcalculateAngle(1, 2, 3, Math.PI); - CoordinateCalculator.testcalculateAngle(1, 2, 3.0000001, Math.PI); - CoordinateCalculator.testcalculateAngle(1, 2, 0.99999999, 0); - CoordinateCalculator.testcalculateAngle(1, 1, Math.sqrt(2), Math.PI / 2); - - CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, 0.0, 1.0, 0.0, 0.0, 2.0); - CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, 0.0, 1.0, Math.PI, 0.0, 0.0); - CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, PI / 4, 1.0, 0, 1.4142, 1.4142); - CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, -PI / 4, 1.0, 0, -1.4142, 1.4142); - // CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, PI / 4, 1.0, PI * 5 / 4, 0.7071, -0.2929); - CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, PI / 4, 1.0, -PI * 1 / 2, 0.0, 1.4142); - CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, PI / 4, 1.0, PI * 1 / 2, 1.4142, 0.0); - CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, -PI / 4, 1.0, -PI * 1 / 2, -1.4142, 0.0); - CoordinateCalculator.testTwoAxisArmCoordTransformToXYPoint(1.0, -PI / 4, 1.0, PI * 1 / 2, 0.0, 1.4142); - - CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(1, 0), 1, - new XYPoint(0, -1), new XYPoint(0, 1)); - CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(2, 2), Math.sqrt(2), - new XYPoint(1, -1), new XYPoint(-1, 1)); - CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(1, 1), new XYPoint(2, 2), Math.sqrt(2), - new XYPoint(2, 0), new XYPoint(0, 2)); - CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(-2, -2), Math.sqrt(2), - new XYPoint(-1, 1), new XYPoint(1, -1)); - CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(1, 1), new XYPoint(-2, -2), Math.sqrt(2), - new XYPoint(0, 2), new XYPoint(2, 0)); - CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(-1, 1), new XYPoint(-2, 2), Math.sqrt(2), - new XYPoint(0, 2), new XYPoint(-2, 0)); - CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(1, -1), new XYPoint(-2, 2), Math.sqrt(2), - new XYPoint(2, 0), new XYPoint(0, -2)); - CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(1, 2), Math.sqrt(5), - new XYPoint(2, -1), new XYPoint(-2, 1)); - CoordinateCalculator.testCalculateTangentPointCoordinates(new XYPoint(0, 0), new XYPoint(4, 6), Math.sqrt(52), - new XYPoint(6, -4), new XYPoint(-6, 4)); - - } - -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/PulleyPointDrawer.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/PulleyPointDrawer.java deleted file mode 100644 index f5ba8da..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/PulleyPointDrawer.java +++ /dev/null @@ -1,80 +0,0 @@ -package com.iflytop.profilometer.measure.drawer; - - -import com.iflytop.profilometer.measure.EncoderSensorRawData; -import com.iflytop.profilometer.measure.drawer.type.XYPoint; - -public class PulleyPointDrawer { - static class Config { - public double arm1Length = 0; - public double arm2Length = 0; - public double pulleyRadius = 0; - public double profilometerHight = 0; - } - - Config cfg = new Config(); // 配置 - XYPoint lastPulleyPoint = null; - - public PulleyPointDrawer() { - - } - - synchronized public void startDraw(double arm1Length, double arm2Length, double pulleyRadius, - double profilometerHight) { -// log.info("startDraw arm1Length:{} arm2Length:{} pulleyRadius:{} profilometerHight:{}", arm1Length, arm2Length, -// pulleyRadius, profilometerHight); - this.cfg.arm1Length = arm1Length; - this.cfg.arm2Length = arm2Length; - this.cfg.pulleyRadius = pulleyRadius; - this.cfg.profilometerHight = profilometerHight; - } - - synchronized public void updateConfig(double arm1Length, double arm2Length, double pulleyRadius, - double profilometerHight) { -// log.info("updateConfig arm1Length:{} arm2Length:{} pulleyRadius:{} profilometerHight:{}", arm1Length, arm2Length, -// pulleyRadius, profilometerHight); - this.cfg.arm1Length = arm1Length; - this.cfg.arm2Length = arm2Length; - this.cfg.pulleyRadius = pulleyRadius; - this.cfg.profilometerHight = profilometerHight; - } - - synchronized public void stopDraw() { - ; - } - - - public EncoderSensorRawData convertToEncoderSensorRawData(Integer encoder1, Integer encoder2) { - EncoderSensorRawData rawData = new EncoderSensorRawData(); - rawData.arm1Angle = encoder1 * 1.0 / 2147483647 * 180; - rawData.arm2Angle = encoder2 * 1.0 / 2147483647 * 180; - return rawData; - } - - /** - * 将编码器数值转换为滑轮坐标，并自动过滤掉相同位置的点 - * @param angle1 编码器0的点 - * @param angle2 编码器1的点 - * @return 滑轮坐标 - */ - public XYPoint convertToPulleyPoint(double angle1, double angle2) { - //编码器数值转弧度 - double arm1rad = angle1 / 180.0 * Math.PI; - double arm2rad = angle2 / 180.0 * Math.PI; - XYPoint newPulleyPoint = CoordinateCalculator.twoAxisArmCoordTransformToXYPointV2(cfg.arm1Length, arm1rad, cfg.arm2Length, arm2rad); - newPulleyPoint.y = newPulleyPoint.y - cfg.profilometerHight + cfg.pulleyRadius; - // if (isTheSamePoint(lastPulleyPoint, newPulleyPoint)) { - // return null; - // } - lastPulleyPoint = newPulleyPoint; - return newPulleyPoint; - } - - static private boolean isTheSamePoint(XYPoint p0, XYPoint p1) { - if (p0 == null || p1 == null) { - return false; - } - return CoordinateCalculator.distance(p0, p1) < 0.003; - } - -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/RailProfileDrawer.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/RailProfileDrawer.java deleted file mode 100644 index f41ae51..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/RailProfileDrawer.java +++ /dev/null @@ -1,272 +0,0 @@ -package com.iflytop.profilometer.measure.drawer; - -import static com.iflytop.profilometer.measure.AppConstantConfig.STILL_JUDGE_THRESHOLD; - -import com.iflytop.profilometer.measure.AppConstantConfig; -import com.iflytop.profilometer.measure.drawer.type.MeasureSide; -import com.iflytop.profilometer.measure.drawer.type.RailProfileDrawerListener; -import com.iflytop.profilometer.measure.drawer.type.RailProfileDrawerState; -import com.iflytop.profilometer.measure.drawer.type.RailProfileMeasureTaskStatus; -import com.iflytop.profilometer.measure.drawer.type.XYPoint; - -public class RailProfileDrawer { - public static final double PRECISION = 0.1;// mm 满足13cm，1500个点 - - public static class Config { - public double arm1Length = 0; - public double arm2Length = 0; - public double pulleyRadius = 0; - public double profilometerHight = 0; - public int startSigJudgeThreshold = 100; - } - - Config cfg = new Config(); // 配置 - RailProfileDrawerState state = new RailProfileDrawerState(); - RailProfileDrawerListener railProfileDrawerListener; - MeasureSide startSide = MeasureSide.LEFT; - - public RailProfileDrawer(RailProfileDrawerListener railProfileDrawerListener) { - this.railProfileDrawerListener = railProfileDrawerListener; - } - - - synchronized public void startDraw(double arm1Length, double arm2Length, double pulleyRadius, - double profilometerHight) { -// log.info("startDraw arm1Length:{} arm2Length:{} pulleyRadius:{} profilometerHight:{}", arm1Length, arm2Length, -// pulleyRadius, profilometerHight); - this.cfg.arm1Length = arm1Length; - this.cfg.arm2Length = arm2Length; - this.cfg.pulleyRadius = pulleyRadius; - this.cfg.profilometerHight = profilometerHight; - this.cfg.startSigJudgeThreshold = AppConstantConfig.OUTLINE_START_RECORD_SIG_JUDGE_TIME_THRESHOLD / AppConstantConfig.POINT_REPORT_PERIOD; - state.resetRecordContext(); - state.state = RailProfileMeasureTaskStatus.WAITING_FOR_MEASURING; - } - - synchronized public void stopDraw() { - state.state = RailProfileMeasureTaskStatus.IDLE; - } - - synchronized public RailProfileDrawerState getState() { - return state; - } - - synchronized public Config getCfg() { - return cfg; - } - - boolean isInState(RailProfileMeasureTaskStatus... state) { - for (RailProfileMeasureTaskStatus s : state) { - if (this.state.state.equals(s)) { - return true; - } - } - return false; - } - - - // public XYPoint convertToPulleyPoint(Integer encoder0, Integer encoder1) { - // //编码器数值转弧度 - // double arm1rad = encoder0 * 1.0 / AppConstantConfig.ENCODER_ACCURACY * Math.PI; - // double arm2rad = encoder1 * 1.0 / AppConstantConfig.ENCODER_ACCURACY * Math.PI; - // XYPoint newPulleyPoint = twoAxisArmCoordTransformToXYPointV2(cfg.arm1Length, arm1rad, cfg.arm2Length, arm2rad); - // } - - synchronized public void processRawEncoderData(Integer data1, Integer data2) { - data1 = data1 & 0xFFFFF000; - data2 = data2 & 0xFFFFF000; - double arm1rad = data1 * 1.0 / 2147483647 * Math.PI; - double arm2rad = data2 * 1.0 / 2147483647 * Math.PI; - processEncoderData(arm1rad, arm2rad); - } - - synchronized private void processEncoderData(double rad1, double rad2) { - if (isInState(RailProfileMeasureTaskStatus.IDLE, RailProfileMeasureTaskStatus.FINISHED)) { - return; - } - - //角度转换成坐标 - XYPoint newPulleyPoint = twoAxisArmCoordTransformToXYPointV2(cfg.arm1Length, rad1, cfg.arm2Length, rad2); - railProfileDrawerListener.onNewPulleyCentroidPos(newPulleyPoint);// 新的质心点 - newPulleyPoint.y = newPulleyPoint.y - cfg.profilometerHight; - boolean isTheSamePoint = isTheSamePoint(state.lastPulleyPoint, newPulleyPoint); - XYPoint lastPulleyPoint = state.lastPulleyPoint; - if (!isTheSamePoint) { - state.lastPulleyPoint = newPulleyPoint; - } - - //位置判断 - state.isInStartPosArea = isInTheStartPos(newPulleyPoint); - - //计算速度 - double distance = distance(lastPulleyPoint, newPulleyPoint); - int speedNowCompute = (int) (distance / PRECISION * 100); - if (isInState(RailProfileMeasureTaskStatus.MEASURING)) { - this.state.speedNow = speedNowCompute; - if (speedNowCompute > 100) { - if (!state.errorSpeedDetected) - railProfileDrawerListener.onErrorSpeedDetected();// 检测到速度异常 - state.errorSpeedDetected = true; - } else { - state.errorSpeedDetected = false; - } - } else { - this.state.speedNow = 0; - state.errorSpeedDetected = false; - } - - //静止判断 - state.isStill = speedNowCompute < STILL_JUDGE_THRESHOLD; - - //启动信号判断 - if (isInState(RailProfileMeasureTaskStatus.WAITING_FOR_MEASURING)) { - //左负数，右正 - MeasureSide side = rad1 < 0 ? MeasureSide.LEFT : MeasureSide.RIGHT; - - if (state.isInStartPosArea && state.isStill) { - state.stayStillDurationCnt++; - } else { - state.stayStillDurationCnt = 0; - } - if (state.stayStillDurationCnt >= cfg.startSigJudgeThreshold) { - MeasureSide expectSide = null; - if (state.measureSide == null) { - expectSide = startSide; - } else { - expectSide = MeasureSide.getOpposite(state.measureSide); - } - - if (!expectSide.equals(side)) { -// log.warn("用户启动记录方向错误"); - railProfileDrawerListener.onErrorStartRecordProfileSig(); - state.stayStillDurationCnt = 0; - } else { - /* - * 启动录制,并且发送启动录制信号 - */ - state.state = RailProfileMeasureTaskStatus.MEASURING; - state.measureSide = expectSide; - state.stayStillDurationCnt = 0; - state.profilePoints.clear(); - state.pulleyPointsRecordForTangentCalculation.clear(); - railProfileDrawerListener.onStartRecordProfileSig(expectSide); // 启动信号 - } - } - } - - if (isInState(RailProfileMeasureTaskStatus.MEASURING)) { - //停止判断 - if (!isTheSamePoint) { - if (speedNowCompute > 1200) { - // 测量停止 - if (state.measureSide.equals(MeasureSide.LEFT)) { - state.leftSideFinished = true; - } - if (state.measureSide.equals(MeasureSide.RIGHT)) { - state.rightSideFinished = true; - } - - if (state.rightSideFinished && state.leftSideFinished) { - state.state = RailProfileMeasureTaskStatus.FINISHED; - railProfileDrawerListener.onEndRecordProfileSig(state.measureSide, true); // 停止信号 - state.speedNow = 0; - state.measureSide = null; - state.errorSpeedDetected = false; - } else { - state.state = RailProfileMeasureTaskStatus.WAITING_FOR_MEASURING; - railProfileDrawerListener.onEndRecordProfileSig(state.measureSide, false); // 停止信号 - } - return; - } - } - - - //轮廓计算 - // 1. 当前的点，找到上一个距离他至少0.05距离以上的点作为参考点 - // - // - state.pulleyPointsRecordForTangentCalculation.add(newPulleyPoint); - int rmnum = 0; - for (int i = 0; i < state.pulleyPointsRecordForTangentCalculation.size(); i++) { - if (distance(newPulleyPoint, state.pulleyPointsRecordForTangentCalculation.get(i + 1)) >= 0.05) { - rmnum++; - } else { - break; - } - } - state.pulleyPointsRecordForTangentCalculation.pop(rmnum); - - XYPoint point1 = null; - XYPoint point2 = null; - if (distance(newPulleyPoint, state.pulleyPointsRecordForTangentCalculation.get(0)) >= 0.05) { - var candidatePoints = CoordinateCalculator.calculateTangentPointCoordinates(newPulleyPoint, - state.pulleyPointsRecordForTangentCalculation.get(0), cfg.pulleyRadius, 0); - point1 = candidatePoints.r; - point2 = candidatePoints.l; - } - // - // 新的轮廓点: - // 考虑掉切线方向上有两个备选点，下面根据不同情况选择一个 - // 1. 第一个轮廓点，假设轮廓点在滑轮质心点的正下方 - // 2. 后续轮廓点，取离上一个轮廓点近的点 - // - if (point1 != null && point2 != null) { - if (state.profilePoints.getLast(0) == null) { - var newPoint = point1.y > point2.y ? point1 : point2; // y轴朝下为正 - state.profilePoints.add(newPoint); - railProfileDrawerListener.onProfilePointPos(newPoint);// 回调 - } else if (state.profilePoints.getLast(0) != null) { - var newPoint = nearPoint(state.profilePoints.getLast(0), point1, point2); - state.profilePoints.add(newPoint); - if (state.profilePoints.getLast(30) != null) {//丢弃掉结束时的点 - railProfileDrawerListener.onProfilePointPos(state.profilePoints.getLast(30));// 回调 - } - } - } - } - } - - - - // - // Utils - // - - static private XYPoint twoAxisArmCoordTransformToXYPointV2(Double arm0Length, Double arm0Rad, Double arm1Length, - Double arm1Rad) { - double x = arm0Length * Math.sin(arm0Rad) + arm1Length * Math.sin(arm0Rad + arm1Rad); - double y = arm0Length * Math.cos(arm0Rad) + arm1Length * Math.cos(arm0Rad + arm1Rad); - return new XYPoint(x, y); - } - - static private boolean isTheSamePoint(XYPoint p0, XYPoint p1) { - if (p0 == null || p1 == null) { - return false; - } - return Math.abs(p0.x - p1.x) < 0.005 && Math.abs(p0.y - p1.y) < 0.005; - // return distance(p0, p1) < 0.001; - } - - static private double distance(XYPoint p0, XYPoint p1) { - return CoordinateCalculator.distance(p0, p1); - } - - - static private boolean isInTheStartPos(XYPoint pos) { - int hight = AppConstantConfig.START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_HIGHT; - int weight = AppConstantConfig.START_OUTLINE_RECORDING_TIMING_JUDGMENT_AREA_WEIGHT; - - if (pos.x <= hight / 2.0 && pos.x >= -hight / 2.0 - && pos.y <= weight / 2.0 && pos.y >= -weight / 2.0) { - return true; - } - return false; - } - - static public XYPoint nearPoint(XYPoint refpoint, XYPoint p1, XYPoint p2) { - double d1 = distance(refpoint, p1); - double d2 = distance(refpoint, p2); - return d1 < d2 ? p1 : p2; - } - -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/TwoLinkArmInverseKinematics.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/TwoLinkArmInverseKinematics.java deleted file mode 100644 index 9024cf9..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/TwoLinkArmInverseKinematics.java +++ /dev/null @@ -1,72 +0,0 @@ -package com.iflytop.profilometer.measure.drawer; - -public class TwoLinkArmInverseKinematics { - - public static void main(String[] args) { - // 机械臂参数 - double L1 = 52; // 第一段臂长 - double L2 = 96; // 第二段臂长 - - // 计算逆运动学 - - int pointNum = 0; - for (double i = -60; i < 60; ) { - double[][] angles = inverseKinematics(i, 80, L1, L2); - if (angles == null) { - System.out.println("无解"); - continue; - } - System.out.println(String.format("%.4f %.4f %.4f %.4f %.4f %.4f", i, 0.0, angles[0][0], angles[0][1], angles[1][0], angles[1][1])); - pointNum++; - i = i + 0.2; - if (pointNum >= 500) { - break; - } - - } - - - } - - /** - * 二轴机械臂逆运动学求解 - * - * @param x 目标位置的 x 坐标 - * @param y 目标位置的 y 坐标 - * @param L1 第一段臂长 - * @param L2 第二段臂长 - * @return 返回两个关节的角度 [theta1, theta2]，单位为弧度。如果无解，返回 null。 - */ - public static double[][] inverseKinematics(double x, double y, double L1, double L2) { - // 计算目标点到原点的距离 - double distanceSquared = y * y + x * x; - double distance = Math.sqrt(distanceSquared); - - // 检查目标点是否在工作空间内 - if (distance > L1 + L2 || distance < Math.abs(L1 - L2)) { - return null; // 无解 - } - - // 计算 theta2（肘部向上和肘部向下两种解） - double cosTheta2 = (distanceSquared - L1 * L1 - L2 * L2) / (2 * L1 * L2); - double sinTheta2 = Math.sqrt(1 - cosTheta2 * cosTheta2); - - // 肘部向上构型 - double theta2_up = Math.atan2(sinTheta2, cosTheta2); - double k1_up = L1 + L2 * Math.cos(theta2_up); - double k2_up = L2 * Math.sin(theta2_up); - double theta1_up = Math.atan2(x, y) - Math.atan2(k2_up, k1_up); - - // 肘部向下构型 - double theta2_down = -Math.atan2(sinTheta2, cosTheta2); - double k1_down = L1 + L2 * Math.cos(theta2_down); - double k2_down = L2 * Math.sin(theta2_down); - double theta1_down = Math.atan2(x, y) - Math.atan2(k2_down, k1_down); - - // 返回两组角度 - return new double[][]{ - {Math.toDegrees(theta1_up), Math.toDegrees(theta2_up)}, // 肘部向上 - {Math.toDegrees(theta1_down), Math.toDegrees(theta2_down)} // 肘部向下 - }; - } -} \ No newline at end of file diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/MeasureSide.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/MeasureSide.java deleted file mode 100644 index 4cfd3ec..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/MeasureSide.java +++ /dev/null @@ -1,18 +0,0 @@ -package com.iflytop.profilometer.measure.drawer.type; - -public enum MeasureSide { - LEFT, RIGHT; - - MeasureSide() { - } - - static public MeasureSide getOpposite(MeasureSide side) { - if (side == LEFT) { - return RIGHT; - } else { - return LEFT; - } - } - - -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileDrawerListener.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileDrawerListener.java deleted file mode 100644 index 35c9106..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileDrawerListener.java +++ /dev/null @@ -1,34 +0,0 @@ -package com.iflytop.profilometer.measure.drawer.type; - -public interface RailProfileDrawerListener { - /** - * 新的质心位置 - */ - void onNewPulleyCentroidPos(XYPoint xyPoint); - - /** - * 新的轮廓点位置 - */ - void onProfilePointPos(XYPoint xyPoint); - - /** - * 开始记录轮廓点信号 - */ - void onStartRecordProfileSig(MeasureSide side); - - /** - * 用户启动记录方向错误 - */ - void onErrorStartRecordProfileSig(); - - /** - * 结束记录轮廓点信号 - */ - void onEndRecordProfileSig(MeasureSide side, Boolean finished); - - /** - * 移动超速，图像可能会发生失真 - */ - void onErrorSpeedDetected(); - -} \ No newline at end of file diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileDrawerState.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileDrawerState.java deleted file mode 100644 index d4b7878..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileDrawerState.java +++ /dev/null @@ -1,42 +0,0 @@ -package com.iflytop.profilometer.measure.drawer.type; - -import com.iflytop.profilometer.measure.ZLockList; - -public class RailProfileDrawerState { - public RailProfileMeasureTaskStatus state = RailProfileMeasureTaskStatus.IDLE; //状态 - - public boolean rightSideFinished = false; // 右侧是否测量完成 - public boolean leftSideFinished = false; // 左侧是否测量完成 - - // StartRecordJudgeContext - public int stayStillDurationCnt = 0; // 停留时间计数 - - // RecordContext - public int speedNow = 0; // 0->100(+) ，用户移动滑轮的速度，速度为接近100时，图像可能会发生失真，需要警告用户,大于100时，需要停止测量 - public boolean errorSpeedDetected = false; // 用户移动滑轮的速度过快 - // public ZLockList pulleyPoints = new ZLockList<>(10); - public ZLockList pulleyPointsRecordForTangentCalculation = new ZLockList<>(10); - public ZLockList profilePoints = new ZLockList<>(); - public XYPoint lastPulleyPoint = new XYPoint(); - - public boolean isInStartPosArea = false; - public boolean isStill = false; - - public MeasureSide measureSide = null; - - - public void resetRecordContext() { - speedNow = 0; - errorSpeedDetected = false; - stayStillDurationCnt = 0; - rightSideFinished = false; - leftSideFinished = false; - isInStartPosArea = false; - isStill = false; - measureSide = null; - // pulleyPoints.clear(); - lastPulleyPoint = null; - profilePoints.clear(); - pulleyPointsRecordForTangentCalculation.clear(); - } -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileMeasureTaskStatus.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileMeasureTaskStatus.java deleted file mode 100644 index 02225d6..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/RailProfileMeasureTaskStatus.java +++ /dev/null @@ -1,32 +0,0 @@ -package com.iflytop.profilometer.measure.drawer.type; - -public enum RailProfileMeasureTaskStatus { - /** - * 空闲 - */ - IDLE, - /** - * 正在测量数据 - */ - MEASURING, - /** - * 等待测量 - */ - WAITING_FOR_MEASURING, - /** - * 测量完成 - */ - FINISHED, - - ; - - public String toCHString() { - return switch (this) { - case IDLE -> "空闲"; - case MEASURING -> "正在测量数据"; - case WAITING_FOR_MEASURING -> "等待测量"; - case FINISHED -> "测量完成"; - default -> "未知状态"; - }; - } -} \ No newline at end of file diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/TrackPoint.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/TrackPoint.java deleted file mode 100644 index fa216a8..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/TrackPoint.java +++ /dev/null @@ -1,44 +0,0 @@ -package com.iflytop.profilometer.measure.drawer.type; - -public class TrackPoint { - public double x; - public double y; - public long date; - - public TrackPoint() { - this.x = 0.0; - this.y = 0.0; - this.date = 0; - } - - public TrackPoint(double x, double y, long date) { - this.x = x; - this.y = y; - this.date = date; - } - - public TrackPoint(XYPoint xyPoint, long date) { - this.x = xyPoint.x; - this.y = xyPoint.y; - this.date = date; - } - - public boolean equals(TrackPoint other, double epsilon) { - if (other == null) { - return false; - } - return Math.abs(this.x - other.x) < epsilon && Math.abs(this.y - other.y) < epsilon; - } - - - - public XYPoint toXYPoint() { - return new XYPoint(x, y); - } - - public String toString() { - return String.format("[%.2f,%.2f]", x, y); - } - - -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/TwoObject.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/TwoObject.java deleted file mode 100644 index 09b15fc..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/TwoObject.java +++ /dev/null @@ -1,16 +0,0 @@ -package com.iflytop.profilometer.measure.drawer.type; - -public class TwoObject { - public R r; - public L l; - - public TwoObject(R r, L l) { - this.r = r; - this.l = l; - } - - - - - -} diff --git a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/XYPoint.java b/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/XYPoint.java deleted file mode 100644 index f248dd3..0000000 --- a/app/src/main/java/com/iflytop/profilometer/measure/drawer/type/XYPoint.java +++ /dev/null @@ -1,47 +0,0 @@ -package com.iflytop.profilometer.measure.drawer.type; - - -public class XYPoint { - public double x; - public double y; - - public XYPoint() { - this.x = 0.0; - this.y = 0.0; - } - - public XYPoint(double x, double y) { - this.x = x; - this.y = y; - } - - public String toString() { - return String.format("(%+.5f, %+.5f)", x, y); - } - - public Boolean equals(XYPoint other, double epsilon) { - if (other == null) { - return false; - } - return Math.abs(this.x - other.x) < epsilon && Math.abs(this.y - other.y) < epsilon; - } - - synchronized public double getX() { - return x; - } - - synchronized public double getY() { - return y; - } - - @Override - public boolean equals(Object other) { - if (other == null) { - return false; - } - if (other instanceof XYPoint) { - return equals((XYPoint) other, 0.0001); - } - return false; - } -} diff --git a/app/src/main/java/com/iflytop/profilometer/model/bo/RailProfileMeasureTaskState.java b/app/src/main/java/com/iflytop/profilometer/model/bo/RailProfileMeasureTaskState.java index e836c30..ced111b 100644 --- a/app/src/main/java/com/iflytop/profilometer/model/bo/RailProfileMeasureTaskState.java +++ b/app/src/main/java/com/iflytop/profilometer/model/bo/RailProfileMeasureTaskState.java @@ -1,7 +1,7 @@ package com.iflytop.profilometer.model.bo; -import com.iflytop.profilometer.measure.drawer.type.RailProfileMeasureTaskStatus; -import com.iflytop.profilometer.measure.drawer.type.XYPoint; +import com.iflytop.profilometer.core.migration.measure.drawer.type.RailProfileMeasureTaskStatus; +import com.iflytop.profilometer.core.migration.measure.drawer.type.XYPoint; import java.util.ArrayList; import java.util.List; diff --git a/app/src/main/java/com/iflytop/profilometer/service/RailProfileMeasureTaskCtrlService.java b/app/src/main/java/com/iflytop/profilometer/service/RailProfileMeasureTaskCtrlService.java index 5cd18d0..226a4fb 100644 --- a/app/src/main/java/com/iflytop/profilometer/service/RailProfileMeasureTaskCtrlService.java +++ b/app/src/main/java/com/iflytop/profilometer/service/RailProfileMeasureTaskCtrlService.java @@ -1,9 +1,9 @@ package com.iflytop.profilometer.service; -import com.iflytop.profilometer.measure.drawer.RailProfileDrawer; -import com.iflytop.profilometer.measure.drawer.type.MeasureSide; -import com.iflytop.profilometer.measure.drawer.type.RailProfileDrawerListener; -import com.iflytop.profilometer.measure.drawer.type.XYPoint; +import com.iflytop.profilometer.core.migration.measure.drawer.RailProfileDrawer; +import com.iflytop.profilometer.core.migration.measure.drawer.type.MeasureSide; +import com.iflytop.profilometer.core.migration.measure.drawer.type.RailProfileDrawerListener; +import com.iflytop.profilometer.core.migration.measure.drawer.type.XYPoint; import com.iflytop.profilometer.model.bo.RailProfileMeasureTaskState; public class RailProfileMeasureTaskCtrlService implements RailProfileDrawerListener {