修改抬头点头控制
This commit is contained in:
parent
5cba58a334
commit
383de43c2f
@ -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
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
Loading…
x
Reference in New Issue
Block a user