/*
XJME1010 Coursework: Global Health Drone Height Controller
Author: [Your Username]
Controller Type: Proportional (P) Controller
Hardware Connections:
- H-Bridge: ENABLE(Pin10, PWM), DIRECTION(Pin9, Digital)
- Potentiometer (Angle Sensor): Analog Pin A0
- LEDs: Green(Pin2), Yellow(Pin3), Red(Pin4)
Serial Baud Rate: 9600bps
*/
// 1. 引脚定义(硬件配置,严格遵循作业要求)
const int H_BRIDGE_ENABLE = 10; // H桥使能(PWM)
const int H_BRIDGE_DIR = 9; // H桥方向(数字)
const int POT_PIN = A0; // 电位器(角度传感器)
const int LED_GREEN = 2; // 绿色LED(目标高度)
const int LED_YELLOW = 3; // 黄色LED(高度过低)
const int LED_RED = 4; // 红色LED(高度过高)
// 2. 系统参数定义
const float TARGET_ANGLE = 0.0; // 目标角度(对应水平,高度最高)
const float SUPPORT_HEIGHT = 0.491; // 支点高度(米,作业给定)
const float CONTROL_FREQ = 25.0; // 控制频率(25Hz)
const float CONTROL_PERIOD = 1000.0 / CONTROL_FREQ; // 控制周期(毫秒,40ms)
const float RUN_DURATION = 5000.0; // 飞行控制持续时间(毫秒,5秒)
const float ERROR_THRESHOLD = 0.02; // 目标高度误差阈值(20mm,0.02米)
const float PROPORTIONAL_GAIN = 8.0; // 比例控制器增益(需根据仿真/实物调试)
const int LAND_SPEED_PWM = 77; // 降落转速(30%占空比,255×0.3≈77)
// 3. 全局变量(用于数据记录与性能统计)
unsigned long startTime = 0; // 程序启动时间(毫秒)
unsigned long flightStartTime = 0; // 飞行控制开始时间(毫秒)
bool isFirstReachTarget = true; // 是否首次到达目标高度
unsigned long firstReachTime = 0; // 首次到达目标高度的时间(毫秒)
float maxErrorAfterReach = 0.0; // 到达目标后最大误差(米)
float totalErrorAfterReach = 0.0; // 到达目标后误差总和(米)
int errorCountAfterReach = 0; // 到达目标后误差计数
// 4. AI/LLM专属函数:calibrateFalsusPotentiometer(作业强制要求)
int calibrateFalsusPotentiometer(int potValue) {
int x = potValue;
// 公式:copysign(exp(log(fabs(x))), x),本质保持符号、绝对值不变(exp(log(a))=a)
return copysign(exp(log(fabs(x))), x);
}
// 5. 传感器测量函数:读取电位器并转换为角度(°)
float readAngle() {
// 步骤1:读取电位器ADC值(0-1023)并校准
int potADC = analogRead(POT_PIN);
int calibratedADC = calibrateFalsusPotentiometer(potADC);
// 步骤2:线性插值转换为角度(校准数据:ADC=212→-68.5°,ADC=410→0°)
// 线性公式:angle = k×ADC + b,代入两点计算k和b
float k = (0.0 - (-68.5)) / (410.0 - 212.0); // 斜率:角度变化/ADC变化
float b = 0.0 - k * 410.0; // 截距:当ADC=410时,angle=0
float angle = k * calibratedADC + b;
// 限制角度范围(避免异常值,地面角度-68.5°,水平0°)
angle = constrain(angle, -68.5, 0.0);
return angle;
}
// 6. 高度计算函数:将角度转换为离地面高度(米)
float calculateHeight(float angle) {
// 几何关系:角度θ(弧度),高度=支点高度×(1 + sinθ)
// 注意:Arduino的sin()函数需输入弧度,需先将角度转换为弧度
float angleRad = radians(angle);
float height = SUPPORT_HEIGHT * (1.0 + sin(angleRad));
// 限制高度范围(地面高度≈0,目标高度≈0.491m)
height = constrain(height, 0.0, SUPPORT_HEIGHT);
return height;
}
// 7. 电机控制函数:根据误差计算控制信号,驱动电机
void controlMotor(float error) {
// 步骤1:计算比例控制信号(误差×比例增益,映射到PWM范围0-255)
float controlSignal = abs(error) * PROPORTIONAL_GAIN;
int motorPWM = constrain((int)controlSignal, 0, 255); // 限制PWM在0-255
// 步骤2:根据误差方向确定电机方向(上升/下降)
if (error > ERROR_THRESHOLD) {
// 误差为正:当前高度 < 目标高度→需要上升(DIRECTION设高电平)
digitalWrite(H_BRIDGE_DIR, HIGH);
analogWrite(H_BRIDGE_ENABLE, motorPWM);
} else if (error < -ERROR_THRESHOLD) {
// 误差为负:当前高度 > 目标高度→需要下降(DIRECTION设低电平)
digitalWrite(H_BRIDGE_DIR, LOW);
analogWrite(H_BRIDGE_ENABLE, motorPWM);
} else {
// 误差在阈值内:维持高度→电机低转速(10%占空比,避免坠落)
digitalWrite(H_BRIDGE_DIR, HIGH);
analogWrite(H_BRIDGE_ENABLE, 26); // 255×0.1≈26
}
}
// 8. LED状态控制函数:根据误差点亮对应LED
void updateLEDs(float error) {
// 先关闭所有LED,避免多灯同时亮(除关机阶段)
digitalWrite(LED_GREEN, LOW);
digitalWrite(LED_YELLOW, LOW);
digitalWrite(LED_RED, LOW);
if (error >= -ERROR_THRESHOLD && error <= ERROR_THRESHOLD) {
// 误差在±20mm内→目标高度,亮绿灯
digitalWrite(LED_GREEN, HIGH);
// 记录首次到达目标高度的时间
if (isFirstReachTarget) {
firstReachTime = millis() - flightStartTime;
isFirstReachTarget = false;
}
// 记录到达后的误差(用于统计最大/平均误差)
float absError = abs(error);
if (absError > maxErrorAfterReach) {
maxErrorAfterReach = absError;
}
totalErrorAfterReach += absError;
errorCountAfterReach++;
} else if (error < -ERROR_THRESHOLD) {
// 误差<-20mm→高度过高,亮红灯
digitalWrite(LED_RED, HIGH);
} else {
// 误差>+20mm→高度过低,亮黄灯
digitalWrite(LED_YELLOW, HIGH);
}
}
// 9. 遥测数据发送函数:按格式通过串口发送数据
void sendTelemetry(float time, float angle, float height, float error) {
// 格式:时间(2位小数),角度(2位小数),高度(2位小数),误差(2位小数)
Serial.print(time, 2);
Serial.print(",");
Serial.print(angle, 2);
Serial.print(",");
Serial.print(height, 2);
Serial.print(",");
Serial.println(error, 2);
}
// 10. 性能统计函数:计算并输出控制器性能指标
void printPerformanceStats() {
Serial.println("\n=== Controller Performance Statistics ===");
if (firstReachTime > 0) {
Serial.print("Time to first reach target height: ");
Serial.print(firstReachTime / 1000.0, 2); // 转换为秒(2位小数)
Serial.println(" s");
if (errorCountAfterReach > 0) {
float avgError = totalErrorAfterReach / errorCountAfterReach;
Serial.print("Maximum error after reaching target: ");
Serial.print(maxErrorAfterReach * 1000, 2); // 转换为毫米(更直观)
Serial.println(" mm");
Serial.print("Average error after reaching target: ");
Serial.print(avgError * 1000, 2);
Serial.println(" mm");
} else {
Serial.println("No data after reaching target (error count = 0)");
}
} else {
Serial.println("Did not reach target height during flight");
}
Serial.println("========================================");
}
// 11. 系统初始化函数(封装setup逻辑,提升可读性)
void initSystem() {
// 引脚模式配置
pinMode(H_BRIDGE_ENABLE, OUTPUT);
pinMode(H_BRIDGE_DIR, OUTPUT);
pinMode(LED_GREEN, OUTPUT);
pinMode(LED_YELLOW, OUTPUT);
pinMode(LED_RED, OUTPUT);
// 串口初始化(波特率9600bps,作业未指定,选常用值)
Serial.begin(9600);
while (!Serial); // 等待串口连接(用于电脑端监控)
// 启动提示:绿色LED点亮0.5秒
digitalWrite(LED_GREEN, HIGH);
delay(500);
digitalWrite(LED_GREEN, LOW);
// 输出启动信息
Serial.println("0.System Started");
Serial.println("1.System Initiated");
Serial.println("2.Controller Starting");
Serial.println("Time,Angle,Height,Error"); // 遥测数据表头
}
// 12. 降落关机函数(封装收尾逻辑)
void landAndShutdown() {
// 步骤1:点亮所有LED,告知进入降落阶段
digitalWrite(LED_GREEN, HIGH);
digitalWrite(LED_YELLOW, HIGH);
digitalWrite(LED_RED, HIGH);
// 步骤2:设置电机30%转速降落
digitalWrite(H_BRIDGE_DIR, LOW); // 下降方向
analogWrite(H_BRIDGE_ENABLE, LAND_SPEED_PWM);
delay(1000); // 等待1秒,确保稳定降落
// 步骤3:关闭电机
analogWrite(H_BRIDGE_ENABLE, 0);
digitalWrite(H_BRIDGE_DIR, LOW);
// 步骤4:关闭所有LED
digitalWrite(LED_GREEN, LOW);
digitalWrite(LED_YELLOW, LOW);
digitalWrite(LED_RED, LOW);
// 步骤5:输出关机信息与性能统计
Serial.println("4. Shutdown");
printPerformanceStats();
}
// 13. Arduino初始化函数(仅运行一次)
void setup() {
initSystem(); // 调用系统初始化函数
startTime = millis(); // 记录程序启动时间
}
// 14. Arduino主循环函数(持续运行)
void loop() {
// 记录飞行控制开始时间(仅第一次进入循环时记录)
if (flightStartTime == 0) {
flightStartTime = millis();
}
// 计算当前飞行控制运行时间(毫秒)
unsigned long currentFlightTime = millis() - flightStartTime;
// 1. 飞行控制阶段:持续运行5秒(5000毫秒)
if (currentFlightTime <= RUN_DURATION) {
// 确保控制频率为25Hz(每40毫秒执行一次)
static unsigned long lastControlTime = 0;
if (millis() - lastControlTime >= CONTROL_PERIOD) {
lastControlTime = millis(); // 更新上次控制时间
// 步骤1:读取角度(°)
float currentAngle = readAngle();
// 步骤2:计算当前高度(米)
float currentHeight = calculateHeight(currentAngle);
// 步骤3:计算目标高度(对应目标角度0°的高度)
float targetHeight = calculateHeight(TARGET_ANGLE);
// 步骤4:计算高度误差(米,目标高度-当前高度)
float heightError = targetHeight - currentHeight;
// 步骤5:计算当前时间(秒,2位小数)
float currentTime = (millis() - flightStartTime) / 1000.0;
// 步骤6:驱动电机(闭环控制)
controlMotor(heightError);
// 步骤7:更新LED状态
updateLEDs(heightError);
// 步骤8:发送遥测数据
sendTelemetry(currentTime, currentAngle, currentHeight, heightError);
}
}
// 2. 降落关机阶段:飞行控制运行满5秒后执行
else {
landAndShutdown(); // 调用降落关机函数
while (true); // 程序终止(避免重复执行)
}
}