接收端#include <SPI.h>
#include <LoRa.h>
#include <TinyGPS++.h>
#include <Servo.h>
// 硬件配置
#define LORA_NSS 10
#define LORA_RST 9
#define LORA_DIO0 2
#define LEFT_MOTOR_PWM 5
#define LEFT_MOTOR_DIR_1 6
#define LEFT_MOTOR_DIR_2 7
#define RIGHT_MOTOR_PWM 8
#define RIGHT_MOTOR_DIR_1 A0
#define RIGHT_MOTOR_DIR_2 A1
#define FEED_SERVO_PIN 4
#define FEED_LED 13
#define GPS_LED A5
#define
BATTERY_PIN A3 // 电池电压检测引脚
#define LORA_FREQ 433.0E6
#define LORA_SF 9
#define LORA_BW 125E3
#define LORA_SYNC_WORD 0x12
#define GPS_BAUD 9600
#define WAYPOINT_MAX 20
#define NAV_THRESHOLD 8.0
#define SIGNAL_TIMEOUT 10000
#define MAX_MOTOR_SPEED 255
#define SERVO_FEED_ANGLE 90
#define SERVO_HOME_ANGLE 0
#define FEED_DURATION 1000
#define STATUS_REPORT_INTERVAL
5000
#define GPS_TIMEOUT
5000
#define HOME_SET_RETRY_INTERVAL 1000
#define
BATTERY_CHECK_INTERVAL
5000 // 电池检测间隔
// 全局对象
TinyGPSPlus gps;
Servo feedServo;
// 航点数据结构
struct Waypoint {
double lat;
double lng;
};
Waypoint waypoints[WAYPOINT_MAX];
uint8_t waypointCount = 0;
uint8_t homeIndex = 0;
uint8_t targetIndex = 0;
// 系统状态
enum State { MANUAL, AUTO_RETURN, AUTO_WAYPOINT };
State currentState = MANUAL;
bool feeding = false;
bool homeSet = false;
bool homeSetAcknowledged = false;
bool returningHome = false; // 是否正在返航中
unsigned long lastCommandTime = 0;
unsigned long feedStartTime = 0;
String lastCommand = "";
unsigned long lastFeedCommandTime = 0;
#define COMMAND_DEBOUNCE
500
unsigned long lastStatusReportTime = 0;
unsigned long lastValidGpsTime = 0;
unsigned long lastHomeSetRetryTime = 0;
unsigned long last
BatteryCheckTime = 0;
bool gpsValid = false;
bool gpsLedState = false;
float
batteryVoltage = 0.0;
int
batteryPercentage = 0;
// 电机控制函数
void setLeftMotor(int speed) {
speed = constrain(speed, -MAX_MOTOR_SPEED, MAX_MOTOR_SPEED);
int absSpeed = abs(speed);
if (speed > 0) {
digitalWrite(LEFT_MOTOR_DIR_1, HIGH);
digitalWrite(LEFT_MOTOR_DIR_2, LOW);
} else if (speed < 0) {
digitalWrite(LEFT_MOTOR_DIR_1, LOW);
digitalWrite(LEFT_MOTOR_DIR_2, HIGH);
} else {
digitalWrite(LEFT_MOTOR_DIR_1, LOW);
digitalWrite(LEFT_MOTOR_DIR_2, LOW);
}
analogWrite(LEFT_MOTOR_PWM, absSpeed);
}
void setRightMotor(int speed) {
speed = constrain(speed, -MAX_MOTOR_SPEED, MAX_MOTOR_SPEED);
int absSpeed = abs(speed);
if (speed > 0) {
digitalWrite(RIGHT_MOTOR_DIR_1, HIGH);
digitalWrite(RIGHT_MOTOR_DIR_2, LOW);
} else if (speed < 0) {
digitalWrite(RIGHT_MOTOR_DIR_1, LOW);
digitalWrite(RIGHT_MOTOR_DIR_2, HIGH);
} else {
digitalWrite(RIGHT_MOTOR_DIR_1, LOW);
digitalWrite(RIGHT_MOTOR_DIR_2, LOW);
}
analogWrite(RIGHT_MOTOR_PWM, absSpeed);
}
void stopMotors() {
setLeftMotor(0);
setRightMotor(0);
}
void moveBoat(int forwardSpeed, int turnSpeed) {
int leftSpeed = forwardSpeed + turnSpeed;
int rightSpeed = forwardSpeed - turnSpeed;
leftSpeed = constrain(leftSpeed, -MAX_MOTOR_SPEED, MAX_MOTOR_SPEED);
rightSpeed = constrain(rightSpeed, -MAX_MOTOR_SPEED, MAX_MOTOR_SPEED);
setLeftMotor(leftSpeed);
setRightMotor(rightSpeed);
}
void setFeedServo(int angle) {
feedServo.write(angle);
digitalWrite(FEED_LED, angle > SERVO_HOME_ANGLE ? HIGH : LOW);
}
void startFeeding() {
if (feeding) stopFeeding();
feeding = true;
feedStartTime = millis();
setFeedServo(SERVO_FEED_ANGLE);
Serial.println("FEED_START");
LoRa.beginPacket();
LoRa.print("ACK:FEED_STARTED");
LoRa.endPacket();
}
void stopFeeding() {
if (!feeding) return;
feeding = false;
setFeedServo(SERVO_HOME_ANGLE);
Serial.println("FEED_STOP");
LoRa.beginPacket();
LoRa.print("ACK:FEED_STOPPED");
LoRa.endPacket();
}
void checkFeedingTime() {
if (feeding && (millis() - feedStartTime > FEED_DURATION)) {
stopFeeding();
}
}
// GPS处理函数
void processGPS() {
while (Serial.available() > 0) {
char c = Serial.read();
if (gps.encode(c)) {
if (gps.location.isValid()) {
lastValidGpsTime = millis();
gpsValid = true;
}
}
}
if (millis() - lastValidGpsTime > GPS_TIMEOUT) {
gpsValid = false;
}
}
// 更新GPS状态指示灯
void updateGpsLed() {
static unsigned long lastBlinkTime = 0;
unsigned long currentTime = millis();
if (gpsValid) {
digitalWrite(GPS_LED, HIGH);
gpsLedState = true;
} else {
if (currentTime - lastBlinkTime >
500) {
lastBlinkTime = currentTime;
gpsLedState = !gpsLedState;
digitalWrite(GPS_LED, gpsLedState ? HIGH : LOW);
}
}
}
// 电池电压检测
float read
BatteryVoltage() {
float rawVoltage = analogRead(
BATTERY_PIN);
float voltage = rawVoltage * (5.0 / 1023.0) * 2; // 假设电压分压比为1:1
return voltage;
}
int calculate
BatteryPercentage(float voltage) {
// 简单线性近似计算电池百分比
int percentage = (voltage - 3.7) / (4.2 - 3.7) * 100;
return constrain(percentage, 0, 100);
}
void check
Battery() {
unsigned long currentTime = millis();
if (currentTime - last
BatteryCheckTime >
BATTERY_CHECK_INTERVAL) {
last
BatteryCheckTime = currentTime;
batteryVoltage = read
BatteryVoltage();
batteryPercentage = calculate
BatteryPercentage(
batteryVoltage);
}
}
void setHome() {
if (gps.location.isValid()) {
waypoints[0].lat = gps.location.lat();
waypoints[0].lng = gps.location.lng();
homeIndex = 0;
homeSet = true;
homeSetAcknowledged = false;
if (waypointCount == 0) waypointCount = 1;
Serial.println("HOME_SET");
// 发送确认响应给发射端
LoRa.beginPacket();
LoRa.print("ACK:HOME_SET_SUCCESS");
LoRa.endPacket();
lastHomeSetRetryTime = millis();
} else {
Serial.println("HOME_SET_FAILED");
LoRa.beginPacket();
LoRa.print("ACK:HOME_SET_FAIL_GPS");
LoRa.endPacket();
}
}
// 状态报告函数
void reportStatus() {
unsigned long currentTime = millis();
if (currentTime - lastStatusReportTime >= STATUS_REPORT_INTERVAL) {
lastStatusReportTime = currentTime;
LoRa.beginPacket();
LoRa.print("STATUS:");
LoRa.print("MODE=");
switch(currentState) {
case MANUAL: LoRa.print("MANUAL"); break;
case AUTO_RETURN: LoRa.print("AUTO_RETURN"); break;
case AUTO_WAYPOINT: LoRa.print("AUTO_WAYPOINT"); break;
}
LoRa.print(",HOME=");
LoRa.print(homeSet ? "SET" : "NOT_SET");
LoRa.print(",HOME_ACK=");
LoRa.print(homeSetAcknowledged ? "YES" : "NO");
LoRa.print(",WP=");
LoRa.print(waypointCount);
LoRa.print(",SAT=");
LoRa.print(gps.satellites.value());
LoRa.print(",HDOP=");
LoRa.print(gps.hdop.isValid() ? gps.hdop.hdop() : 0.0);
LoRa.print(",LAT=");
LoRa.print(gps.location.lat(), 6);
LoRa.print(",LNG=");
LoRa.print(gps.location.lng(), 6);
LoRa.print(",GPS_VALID=");
LoRa.print(gpsValid ? "YES" : "NO");
LoRa.print(",
BAT=");
LoRa.print(
batteryPercentage);
LoRa.endPacket();
}
}
// 重试发送HOME_SET直到被确认
void retryHomeSet() {
if (homeSet && !homeSetAcknowledged && millis() - lastHomeSetRetryTime > HOME_SET_RETRY_INTERVAL) {
LoRa.beginPacket();
LoRa.print("ACK:HOME_SET_SUCCESS");
LoRa.endPacket();
lastHomeSetRetryTime = millis();
}
}
void processCommand(String command) {
unsigned long currentTime = millis();
if (command == lastCommand && (currentTime - lastCommandTime < COMMAND_DEBOUNCE)) {
return;
}
lastCommandTime = currentTime;
lastCommand = command;
Serial.print("CMD:");
Serial.println(command);
if (command.startsWith("MANUAL:")) {
currentState = MANUAL;
returningHome = false; // 退出返航状态
int commaIndex = command.indexOf(',');
if (commaIndex != -1) {
int forwardSpeed = command.substring(7, commaIndex).toInt();
int turnSpeed = command.substring(commaIndex + 1).toInt();
forwardSpeed = constrain(forwardSpeed, -255, 255);
turnSpeed = constrain(turnSpeed, -255, 255);
moveBoat(forwardSpeed, turnSpeed);
}
} else if (command == "FEED") {
if (currentTime - lastFeedCommandTime > COMMAND_DEBOUNCE) {
lastFeedCommandTime = currentTime;
startFeeding();
}
} else if (command == "FEED_STOP") {
stopFeeding();
} else if (command == "SET_HOME") {
setHome();
} else if (command == "ADD_WP") {
if (gps.location.isValid()) {
if (waypointCount < WAYPOINT_MAX) {
waypoints[waypointCount].lat = gps.location.lat();
waypoints[waypointCount].lng = gps.location.lng();
waypointCount++;
Serial.print("WP_ADD:");
Serial.println(waypointCount - 1);
LoRa.beginPacket();
LoRa.print("ACK:WAYPOINT_ADDED");
LoRa.endPacket();
} else {
LoRa.beginPacket();
LoRa.print("ACK:ADD_WP_FAILED");
LoRa.endPacket();
}
} else {
LoRa.beginPacket();
LoRa.print("ACK:ADD_WP_FAIL_GPS");
LoRa.endPacket();
}
} else if (command == "GET_GPS") {
LoRa.beginPacket();
if (gps.location.isValid()) {
LoRa.print("GPS_POSITION:");
LoRa.print(gps.location.lat(), 6);
LoRa.print(",");
LoRa.print(gps.location.lng(), 6);
LoRa.print(",");
LoRa.print(gps.satellites.value());
LoRa.print(",");
LoRa.print(gps.hdop.isValid() ? gps.hdop.hdop() : 0.0);
} else {
LoRa.print("ACK:NO_GPS_FIX");
}
LoRa.endPacket();
} else if (command.startsWith("START_WP:")) {
if (gps.location.isValid()) {
int wp = command.substring(9).toInt();
if (wp >= 0 && wp < waypointCount) {
targetIndex = wp;
currentState = AUTO_WAYPOINT;
returningHome = false;
Serial.print("GOTO:");
Serial.println(wp);
LoRa.beginPacket();
LoRa.print("ACK:START_WP_SUCCESS");
LoRa.endPacket();
} else {
LoRa.beginPacket();
LoRa.print("ACK:START_WP_INVALID");
LoRa.endPacket();
}
} else {
LoRa.beginPacket();
LoRa.print("ACK:START_WP_FAIL_GPS");
LoRa.endPacket();
}
} else if (command == "RETURN_HOME") {
if (homeSet && gps.location.isValid()) {
targetIndex = homeIndex;
currentState = AUTO_RETURN;
returningHome = true;
Serial.println("RETURN_HOME");
LoRa.beginPacket();
LoRa.print("ACK:RETURN_HOME_SUCCESS");
LoRa.endPacket();
} else if (!homeSet) {
LoRa.beginPacket();
LoRa.print("ACK:RETURN_HOME_NO_HOME");
LoRa.endPacket();
} else {
LoRa.beginPacket();
LoRa.print("ACK:RETURN_HOME_FAIL_GPS");
LoRa.endPacket();
}
} else if (command == "GET_STATUS") {
LoRa.beginPacket();
LoRa.print("STATUS:");
LoRa.print("MODE=");
switch(currentState) {
case MANUAL: LoRa.print("MANUAL"); break;
case AUTO_RETURN: LoRa.print("AUTO_RETURN"); break;
case AUTO_WAYPOINT: LoRa.print("AUTO_WAYPOINT"); break;
}
LoRa.print(",HOME=");
LoRa.print(homeSet ? "SET" : "NOT_SET");
LoRa.print(",HOME_ACK=");
LoRa.print(homeSetAcknowledged ? "YES" : "NO");
LoRa.print(",WP=");
LoRa.print(waypointCount);
LoRa.print(",SAT=");
LoRa.print(gps.satellites.value());
LoRa.print(",HDOP=");
LoRa.print(gps.hdop.isValid() ? gps.hdop.hdop() : 0.0);
LoRa.print(",LAT=");
LoRa.print(gps.location.lat(), 6);
LoRa.print(",LNG=");
LoRa.print(gps.location.lng(), 6);
LoRa.print(",GPS_VALID=");
LoRa.print(gpsValid ? "YES" : "NO");
LoRa.print(",
BAT=");
LoRa.print(
batteryPercentage);
LoRa.endPacket();
}
}
void setup() {
Serial.begin(GPS_BAUD);
// 初始化引脚
pinMode(GPS_LED, OUTPUT);
digitalWrite(GPS_LED, LOW);
pinMode(
BATTERY_PIN, INPUT);
// 配置GPS模块提高更新率
delay(1000);
// 设置更新率为5Hz (200ms)
byte setRate[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A};
for(int i = 0; i < sizeof(setRate); i++) {
Serial.write(setRate[i]);
}
delay(100);
pinMode(LEFT_MOTOR_DIR_1, OUTPUT);
pinMode(LEFT_MOTOR_DIR_2, OUTPUT);
pinMode(LEFT_MOTOR_PWM, OUTPUT);
pinMode(RIGHT_MOTOR_DIR_1, OUTPUT);
pinMode(RIGHT_MOTOR_DIR_2, OUTPUT);
pinMode(RIGHT_MOTOR_PWM, OUTPUT);
pinMode(FEED_LED, OUTPUT);
feedServo.attach(FEED_SERVO_PIN);
setFeedServo(SERVO_HOME_ANGLE);
pinMode(LORA_RST, OUTPUT);
digitalWrite(LORA_RST, HIGH);
LoRa.setPins(LORA_NSS, LORA_RST, LORA_DIO0);
if (!LoRa.begin(LORA_FREQ)) {
Serial.println("LORA_FAIL");
while (1);
}
LoRa.setSpreadingFactor(LORA_SF);
LoRa.setSignalBandwidth(LORA_BW);
LoRa.setSyncWord(LORA_SYNC_WORD);
Serial.println("LORA_OK");
Serial.println("WAIT_GPS");
// 尝试自动设置Home点
unsigned long startTime = millis();
while (millis() - startTime < 1
5000) {
processGPS();
if (gps.location.isValid() && gps.satellites.value() >= 4) {
setHome();
break;
}
delay(100);
}
if (!homeSet) {
Serial.println("GPS_NOT_READY");
}
Serial.println("SYSTEM_READY");
}
void loop() {
unsigned long currentTime = millis();
processGPS();
updateGpsLed();
check
Battery();
reportStatus();
retryHomeSet();
if (currentState == AUTO_RETURN || currentState == AUTO_WAYPOINT) {
if (gps.location.isValid()) {
if (targetIndex >= waypointCount) {
stopMotors();
currentState = MANUAL;
returningHome = false;
} else {
double currentLat = gps.location.lat();
double currentLng = gps.location.lng();
double distance = TinyGPSPlus::distanceBetween(
currentLat, currentLng,
waypoints[targetIndex].lat, waypoints[targetIndex].lng
);
if (distance < NAV_THRESHOLD) {
stopMotors();
if (currentState == AUTO_RETURN) {
currentState = MANUAL;
returningHome = false;
LoRa.beginPacket();
LoRa.print("ACK:AUTO_RETURN_COMPLETE");
LoRa.endPacket();
} else {
LoRa.beginPacket();
LoRa.print("ACK:AUTO_WAYPOINT_ARRIVED");
LoRa.endPacket();
}
} else {
double bearing = TinyGPSPlus::courseTo(
currentLat, currentLng,
waypoints[targetIndex].lat, waypoints[targetIndex].lng
);
double heading = gps.course.isValid() ? gps.course.deg() : 0;
double angleDiff = bearing - heading;
if (angleDiff > 180) angleDiff -= 360;
if (angleDiff < -180) angleDiff += 360;
int forwardSpeed = MAX_MOTOR_SPEED;
int turnSpeed = 0;
if (abs(angleDiff) > 30) {
turnSpeed = (angleDiff > 0) ? MAX_MOTOR_SPEED : -MAX_MOTOR_SPEED;
} else if (abs(angleDiff) > 10) {
turnSpeed = (angleDiff > 0) ? 200 : -200;
} else if (abs(angleDiff) > 5) {
turnSpeed = (angleDiff > 0) ? 100 : -100;
}
moveBoat(forwardSpeed, turnSpeed);
}
}
} else {
stopMotors();
currentState = MANUAL;
returningHome = false;
LoRa.beginPacket();
LoRa.print("ACK:NAVIGATION_GPS_LOST");
LoRa.endPacket();
}
}
if (currentState == MANUAL && (currentTime - lastCommandTime > SIGNAL_TIMEOUT)) {
stopMotors();
}
checkFeedingTime();
int packetSize = LoRa.parsePacket();
if (packetSize) {
String command = "";
while (LoRa.available()) {
char c = (char)LoRa.read();
if (c >= 32 && c <= 126) {
command += c;
}
}
if (command.length() > 0) {
processCommand(command);
// 检查是否是HOME_SET的确认
if (command == "ACK:HOME_SET_SUCCESS") {
homeSetAcknowledged = true;
Serial.println("HOME_SET_ACKNOWLEDGED");
}
}
}
}
发射端#include <SPI.h>
#include <LoRa.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
// OLED配置
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// OLED状态标志
bool oledInitialized = false;
// PAN3031ZTR4 LoRa模块引脚定义
#define LORA_NSS 10
#define LORA_RST 9
#define LORA_DIO0 2
// 控制引脚
#define JOYSTICK_X A0
#define JOYSTICK_Y A1
#define BUTTON_FEED 4
#define BUTTON_RETURN_HOME 5 // 原模式切换按键，现改为返回安全点
#define BUTTON_B2 6 // 循环选择和出发航点
#define BUTTON_B4 7 // 添加航点
#define
BATTERY_PIN A2 // 电池电压检测引脚
// 系统参数
#define LORA_FREQ 433.0E6 // LoRa频率
#define DEADZONE
50 // 摇杆死区
#define SEND_INTERVAL 100 // 发送间隔(毫秒)
#define NUM_SAMPLES 3 // 摇杆采样次数
#define DEBOUNCE_TIME
50 // 按键防抖时间(毫秒)
#define LONG_PRESS_TIME 1000 // 长按时间(毫秒)
#define SIGNAL_LOST_TIMEOUT
5000 // 信号丢失超时(5秒)
#define DISPLAY_UPDATE_INTERVAL
500 // OLED显示更新间隔(毫秒)
uint8_t waypointCount = 0; // 总航点数
uint8_t selectedWaypoint = 0; // 当前
选中的航点
bool homeSet = false; // 安全点是否已设置
unsigned long lastSendTime = 0;
unsigned long lastReceiveTime = 0; // 最后接收时间
unsigned long lastVoltageCheckTime = 0; // 上次检测电压的时间
unsigned long lastDisplayUpdateTime = 0; // 上次更新显示的时间
bool signalLost = false; // 信号丢失标志
bool displayUpdateNeeded = true; // 需要更新显示标志
bool returningHome = false; // 是否正在返航中
// 摇杆值平滑处理变量
int xSamples[NUM_SAMPLES] = {0};
int ySamples[NUM_SAMPLES] = {0};
// GPS状态变量
int lastSatelliteCount = 0;
float lastHdop = 0;
float lastLat = 0;
float lastLng = 0;
// 电池状态
int
batteryPercentage = 0;
// 按键状态结构体
struct Button {
uint8_t pin : 5; // 引脚号(0-31)
bool lastState : 1;
bool pressed : 1;
bool handled : 1;
bool longPressed : 1; // 长按状态
unsigned long pressStartTime; // 按键按下开始时间
};
// 初始化按键
Button feedButton = {BUTTON_FEED, HIGH, false, false, false, 0};
Button returnHomeButton = {BUTTON_RETURN_HOME, HIGH, false, false, false, 0}; // 返回安全点
Button b2Button = {BUTTON_B2, HIGH, false, false, false, 0}; // 循环选择和出发航点
Button b4Button = {BUTTON_B4, HIGH, false, false, false, 0}; // 添加航点
// 标志位用于防止长按重复触发
bool b2LongPressHandled = false;
bool b4LongPressHandled = false;
// 函数声明
void handleFeedButton();
void handleReturnHomeButton(); // 返回安全点
void handleAddWPButton();
void handleSwitchWPButton(); // 循环选择和出发航点
void handleStartWPButton(); // 出发
选中的航点
void sendCommand(const char* cmd);
void processResponse(String &response);
void updateDisplay();
int readSmoothAnalog(int pin, int samples[]);
void updateButton(Button &btn, unsigned long currentTime);
bool initOLED();
void returnToHome();
void setHome();
float read
BatteryVoltage();
int calculate
BatteryPercentage(float voltage);
// 专用OLED初始化函数
bool initOLED() {
Serial.println(F("Initializing OLED..."));
// 初始化I2C总线
Wire.begin();
Wire.setClock(100000); // 降低I2C速度到100kHz
delay(100);
// 尝试硬件复位（如果复位引脚可用）
if (OLED_RESET > 0) {
pinMode(OLED_RESET, OUTPUT);
digitalWrite(OLED_RESET, LOW);
delay(
50);
digitalWrite(OLED_RESET, HIGH);
delay(1
50);
}
// 尝试初始化OLED
bool oledFound = false;
// 尝试0x3C地址
if (display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
Serial.println(F("OLED found at 0x3C"));
oledFound = true;
}
// 尝试0x3D地址
else if (display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {
Serial.println(F("OLED found at 0x3D"));
oledFound = true;
}
if (!oledFound) {
Serial.println(F("OLED init failed!"));
Serial.println(F("Please check OLED wiring:"));
Serial.println(F("SDA -> A4, SCL -> A5, VCC -> 3.3V/5V, GND -> GND"));
return false;
}
// 初始化成功后的设置
display.clearDisplay();
display.setTextColor(SSD1306_WHITE);
display.setTextSize(1);
display.setCursor(0, 0);
display.println(F("Initializing..."));
display.display();
Serial.println(F("OLED init success"));
return true;
}
void setup() {
Serial.begin(115200);
// 给硬件上电时间
delay(1000); // 延长到1秒确保所有硬件就绪
// 初始化OLED - 使用专用函数
oledInitialized = initOLED();
if (oledInitialized) {
display.clearDisplay();
display.setCursor(0, 20);
display.println(F("Initializing..."));
display.display();
} else {
Serial.println(F("Continuing without OLED display"));
}
// PAN3031ZTR4 LoRa初始化
Serial.println(F("Initializing LoRa..."));
pinMode(LORA_RST, OUTPUT);
digitalWrite(LORA_RST, HIGH);
delay(100);
LoRa.setPins(LORA_NSS, LORA_RST, LORA_DIO0);
if (!LoRa.begin(LORA_FREQ)) {
Serial.println(F("LoRa init failed!"));
if(oledInitialized) {
display.clearDisplay();
display.setCursor(0,0);
display.println(F("LoRa failed!"));
display.display();
}
while (1); // 无限循环
}
Serial.println(F("LoRa init success"));
LoRa.setSpreadingFactor(9);
LoRa.setSignalBandwidth(125E3);
// 按键初始化
Serial.println(F("Initializing buttons..."));
pinMode(BUTTON_FEED, INPUT_PULLUP);
pinMode(BUTTON_RETURN_HOME, INPUT_PULLUP); // 返回安全点
pinMode(BUTTON_B2, INPUT_PULLUP); // 循环选择和出发航点
pinMode(BUTTON_B4, INPUT_PULLUP); // 添加航点
Serial.println(F("Buttons init done"));
// 初始化摇杆采样数组
for(int i = 0; i < NUM_SAMPLES; i++) {
xSamples[i] = analogRead(JOYSTICK_X);
ySamples[i] = analogRead(JOYSTICK_Y);
}
// 设置安全点（航点0）
setHome();
lastReceiveTime = millis(); // 初始化最后接收时间
if (oledInitialized) {
display.clearDisplay();
display.setCursor(0, 20);
display.println(F("System Ready"));
display.display();
delay(
500);
} else {
Serial.println(F("System Ready - OLED not available"));
}
}
// 设置安全点（航点0）
void setHome() {
// 发送设置安全点指令
sendCommand("SET_HOME");
Serial.println(F("Setting home point..."));
// 显示设置中信息
if(oledInitialized) {
display.clearDisplay();
display.setCursor(0, 20);
display.println(F("SETTING HOME..."));
display.display();
}
// 等待设置完成确认（非阻塞方式）
unsigned long startTime = millis();
bool responseReceived = false;
while ((millis() - startTime < 10000) && !responseReceived) { // 延长到10秒超时
int packetSize = LoRa.parsePacket();
if (packetSize) {
String response = "";
while (LoRa.available()) {
response += (char)LoRa.read();
}
Serial.print(F("Received response in setHome: "));
Serial.println(response);
// 处理响应
processResponse(response);
if (homeSet) {
responseReceived = true;
}
}
}
if (!homeSet) {
Serial.println(F("Home setting timed out!"));
if(oledInitialized) {
display.clearDisplay();
display.setCursor(0, 20);
display.println(F("HOME TIMEOUT!"));
display.display();
delay(1000);
}
} else {
Serial.println(F("Home set successfully!"));
if(oledInitialized) {
display.clearDisplay();
display.setCursor(0, 20);
display.println(F("HOME SET SUCCESSFULLY!"));
display.display();
delay(
500);
}
}
displayUpdateNeeded = true; // 标记需要更新显示
}
// 平滑摇杆值读取函数
int readSmoothAnalog(int pin, int samples[]) {
// 移出旧值
for(int i = 0; i < NUM_SAMPLES - 1; i++) {
samples[i] = samples[i+1];
}
// 添加新值
samples[NUM_SAMPLES - 1] = analogRead(pin);
// 计算平均值
long sum = 0;
for(int i = 0; i < NUM_SAMPLES; i++) {
sum += samples[i];
}
return sum / NUM_SAMPLES;
}
// 改进的按键检测函数 - 添加长按检测
void updateButton(Button &btn, unsigned long currentTime) {
bool currentState = digitalRead(btn.pin);
// 检测按键按下（下降沿）
if (currentState == LOW && btn.lastState == HIGH) {
btn.pressed = true;
btn.handled = false;
btn.longPressed = false;
btn.pressStartTime = currentTime;
}
// 检测按键释放（上升沿）
if (currentState == HIGH && btn.lastState == LOW) {
btn.pressed = false;
// 如果是短按且未处理
if (!btn.longPressed && !btn.handled) {
btn.handled = true; // 标记为已处理
}
}
// 检测长按
if (btn.pressed && !btn.longPressed && (currentTime - btn.pressStartTime > LONG_PRESS_TIME)) {
btn.longPressed = true;
}
btn.lastState = currentState;
}
// 信号丢失处理 - 返回安全点
void returnToHome() {
if (returningHome) {
// 已经在返航中，不再重复发送
命令
return;
}
Serial.println(F("Signal lost! Return home"));
if(oledInitialized) {
display.clearDisplay();
display.setCursor(0, 20);
display.println(F("SIGNAL LOST"));
display.setCursor(0, 40);
display.println(F("RETURNING HOME"));
display.display();
}
// 发送返回安全点指令
sendCommand("RETURN_HOME");
returningHome = true; // 标记正在返航中
// 重置计时器
lastReceiveTime = millis();
}
void loop() {
unsigned long currentTime = millis();
// 更新按键状态
updateButton(feedButton, currentTime);
updateButton(returnHomeButton, currentTime); // 返回安全点
updateButton(b2Button, currentTime); // 循环选择和出发航点
updateButton(b4Button, currentTime); // 添加航点
// 处理按键事件
if (feedButton.handled) {
handleFeedButton();
feedButton.handled = false;
displayUpdateNeeded = true;
}
if (returnHomeButton.handled) {
handleReturnHomeButton(); // 返回安全点
returnHomeButton.handled = false;
displayUpdateNeeded = true;
}
if (b2Button.handled) {
handleSwitchWPButton(); // 循环选择和出发航点
b2Button.handled = false;
displayUpdateNeeded = true;
}
if (b2Button.longPressed && !b2LongPressHandled) {
handleStartWPButton(); // 出发
选中的航点
b2LongPressHandled = true; // 设置标志位
displayUpdateNeeded = true;
} else if (!b2Button.longPressed) {
b2LongPressHandled = false; // 清除标志位
}
if (b4Button.handled) {
handleAddWPButton(); // 添加航点
b4Button.handled = false;
displayUpdateNeeded = true;
}
// 发送控制指令
if (currentTime - lastSendTime > SEND_INTERVAL) {
// 使用平滑后的摇杆值
int xValue = readSmoothAnalog(JOYSTICK_X, xSamples);
int yValue = readSmoothAnalog(JOYSTICK_Y, ySamples);
// 计算电机速度
int leftSpeed = 0;
int rightSpeed = 0;
int xDiff = xValue - 512;
int yDiff = yValue - 512;
if (abs(xDiff) > DEADZONE || abs(yDiff) > DEADZONE) {
// 简化计算节省内存
float x = constrain(xDiff / 512.0, -1.0, 1.0);
float y = constrain(yDiff / 512.0, -1.0, 1.0);
leftSpeed = constrain(255 * (y + x), -255, 255);
rightSpeed = constrain(255 * (y - x), -255, 255);
}
// 使用字符数组代替String节省内存
char manualCmd[20];
snprintf(manualCmd, sizeof(manualCmd), "MANUAL:
%d,
%d", leftSpeed, rightSpeed);
sendCommand(manualCmd);
lastSendTime = currentTime;
}
// 处理接收到的LoRa数据
int packetSize = LoRa.parsePacket();
if (packetSize) {
String response = "";
while (LoRa.available()) {
response += (char)LoRa.read();
}
// 更新最后接收时间
lastReceiveTime = currentTime;
// 处理响应
processResponse(response);
}
// 检查信号丢失
if (homeSet && (currentTime - lastReceiveTime > SIGNAL_LOST_TIMEOUT)) {
signalLost = true;
} else {
signalLost = false;
}
// 在信号丢失状态下触发返航
if (signalLost) {
returnToHome();
}
// 定期检测电压
if (currentTime - lastVoltageCheckTime >
5000) { // 每5秒检测一次电压
lastVoltageCheckTime = currentTime;
float
batteryVoltage = read
BatteryVoltage();
batteryPercentage = calculate
BatteryPercentage(
batteryVoltage);
Serial.print(F("
Battery Voltage: "));
Serial.print(
batteryVoltage);
Serial.print(F(" V,
Battery Percentage: "));
Serial.print(
batteryPercentage);
Serial.println(F("
%"));
displayUpdateNeeded = true;
}
// 更新显示
if (displayUpdateNeeded || (currentTime - lastDisplayUpdateTime > DISPLAY_UPDATE_INTERVAL)) {
lastDisplayUpdateTime = currentTime;
updateDisplay();
displayUpdateNeeded = false;
}
}
// 按键处理函数
void handleFeedButton() {
sendCommand("FEED");
Serial.println(F("Feed pressed"));
}
void handleReturnHomeButton() {
// 返回安全点逻辑
returnToHome();
Serial.println(F("Returning Home"));
displayUpdateNeeded = true;
}
void handleAddWPButton() {
// 添加航点逻辑
sendCommand("ADD_WP");
Serial.println(F("Adding waypoint"));
waypointCount++;
displayUpdateNeeded = true;
}
void handleSwitchWPButton() {
// 循环选择航点逻辑
selectedWaypoint = (selectedWaypoint + 1)
% waypointCount;
Serial.print(F("Selected Waypoint: "));
Serial.println(selectedWaypoint);
displayUpdateNeeded = true;
}
void handleStartWPButton() {
// 出发
选中的航点逻辑
char wpCmd[20];
snprintf(wpCmd, sizeof(wpCmd), "START_WP:
%d", selectedWaypoint);
sendCommand(wpCmd);
Serial.print(F("Starting Waypoint: "));
Serial.println(selectedWaypoint);
displayUpdateNeeded = true;
}
void sendCommand(const char* cmd) {
LoRa.beginPacket();
LoRa.print(cmd);
LoRa.endPacket();
}
void processResponse(String &response) {
if (response.startsWith("HOME_SET")) {
homeSet = true;
returningHome = false; // 重置返航状态
} else if (response.startsWith("WAYPOINT_ADDED")) {
// 处理新增航点响应
} else if (response.startsWith("GPS_POSITION")) {
// 处理GPS位置响应
} else if (response.startsWith("ACK:AUTO_RETURN_COMPLETE") ||
response.startsWith("ACK:NAVIGATION_GPS_LOST")) {
returningHome = false; // 返航完成或失败，重置状态
}
}
void updateDisplay() {
if(!oledInitialized) {
return;
}
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.print(F("Mode: MANUAL"));
display.setCursor(0, 10);
display.print(F("
Bat: "));
display.print(
batteryPercentage);
display.println(F("
%"));
display.setCursor(0, 20);
display.print(F("WPs: "));
display.print(waypointCount);
display.print(F(", Sel WP: "));
display.print(selectedWaypoint);
display.setCursor(0, 30);
if(signalLost) {
display.println(F("Signal Lost! Returning Home"));
} else if (returningHome) {
display.println(F("Returning Home..."));
} else {
display.println(F("Signal OK"));
}
display.setCursor(0, 40);
display.print(F("Home: "));
display.print(homeSet ? F("SET") : F("NOT SET"));
display.display();
}
float read
BatteryVoltage() {
float rawVoltage = analogRead(
BATTERY_PIN);
float voltage = rawVoltage * (5.0 / 1023.0) * 2; // Assuming a voltage divider of 1:1
return voltage;
}
int calculate
BatteryPercentage(float voltage) {
// Simple linear approximation for demonstration purposes
// Replace with actual calibration data as needed
int percentage = (voltage - 3.7) / (4.2 - 3.7) * 100;
return constrain(percentage, 0, 100);
}
上传后串口信息发射端Received response in setHome:
Received response in setHome:
Received response in setHome:
Received response in setHome:
Received response in setHome:
Received response in setHome:
Received response in setHome:
Home setting timed out!
接收端CMD:MANUAL:0,0
CMD:MANUAL:0,0
CMD:MANUAL:0,0
CMD:MODC
CMD:SET_HOME
HOME_SET
CMD:MANUAL:0,0，，安全点设置失败，解决安全点设置失败问题，只修改接收端代码
本文介绍了一个使用批处理命令实现从文本文件中按一定概率筛选并输出行到另一个文件的方法。通过设置随机数，可以控制筛选的比例。
扫一扫
7722
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
