diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt index e281fd2..a8dd294 100644 --- a/app/CMakeLists.txt +++ b/app/CMakeLists.txt @@ -7,5 +7,8 @@ set(libs_include_DIR src/main/cpp/opus/include) include_directories(${libs_include_DIR}) link_directories(${libs_DIR}) +# Opus JNI add_library( opusJni SHARED src/main/cpp/opus-lib.cpp) target_link_libraries( opusJni opus ${log-lib} ) + +# Head Touch JNI module has been removed diff --git a/app/src/main/java/com/ismart/ib86/app/MainActivity.java b/app/src/main/java/com/ismart/ib86/app/MainActivity.java index 33881c8..3d1c5ea 100644 --- a/app/src/main/java/com/ismart/ib86/app/MainActivity.java +++ b/app/src/main/java/com/ismart/ib86/app/MainActivity.java @@ -98,7 +98,6 @@ public class MainActivity extends AppCompatActivity implements WebSocketManager. private boolean isPlaying = false; private String sessionId = ""; // 添加session_id字段 private long lastAudioDataTime = 0; // 新增：记录最后一次收到音频数据的时间 - private Thread motorControlThread; // 用于跟踪电机控制线程 private boolean isCheckingPlaybackStatus = false; //xiaozhi end // HeadTouchManager removed @@ -159,8 +158,8 @@ public class MainActivity extends AppCompatActivity implements WebSocketManager. // btnRotateForward.setOnClickListener(v -> { // if (motorController != null) { // int dutyCycle = (int) (PWM_PERIOD * currentDutyCyclePercent / 100.0); -// motorController.rotateReverse(PWM_PERIOD, 0); // 确保反转PWM关闭 -// motorController.rotateForward(PWM_PERIOD, dutyCycle); +// motorController.rotateVerticalDown(PWM_PERIOD, 0); // 确保垂直向下PWM关闭 +// motorController.rotateVerticalUp(PWM_PERIOD, dutyCycle); // } // }); // @@ -168,8 +167,8 @@ public class MainActivity extends AppCompatActivity implements WebSocketManager. // btnRotateReverse.setOnClickListener(v -> { // if (motorController != null) { // int dutyCycle = (int) (PWM_PERIOD * currentDutyCyclePercent / 100.0); -// motorController.rotateForward(PWM_PERIOD, 0); // 确保正转PWM关闭 -// motorController.rotateReverse(PWM_PERIOD, dutyCycle); +// motorController.rotateVerticalUp(PWM_PERIOD, 0); // 确保垂直向上PWM关闭 +// motorController.rotateVerticalDown(PWM_PERIOD, dutyCycle); // } // }); @@ -207,7 +206,34 @@ public class MainActivity extends AppCompatActivity implements WebSocketManager. // 初始化设备管理器 initDeviceManager(); - testMotorControl(); + // testMotorControl(); + + + motorController.setVerticalSensorEnabled(true); + + new Thread(() -> { + boolean direction = true; + try { + motorController.controlVerticalCalibration(); + Thread.sleep(2000); + int max = motorController.getVerticalMaxAngle(); + int min = motorController.getVerticalMinAngle(); + int mid = max - min; + Log.d(TAG,"ver max:" + max + " ver min:"+min + " mid:"+ mid); +// while(true) +// { +// try { +// angleListener.rotateToAngle(direction, direction? 15 : 15 , direction ? 35 : 12); +// direction = !direction; +// Thread.sleep(200); +// }catch (InterruptedException e) { +// Log.e(TAG, "初始化延时失败", e); +// } +// } + } catch (InterruptedException e) { + Log.e(TAG, "初始化延时失败", e); + } + }).start(); //xiaozhi initAudio(); @@ -366,10 +392,54 @@ public class MainActivity extends AppCompatActivity implements WebSocketManager. // deviceManager.sendWearDetectionRequest(); // handler.postDelayed(wearDetectionRunnable, 1000); } - + @Override + protected void onStop(){ + super.onStop(); + Log.d(TAG, "onStop"); + } @Override protected void onDestroy() { super.onDestroy(); + Log.d(TAG, "onDestroy"); + // 移至后台线程执行耗时操作，避免ANR + new Thread(() -> { + try { + + // 关闭GPIO电源 + if (gpioManager != null) { + try { + gpioManager.setValue(BT_POWER_EN, GpioManager.VALUE_LOW); + } catch (Exception e) { + LogManager.e(TAG, "关闭GPIO电源失败: " + e.getMessage()); + } + gpioManager = null; + } + + // 释放电机控制器资源 + if (motorController != null) { + try { + motorController.release(); + LogManager.d(TAG, "电机控制器资源已释放"); + } catch (Exception e) { + LogManager.e(TAG, "释放电机控制器资源失败: " + e.getMessage()); + } + } + // 停止所有测量 + if (deviceManager != null) { + deviceManager.stopGH3220Measure(ASR5515Protocol.GH3220MeasureType.HR); + deviceManager.stopGH3220Measure(ASR5515Protocol.GH3220MeasureType.SPO2); + deviceManager.stopGH3220Measure(ASR5515Protocol.GH3220MeasureType.LEAD); + deviceManager.syncHostStatus(false); + deviceManager.close(); + deviceManager = null; + } + + + } catch (Exception e) { + LogManager.e(TAG, "资源释放过程中发生错误: " + e.getMessage()); + } + }).start(); + isDestroyed = true; hasStartedCall = false; if (webSocketManager != null) { @@ -412,55 +482,6 @@ public class MainActivity extends AppCompatActivity implements WebSocketManager. animationManager.release(); animationManager = null; } - - // 移至后台线程执行耗时操作，避免ANR - new Thread(() -> { - try { - // 停止所有测量 - if (deviceManager != null) { - deviceManager.stopGH3220Measure(ASR5515Protocol.GH3220MeasureType.HR); - deviceManager.stopGH3220Measure(ASR5515Protocol.GH3220MeasureType.SPO2); - deviceManager.stopGH3220Measure(ASR5515Protocol.GH3220MeasureType.LEAD); - deviceManager.syncHostStatus(false); - deviceManager.close(); - deviceManager = null; - } - - // 关闭GPIO电源 - if (gpioManager != null) { - try { - gpioManager.setValue(BT_POWER_EN, GpioManager.VALUE_LOW); - } catch (Exception e) { - LogManager.e(TAG, "关闭GPIO电源失败: " + e.getMessage()); - } - gpioManager = null; - } - - // 中断电机控制线程 - if (motorControlThread != null && motorControlThread.isAlive()) { - try { - motorControlThread.interrupt(); - motorControlThread.join(1000); // 等待线程结束，最多等待1秒 - LogManager.d(TAG, "电机控制线程已中断"); - } catch (InterruptedException e) { - LogManager.e(TAG, "等待电机控制线程结束时被中断"); - } - motorControlThread = null; - } - - // 释放电机控制器资源 - if (motorController != null) { - try { - motorController.release(); - LogManager.d(TAG, "电机控制器资源已释放"); - } catch (Exception e) { - LogManager.e(TAG, "释放电机控制器资源失败: " + e.getMessage()); - } - } - } catch (Exception e) { - LogManager.e(TAG, "资源释放过程中发生错误: " + e.getMessage()); - } - }).start(); } private Handler handler = new Handler(); @@ -483,7 +504,7 @@ public class MainActivity extends AppCompatActivity implements WebSocketManager. } } -private void hideSystemBars() { + private void hideSystemBars() { Window window = getWindow(); if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.R) { WindowInsetsController insetsController = window.getInsetsController(); @@ -505,93 +526,14 @@ private void hideSystemBars() { /** * 测试电机控制功能 */ -private void testMotorControl() { + private void testMotorControl() { if (motorController == null) { LogManager.e(TAG, "电机控制器未初始化"); return; } + byte[] data = new byte[1]; + motorController.sendCommand(MotorController.CMD_CYCLE_TEST, new byte[]{0x01}); - // 如果之前的线程还在运行，先中断它 - if (motorControlThread != null && motorControlThread.isAlive()) { - motorControlThread.interrupt(); - try { - motorControlThread.join(1000); // 等待线程结束，最多等待1秒 - } catch (InterruptedException e) { - LogManager.e(TAG, "等待之前的电机控制线程结束时被中断"); - } - } - - motorControlThread = new Thread(() -> { - try { - // 打开电机 - LogManager.d(TAG, "打开电机"); - if (!motorController.turnOnMotor()) { - LogManager.e(TAG, "打开电机失败"); - return; - } - // 设置PWM参数 - final int period = 1000000; - final int fdutyCycle = 1000000; // 使用90%的占空比，避免duty_cycle等于period - final int rdutyCycle = 1000000; // 使用90%的占空比，避免duty_cycle等于period -// Thread.sleep(2000); -// motorController.rotateReverse(period, 0); // 确保反转PWM关闭 -// if (!motorController.rotateForward(period, dutyCycle)) { -// LogManager.e(TAG, "电机正转失败"); -// } -// Thread.sleep(2000); -// // 电机反转 -// motorController.rotateForward(period, 0); -// if (!motorController.rotateReverse(period, dutyCycle)) { -// LogManager.e(TAG, "电机反转失败"); -// } - - // 循环20次正反转 - for (int i = 0; i < 10000; i++) { - LogManager.d(TAG, "开始第" + (i + 1) + "次正反转测试"); - - // 电机正转 - LogManager.d(TAG, "电机正转"); - motorController.rotateReverse(period, 0); // 确保反转PWM关闭 - if (!motorController.rotateForward(period, fdutyCycle)) { - LogManager.e(TAG, "电机正转失败"); - break; - } - - // 等待100ms - Thread.sleep(120); - - // 停止正转 - motorController.rotateForward(period, 0); - Thread.sleep(1000); - // 电机反转 - LogManager.d(TAG, "电机反转"); - if (!motorController.rotateReverse(period, rdutyCycle)) { - LogManager.e(TAG, "电机反转失败"); - break; - } - - // 等待100ms - Thread.sleep(40); - - // 停止反转 - motorController.rotateReverse(period, 0); - - Thread.sleep(1000); - } - - // 测试完成，关闭电机 - LogManager.d(TAG, "电机测试完成，关闭电机"); - motorController.turnOffMotor(); - - } catch (InterruptedException e) { - LogManager.e(TAG, "电机测试被中断: " + e.getMessage()); - motorController.turnOffMotor(); - } catch (Exception e) { - LogManager.e(TAG, "电机测试异常: " + e.getMessage()); - motorController.turnOffMotor(); - } - }); - motorControlThread.start(); } @Override diff --git a/app/src/main/java/com/ismart/ib86/feature/motor/MotorController.java b/app/src/main/java/com/ismart/ib86/feature/motor/MotorController.java index b7577e3..2f31c42 100644 --- a/app/src/main/java/com/ismart/ib86/feature/motor/MotorController.java +++ b/app/src/main/java/com/ismart/ib86/feature/motor/MotorController.java @@ -1,12 +1,24 @@ package com.ismart.ib86.feature.motor; +import android.os.Handler; +import android.os.Looper; import android.util.Log; +import android.serialport.SerialPort; import com.ismart.ib86.common.utils.LogManager; import com.ismart.ib86.feature.gpio.GpioManager; +import java.io.BufferedReader; +import java.io.File; +import java.io.FileDescriptor; +import java.io.FileInputStream; +import java.io.FileOutputStream; +import java.io.FileReader; import java.io.FileWriter; import java.io.IOException; +import java.util.Arrays; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; /** * 电机控制器类，用于控制机器人抬头点头的电机 @@ -16,28 +28,67 @@ public class MotorController { private static final String TAG = "MotorController"; private static volatile MotorController instance; - // GPIO控制常量 - private static final int MOTOR_CONTROL_GPIO = 59; // 电机开关GPIO - private static final int MOTOR_SLEEP_GPIO = 65; - private static final int GPIO_HIGH = 1; // GPIO高电平 - private static final int GPIO_LOW = 0; // GPIO低电平 - - // PWM路径常量 - private static final String PWM_FORWARD_PATH = "/sys/class/pwm/pwmchip0/pwm0/"; - private static final String PWM_REVERSE_PATH = "/sys/class/pwm/pwmchip2/pwm0/"; - private static final String PWM_PERIOD_FILE = "period"; - private static final String PWM_DUTY_CYCLE_FILE = "duty_cycle"; - private static final String PWM_ENABLE_FILE = "enable"; + // 水平电机串口控制常量 + private static final String HORIZONTAL_SERIAL_PORT = "/dev/ttyS2"; + private static final int HORIZONTAL_BAUD_RATE = 115200; + private static final byte START_BYTE = (byte)0xAA; + private static final byte END_BYTE = 0x55; + + // 垂直电机控制常量 + private static final String VERTICAL_ROTATE_TO_ANGLE_PATH = "/sys/bus/i2c/drivers/kth5502/8-006a/rotate_to_angle"; + private static final int VERTICAL_DIRECTION_UP = 1; // 抬头 + private static final int VERTICAL_DIRECTION_DOWN = 0; // 低头 + + // 垂直位置传感器常量 + private static final String VERTICAL_SENSOR_ENABLE_PATH = "/sys/bus/i2c/drivers/kth5502/8-006a/work_enabled"; + private static final int SENSOR_ENABLE_ON = 1; // 打开传感器 + private static final int SENSOR_ENABLE_OFF = 0; // 关闭传感器 + + // 垂直方向校准常量 + private static final String VERTICAL_CALIBRATE_PATH = "/sys/bus/i2c/drivers/kth5502/8-006a/calibrate"; + private static final int VERTICAL_CALIBRATE_START = 1; // 开始垂直方向校准 + private static final int VERTICAL_CALIBRATE_STOP = 0; // 停止垂直方向校准 + + // 垂直方向角度限制常量 + private static final String VERTICAL_MAX_ANGLE_PATH = "/sys/bus/i2c/drivers/kth5502/8-006a/max_angle"; + private static final String VERTICAL_MIN_ANGLE_PATH = "/sys/bus/i2c/drivers/kth5502/8-006a/min_angle"; + + // 水平电机命令常量 + public static final byte CMD_ROTATE_TO_ANGLE = 0x04; // 控制电机转动到目标角度 + public static final byte CMD_GET_CURRENT_ANGLE = 0x05; // 获取当前角度 + public static final byte CMD_GET_MAX_ANGLE = 0x06; // 获取正转/反转最大角度 + public static final byte CMD_CYCLE_TEST = 0x07; // 开启循环测试 + + // 方向常量 + public static final byte DIRECTION_FORWARD = 0x00; // 正转 + public static final byte DIRECTION_REVERSE = 0x01; // 反转 // 电机状态枚举 public enum MotorState { STOPPED, // 停止状态 - FORWARD, // 正转状态 - REVERSE // 反转状态 + FORWARD, // 正转状态（上下） + REVERSE, // 反转状态（上下） + ROTATING_LEFT, // 向左转动状态（水平） + ROTATING_RIGHT // 向右转动状态（水平） } // 当前电机状态 private MotorState currentState = MotorState.STOPPED; + private MotorState horizontalState = MotorState.STOPPED; + private MotorState verticalState = MotorState.STOPPED; + private int horizontalSpeed = 50; // 默认水平转动速度50% + private int verticalSpeed = 50; // 默认垂直转动速度50% + + // 串口通信相关变量 + private SerialPort horizontalSerialPort; + private FileInputStream horizontalInputStream; + private FileOutputStream horizontalOutputStream; + private boolean isHorizontalSerialOpen = false; + private ExecutorService serialExecutor; + private Handler mainHandler; + + // 数据接收回调 + private OnHorizontalDataReceivedListener horizontalDataListener; /** * 获取MotorController单例实例 @@ -58,7 +109,69 @@ public class MotorController { * 私有构造函数，防止外部实例化 */ private MotorController() { - // 私有构造函数 + // 初始化线程池和Handler + serialExecutor = Executors.newSingleThreadExecutor(); + mainHandler = new Handler(Looper.getMainLooper()); + + // 检查垂直位置传感器文件是否存在 + File verticalSensorFile = new File(VERTICAL_SENSOR_ENABLE_PATH); + if (!verticalSensorFile.exists()) { + LogManager.e(TAG, "Vertical sensor control file not found: " + VERTICAL_SENSOR_ENABLE_PATH); + } else { + LogManager.d(TAG, "Vertical sensor control file found"); + } + + // 检查垂直方向校准功能文件是否存在 + File verticalCalibrateFile = new File(VERTICAL_CALIBRATE_PATH); + if (!verticalCalibrateFile.exists()) { + LogManager.e(TAG, "Vertical calibration control file not found: " + VERTICAL_CALIBRATE_PATH); + } else { + LogManager.d(TAG, "Vertical calibration control file found"); + } + + // 检查垂直方向最大角度文件是否存在 + File verticalMaxAngleFile = new File(VERTICAL_MAX_ANGLE_PATH); + if (!verticalMaxAngleFile.exists()) { + LogManager.e(TAG, "Vertical max angle control file not found: " + VERTICAL_MAX_ANGLE_PATH); + } else { + LogManager.d(TAG, "Vertical max angle control file found"); + } + + // 检查垂直方向最小角度文件是否存在 + File verticalMinAngleFile = new File(VERTICAL_MIN_ANGLE_PATH); + if (!verticalMinAngleFile.exists()) { + LogManager.e(TAG, "Vertical min angle control file not found: " + VERTICAL_MIN_ANGLE_PATH); + } else { + LogManager.d(TAG, "Vertical min angle control file found"); + } + + // 检查垂直旋转至角度文件是否存在 + File verticalRotateToAngleFile = new File(VERTICAL_ROTATE_TO_ANGLE_PATH); + if (!verticalRotateToAngleFile.exists()) { + LogManager.e(TAG, "Vertical rotate to angle control file not found: " + VERTICAL_ROTATE_TO_ANGLE_PATH); + } else { + LogManager.d(TAG, "Vertical rotate to angle control file found"); + } + } + + /** + * 水平电机数据接收监听器接口 + */ + public interface OnHorizontalDataReceivedListener { + /** + * 当接收到数据时调用 + * @param cmd 命令字节 + * @param data 数据字节数组 + */ + void onDataReceived(byte cmd, byte[] data); + } + + /** + * 设置水平电机数据接收监听器 + * @param listener 监听器 + */ + public void setOnHorizontalDataReceivedListener(OnHorizontalDataReceivedListener listener) { + this.horizontalDataListener = listener; } /** @@ -67,183 +180,534 @@ public class MotorController { */ public boolean init() { try { - // 初始化GPIO - GpioManager.getInstance().setDirection(MOTOR_CONTROL_GPIO, GpioManager.DIRECTION_OUT); - // 默认关闭电机 - return turnOffMotor(); + // 初始化水平电机串口 + initHorizontalSerial(); + return true; } catch (Exception e) { Log.e(TAG, "Failed to initialize motor controller", e); return false; } } - + /** - * 打开电机 - * @return 是否成功 + * 初始化水平电机串口 + * @return 是否初始化成功 */ - public boolean turnOnMotor() { + private boolean initHorizontalSerial() { try { - rotateForward(1000000, 0); // 确保正转PWM关闭 - enablePwm(PWM_FORWARD_PATH, false); - enablePwm(PWM_FORWARD_PATH, true); - rotateReverse(1000000, 0); // 确保反转PWM关闭 - enablePwm(PWM_REVERSE_PATH, true); - boolean state = GpioManager.getInstance().setValue(MOTOR_CONTROL_GPIO, GPIO_HIGH); - // 设置SLEEP - GpioManager.getInstance().setDirection(MOTOR_SLEEP_GPIO, GpioManager.DIRECTION_OUT); - GpioManager.getInstance().setValue(MOTOR_SLEEP_GPIO, GpioManager.VALUE_HIGH); - return state; - } catch (Exception e) { - Log.e(TAG, "Failed to turn on motor", e); - return false; - } - } - - /** - * 关闭电机 - * @return 是否成功 - */ - public boolean turnOffMotor() { - try { - // 根据当前状态将相应PWM通道的参数设为0 - String pwmPath; - switch (currentState) { - case FORWARD: - pwmPath = PWM_FORWARD_PATH; - break; - case REVERSE: - pwmPath = PWM_REVERSE_PATH; - break; - case STOPPED: - default: - // 如果已经是停止状态，直接关闭GPIO - return GpioManager.getInstance().setValue(MOTOR_CONTROL_GPIO, GPIO_LOW); + // 打开串口 + File device = new File(HORIZONTAL_SERIAL_PORT); + if (!device.exists()) { + Log.e(TAG, "Serial port device not found: " + HORIZONTAL_SERIAL_PORT); + return false; } - // 将period和dutyCycle设为0 - setPwmParameters(pwmPath, 1000000, 0); - - // 关闭电机控制GPIO - boolean result = GpioManager.getInstance().setValue(MOTOR_CONTROL_GPIO, GPIO_LOW); - - // 更新状态为停止 - currentState = MotorState.STOPPED; - - return result; - } catch (Exception e) { - Log.e(TAG, "Failed to turn off motor", e); - return false; - } - } - - /** - * 控制电机正转 - * @param period PWM周期（纳秒） - * @param dutyCycle PWM占空比（纳秒） - * @return 是否成功 - */ - public boolean rotateForward(int period, int dutyCycle) { - if (setPwmParameters(PWM_FORWARD_PATH, period, dutyCycle)) { - currentState = MotorState.FORWARD; - return true; - } - return false; - } - - /** - * 控制电机反转 - * @param period PWM周期（纳秒） - * @param dutyCycle PWM占空比（纳秒） - * @return 是否成功 - */ - public boolean rotateReverse(int period, int dutyCycle) { - if (setPwmParameters(PWM_REVERSE_PATH, period, dutyCycle)) { - currentState = MotorState.REVERSE; - return true; - } - return false; - } - - /** - * 设置PWM参数 - * @param basePath PWM基础路径 - * @param period 周期 - * @param dutyCycle 占空比 - * @return 是否成功 - */ - private boolean setPwmParameters(String basePath, int period, int dutyCycle) { - try { - //enablePwm(basePath, false); - writeToFile(basePath + PWM_PERIOD_FILE, String.valueOf(period)); - writeToFile(basePath + PWM_DUTY_CYCLE_FILE, String.valueOf(dutyCycle)); - // 使能PWM - //enablePwm(basePath, true); - return true; - } catch (IOException e) { - Log.e(TAG, "Failed to set PWM parameters", e); - return false; - } - } - - /** - * 写入文件 - * @param filePath 文件路径 - * @param content 内容 - * @throws IOException IO异常 - */ - private void writeToFile(String filePath, String content) throws IOException { - FileWriter writer = null; - try { - writer = new FileWriter(filePath); - writer.write(content); - } finally { - if (writer != null) { - try { - writer.close(); - } catch (IOException e) { - Log.e(TAG, "Failed to close file writer", e); - } + try { + // 使用Android串口库打开串口 + horizontalSerialPort = new SerialPort(new File(HORIZONTAL_SERIAL_PORT), HORIZONTAL_BAUD_RATE); + horizontalInputStream = (FileInputStream) horizontalSerialPort.getInputStream(); + horizontalOutputStream = (FileOutputStream) horizontalSerialPort.getOutputStream(); + isHorizontalSerialOpen = true; + + // 启动接收线程 + startReceiving(); + + Log.i(TAG, "Horizontal motor serial port initialized successfully"); + return true; + } catch (Exception e) { + Log.e(TAG, "Failed to open serial port", e); + return false; } + } catch (Exception e) { + Log.e(TAG, "Failed to initialize horizontal serial", e); + return false; } } - /** - * 控制PWM使能状态 - * @param basePath PWM基础路径 - * @param enable true表示使能，false表示禁用 - * @return 是否成功 - */ - public boolean enablePwm(String basePath, boolean enable) { - try { - writeToFile(basePath + PWM_ENABLE_FILE, enable ? "1" : "0"); - return true; - } catch (IOException e) { - Log.e(TAG, "Failed to " + (enable ? "enable" : "disable") + " PWM", e); - return false; - } - } - - /** - * 控制电机旋转到指定角度（预留API，暂不实现） - * @param angle 目标角度（度） - * @param speed 旋转速度（可选参数） - * @return 是否成功 - */ - public boolean rotateToAngle(float angle, float speed) { - // TODO: 实现角度控制功能 - Log.i(TAG, "rotateToAngle API called, but not implemented yet"); - return false; - } - - /** * 释放资源 */ public void release() { try { - // 关闭电机 - turnOffMotor(); + sendCommand(MotorController.CMD_CYCLE_TEST, new byte[]{0x00}); + + // 停止接收线程 + stopReceiving(); + + // 关闭串口 + closeHorizontalSerial(); } catch (Exception e) { Log.e(TAG, "Failed to release resources", e); } } + /** + * 关闭水平电机串口 + */ + private void closeHorizontalSerial() { + if (horizontalSerialPort != null) { + try { + if (horizontalInputStream != null) { + horizontalInputStream.close(); + horizontalInputStream = null; + } + if (horizontalOutputStream != null) { + horizontalOutputStream.close(); + horizontalOutputStream = null; + } + horizontalSerialPort.closePort(); + horizontalSerialPort = null; + isHorizontalSerialOpen = false; + } catch (IOException e) { + Log.e(TAG, "Failed to close horizontal serial port", e); + } + } + } + + /** + * 启动数据接收线程 + */ + private void startReceiving() { + if (!isHorizontalSerialOpen || horizontalInputStream == null) { + Log.e(TAG, "Cannot start receiving: serial port not open"); + return; + } + + serialExecutor.execute(new Runnable() { + @Override + public void run() { + byte[] buffer = new byte[1024]; + int size; + + try { + while (isHorizontalSerialOpen) { + if (horizontalInputStream.available() > 0) { + size = horizontalInputStream.read(buffer); + Log.d(TAG, "size :" + size); + if (size > 0) { + processReceivedData(buffer, size); + } + } + try { + Thread.sleep(10); // 短暂休眠，避免CPU占用过高 + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + break; + } + } + } catch (IOException e) { + Log.e(TAG, "Error reading from serial port", e); + } + } + }); + } + + /** + * 停止数据接收线程 + */ + private void stopReceiving() { + isHorizontalSerialOpen = false; + } + + /** + * 处理接收到的数据 + * @param buffer 数据缓冲区 + * @param size 数据大小 + */ + private void processReceivedData(byte[] buffer, int size) { + // 查找起始字节 + Log.d(TAG, "[ttyS2] Received data: " + bytesToHexString(buffer)); + + int startIndex = -1; + for (int i = 0; i < size; i++) { + if (buffer[i] == START_BYTE) { + startIndex = i; + break; + } + } + + if (startIndex == -1) { + return; // 没有找到起始字节 + } + + // 确保数据包至少包含起始字节、长度、命令、CRC和结束字节 + if (startIndex + 5 >= size) { + return; // 数据不完整 + } + + // 获取数据长度 + int dataLen = buffer[startIndex + 1] & 0xFF; + + // 确保数据包完整 + if (startIndex + 2 + dataLen + 2 + 1 > size) { + return; // 数据不完整 + } + + // 检查结束字节 + if (buffer[startIndex + 2 + dataLen + 2] != END_BYTE) { + return; // 结束字节不匹配 + } + + // 提取命令字节 + byte cmd = buffer[startIndex + 2]; + + // 提取数据部分（如果有） + byte[] data = null; + if (dataLen > 1) { // 数据长度包含命令字节，所以要大于1才有数据 + data = new byte[dataLen - 1]; + System.arraycopy(buffer, startIndex + 3, data, 0, dataLen - 1); + } + + // 验证CRC + byte[] crcBytes = new byte[2]; + System.arraycopy(buffer, startIndex + 2 + dataLen, crcBytes, 0, 2); + int receivedCrc = ((crcBytes[0] & 0xFF) << 8) | (crcBytes[1] & 0xFF); + + // 准备CRC计算的数据（包含长度字节、命令字节和数据字节） + byte[] crcData = new byte[dataLen + 1]; // 长度字节 + 命令字节 + 数据字节 + crcData[0] = buffer[startIndex + 1]; // 长度字节 + crcData[1] = cmd; // 命令字节 + if (data != null) { + System.arraycopy(data, 0, crcData, 2, data.length); + } + int calculatedCrc = calculateCRC16(crcData); + + // 验证CRC + if (receivedCrc != calculatedCrc) { + StringBuilder sb = new StringBuilder(); + for (byte b : crcData) { + sb.append(String.format("0x%02X ", b & 0xFF)); + } + Log.e(TAG, "CRC check failed: received=" + receivedCrc + ", calculated=" + calculatedCrc); + Log.e(TAG, "CRC data: " + sb.toString()); + Log.e(TAG, "Packet: " + bytesToHexString(Arrays.copyOfRange(buffer, startIndex, startIndex + 2 + dataLen + 3))); + return; + } + + // 通知监听器 + final byte finalCmd = cmd; + final byte[] finalData = data; + if (horizontalDataListener != null) { + mainHandler.post(new Runnable() { + @Override + public void run() { + horizontalDataListener.onDataReceived(finalCmd, finalData); + } + }); + } + } + + /** + * 计算CRC16校验码 (MODBUS标准) + * @param data 数据 + * @return CRC16校验码 + */ + private int calculateCRC16(byte[] data) { + int crc = 0xFFFF; + + if (data == null || data.length == 0) { + return crc; + } + + for (int i = 0; i < data.length; i++) { + crc ^= (data[i] & 0xFF); + for (int j = 0; j < 8; j++) { + if ((crc & 0x0001) != 0) { + crc = (crc >>> 1) ^ 0xA001; + } else { + crc = crc >>> 1; + } + } + } + + // 调试输出 + StringBuilder sb = new StringBuilder(); + for (byte b : data) { + sb.append(String.format("0x%02X ", b & 0xFF)); + } + //Log.d(TAG, String.format("CRC16 data: %s, result: 0x%04X", sb.toString(), crc)); + + return crc; + } + + /** + * 发送命令到水平电机 + * @param cmd 命令字节 + * @param data 数据字节数组，可以为null + * @return 是否发送成功 + */ + public boolean sendCommand(byte cmd, byte[] data) { + if (!isHorizontalSerialOpen || horizontalOutputStream == null) { + Log.e(TAG, "Cannot send command: serial port not open"); + return false; + } + + try { + // 计算数据长度（包含命令字节） + int dataLen = (data == null) ? 1 : data.length + 1; + + // 构建完整的数据包 + byte[] packet = new byte[dataLen + 5]; // 起始字节 + 长度 + 数据(含命令) + CRC(2字节) + 结束字节 + packet[0] = START_BYTE; + packet[1] = (byte) dataLen; + packet[2] = cmd; + + if (data != null) { + System.arraycopy(data, 0, packet, 3, data.length); + } + + // 准备CRC计算的数据（从长度字节到数据字节） + byte[] crcData = new byte[dataLen + 1]; // 长度字节 + 命令字节 + 数据字节 + crcData[0] = (byte) dataLen; + crcData[1] = cmd; + if (data != null) { + System.arraycopy(data, 0, crcData, 2, data.length); + } + + // 计算CRC + int crc = calculateCRC16(crcData); + + // 添加CRC（高字节在前，低字节在后 - 根据实际协议） + packet[dataLen + 2] = (byte) ((crc >> 8) & 0xFF); // 高字节 + packet[dataLen + 3] = (byte) (crc & 0xFF); // 低字节 + + // 添加结束字节 + packet[dataLen + 4] = END_BYTE; + + Log.d(TAG, "[ttyS2] Sending data: " + bytesToHexString(packet)); + // 发送数据 + horizontalOutputStream.write(packet); + horizontalOutputStream.flush(); + + return true; + } catch (IOException e) { + Log.e(TAG, "Failed to send command", e); + return false; + } + } + + /** + * 控制水平电机转动到指定角度 + * @param direction 方向：0x00=正转，0x01=反转 + * @param angle 目标角度（度），实际发送时会乘以10 + * @param speed 速度百分比（0-100） + * @return 是否发送成功 + */ + public boolean rotateHorizontalToAngle(byte direction, float angle, int speed) { + // 将角度转换为原始值（乘以10） + int rawAngle = (int) (angle * 10); + + // 限制速度范围 + int limitedSpeed = Math.max(0, Math.min(100, speed)); + + // 构建数据包 + byte[] data = new byte[4]; + data[0] = direction; // 方向 + data[1] = (byte) ((rawAngle >> 8) & 0xFF); // 角度高字节 + data[2] = (byte) (rawAngle & 0xFF); // 角度低字节 + data[3] = (byte) limitedSpeed; // 占空比 + + // 发送命令 + return sendCommand(CMD_ROTATE_TO_ANGLE, data); + } + + /** + * 获取水平电机当前角度 + * @return 是否发送成功 + */ + public boolean getHorizontalCurrentAngle() { + return sendCommand(CMD_GET_CURRENT_ANGLE, null); + } + + /** + * 获取水平电机正转/反转最大角度 + * @return 是否发送成功 + */ + public boolean getHorizontalMaxAngle() { + return sendCommand(CMD_GET_MAX_ANGLE, null); + } + + /** + * 控制水平电机循环测试 + * @param start true表示启动循环测试，false表示停止循环测试 + * @return 是否发送成功 + */ + public boolean controlHorizontalCycleTest(boolean start) { + byte[] data = new byte[1]; + data[0] = start ? (byte)0x01 : (byte)0x00; + return sendCommand(CMD_CYCLE_TEST, data); + } + + /** + * 设置垂直位置传感器工作状态 + * @param enable true=打开传感器并开始监听角度变化，false=关闭传感器并停止监听 + * @return 是否设置成功 + */ + public boolean setVerticalSensorEnabled(boolean enable) { + try { + FileWriter writer = new FileWriter(VERTICAL_SENSOR_ENABLE_PATH); + writer.write(enable ? String.valueOf(SENSOR_ENABLE_ON) : String.valueOf(SENSOR_ENABLE_OFF)); + writer.flush(); + writer.close(); + + // 启用/禁用角度监听 + LogManager.d(TAG, "Vertical sensor set to: " + (enable ? "ON" : "OFF")); + return true; + } catch (IOException e) { + LogManager.e(TAG, "Failed to set vertical sensor state: " + e.getMessage()); + return false; + } + } + + /** + * 获取垂直位置传感器当前工作状态 + * @return true=传感器已打开，false=传感器已关闭或读取失败 + */ + public boolean isVerticalSensorEnabled() { + try { + BufferedReader reader = new BufferedReader(new FileReader(VERTICAL_SENSOR_ENABLE_PATH)); + String line = reader.readLine(); + reader.close(); + return line != null && line.trim().equals(String.valueOf(SENSOR_ENABLE_ON)); + } catch (IOException e) { + LogManager.e(TAG, "Failed to read vertical sensor state: " + e.getMessage()); + return false; + } + } + + /** + * 触发垂直方向自动校准功能 + * @return 是否控制成功 + */ + public boolean controlVerticalCalibration() { + try { + FileWriter writer = new FileWriter(VERTICAL_CALIBRATE_PATH); + writer.write(String.valueOf(VERTICAL_CALIBRATE_START)); + writer.flush(); + writer.close(); + LogManager.d(TAG, "Vertical calibration started"); + return true; + } catch (IOException e) { + LogManager.e(TAG, "Failed to control vertical calibration: " + e.getMessage()); + return false; + } + } + + /** + * 设置垂直方向最大角度限制 + * @param angle 垂直方向最大角度值 + * @return 是否设置成功 + */ + public boolean setVerticalMaxAngle(int angle) { + try { + FileWriter writer = new FileWriter(VERTICAL_MAX_ANGLE_PATH); + writer.write(String.valueOf(angle)); + writer.flush(); + writer.close(); + LogManager.d(TAG, "Vertical max angle set to: " + angle); + return true; + } catch (IOException e) { + LogManager.e(TAG, "Failed to set vertical max angle: " + e.getMessage()); + return false; + } + } + + /** + * 获取当前垂直方向最大角度限制 + * @return 垂直方向最大角度值，读取失败返回-1 + */ + public int getVerticalMaxAngle() { + try { + BufferedReader reader = new BufferedReader(new FileReader(VERTICAL_MAX_ANGLE_PATH)); + String line = reader.readLine(); + reader.close(); + return line != null ? Integer.parseInt(line.trim()) : -1; + } catch (IOException | NumberFormatException e) { + LogManager.e(TAG, "Failed to read vertical max angle: " + e.getMessage()); + return -1; + } + } + + /** + * 设置垂直方向最小角度限制 + * @param angle 垂直方向最小角度值 + * @return 是否设置成功 + */ + public boolean setVerticalMinAngle(int angle) { + try { + FileWriter writer = new FileWriter(VERTICAL_MIN_ANGLE_PATH); + writer.write(String.valueOf(angle)); + writer.flush(); + writer.close(); + LogManager.d(TAG, "Vertical min angle set to: " + angle); + return true; + } catch (IOException e) { + LogManager.e(TAG, "Failed to set vertical min angle: " + e.getMessage()); + return false; + } + } + + /** + * 获取当前垂直方向最小角度限制 + * @return 垂直方向最小角度值，读取失败返回-1 + */ + public int getVerticalMinAngle() { + try { + BufferedReader reader = new BufferedReader(new FileReader(VERTICAL_MIN_ANGLE_PATH)); + String line = reader.readLine(); + reader.close(); + return line != null ? Integer.parseInt(line.trim()) : -1; + } catch (IOException | NumberFormatException e) { + LogManager.e(TAG, "Failed to read vertical min angle: " + e.getMessage()); + return -1; + } + } + + public static String bytesToHexString(byte[] bytes) { + if (bytes == null || bytes.length == 0) { + return ""; + } + StringBuilder sb = new StringBuilder(); + for (byte b : bytes) { + sb.append(String.format("%02X ", b & 0xFF)); + } + return sb.toString().trim(); + } + + /** + * 控制垂直电机旋转至指定角度 + * @param direction 方向：1=抬头，0=低头 + * @param dutyCycle 占空比（0-100） + * @param angle 目标角度（度） + * @return 是否控制成功 + */ + public boolean rotateVerticalToAngle(int direction, int dutyCycle, int angle) { + // 确保方向参数正确 + if (direction != VERTICAL_DIRECTION_UP && direction != VERTICAL_DIRECTION_DOWN) { + Log.e(TAG, "Invalid vertical motor direction: " + direction); + return false; + } + + // 限制占空比范围 + int limitedDutyCycle = Math.max(0, Math.min(100, dutyCycle)); + + try { + // 构建控制命令 + String command = direction + "," + limitedDutyCycle + "," + angle; + + // 写入控制文件 + FileWriter writer = new FileWriter(VERTICAL_ROTATE_TO_ANGLE_PATH); + writer.write(command); + writer.flush(); + writer.close(); + + // 更新垂直电机状态 + verticalState = (direction == VERTICAL_DIRECTION_UP) ? MotorState.FORWARD : MotorState.REVERSE; + verticalSpeed = limitedDutyCycle; + + LogManager.d(TAG, "Vertical motor rotate to angle: " + command); + return true; + } catch (IOException e) { + LogManager.e(TAG, "Failed to rotate vertical motor to angle: " + e.getMessage()); + return false; + } + } } \ No newline at end of file