修改抬头点头控制

This commit is contained in:
peng 2025-08-18 15:26:44 +08:00
parent 5cba58a334
commit 383de43c2f
3 changed files with 720 additions and 311 deletions

View File

@ -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

View File

@ -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

View File

@ -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;
}
}
}