程序
#include <Wire.h>
#include <MPU6050_tockn.h>
#include <WiFi.h>
#include <WebServer.h>
MPU6050 mpu6050(Wire);
// 优化后的PID参数
float kp = 16.0;
float ki = 1.65;
float kd = 3.2;
float ki_limit = 25.0;
// 控制参数
float target_angle = 0.0;
const int motor_max = 255;
const float max_angle = 40.0;
const float min_angle = -40.0;
// PID计算变量
float error = 0, last_error = 0;
float integral = 0, derivative = 0;
float output = 0;
// 输出平滑滤波
float filtered_output = 0;
const float output_filter_alpha = 0.7;
// 输出变化率限制
float last_output = 0;
const float max_output_change = 15.0;
// 控制模式
enum ControlMode { MANUAL, AUTOMATIC };
ControlMode control_mode = AUTOMATIC;
// MPU变量 - 使用MPU6050_tockn库
float angleX = 0;
float gyroX = 0;
// 电机引脚定义
const int motor1_pwm = 16;
const int motor1_dir = 17;
const int motor2_pwm = 5;
const int motor2_dir = 18;
// I2C引脚定义
const int i2c_sda = 21;
const int i2c_scl = 22;
bool mpu_connected = false;
// Web服务器
WebServer server(80);
// WiFi配置
const char* ssid = "PID_Balance_Car";
const char* password = "12345678";
// 角度校准相关变量
float angle_offset = 0.0;
bool calibrating = false;
int calibration_step = 0;
float angle_upright = 0.0;
float angle_forward = 0.0;
float angle_backward = 0.0;
// 死区调整
int deadzone_left = 30;
int deadzone_right = 30;
// 左右轮补偿系数
float motor_compensation_left = 1.0;
float motor_compensation_right = 1.0;
// 运动控制参数
int move_speed = 0;
int turn_speed = 0;
bool movement_active = false;
unsigned long last_movement_time = 0;
const unsigned long movement_timeout = 2000;
const int speed_increment = 10;
// 网页界面HTML
const char* html_content = R"rawliteral(
<!DOCTYPE HTML>
<html>
<head>
<title>自平衡小车PID调试</title>
<meta content="width=device-width, initial-scale=1">
<style>
body {
font-family: Arial;
text-align: center;
margin: 0 auto;
padding: 20px;
background: linear-gradient(135deg, #667eea 0%, #764ba2 100%);
color: white;
}
.container {
max-width: 800px;
margin: 0 auto;
background: rgba(255,255,255,0.1);
padding: 20px;
border-radius: 15px;
backdrop-filter: blur(10px);
}
.status-box {
background: rgba(0,0,0,0.3);
padding: 15px;
margin: 10px 0;
border-radius: 10px;
border: 1px solid rgba(255,255,255,0.2);
}
.pid-section {
display: flex;
justify-content: space-between;
flex-wrap: wrap;
margin: 20px 0;
}
.pid-control {
background: rgba(255,255,255,0.15);
padding: 15px;
border-radius: 10px;
margin: 10px;
flex: 1;
min-width: 200px;
}
.button-group {
display: grid;
grid-template-columns: 1fr 1fr;
gap: 10px;
margin: 10px 0;
}
button {
padding: 12px;
border: none;
border-radius: 8px;
background: #4CAF50;
color: white;
font-size: 16px;
cursor: pointer;
transition: all 0.3s;
}
button:hover {
background: #45a049;
transform: translateY(-2px);
}
.kp-btn { background: #FF6B6B; }
.ki-btn { background: #4ECDC4; }
.kd-btn { background: #45B7D1; }
.mode-btn { background: #FFA500; }
.stop-btn { background: #FF4757; }
.calibrate-btn { background: #9C27B0; }
.deadzone-btn { background: #FF9800; }
.movement-btn { background: #2196F3; }
.cal-step-btn { background: #673AB7; }
.compensation-btn { background: #795548; }
.value {
font-size: 24px;
font-weight: bold;
margin: 10px 0;
color: #FFD700;
}
h1 {
text-shadow: 2px 2px 4px rgba(0,0,0,0.3);
}
.angle-indicator {
width: 200px;
height: 20px;
background: rgba(255,255,255,0.2);
border-radius: 10px;
margin: 10px auto;
position: relative;
overflow: hidden;
}
.angle-bar {
height: 100%;
background: linear-gradient(90deg, #FF6B6B, #4ECDC4);
width: 50%;
transition: all 0.3s;
}
.debug-info {
font-size: 12px;
color: #ccc;
margin-top: 5px;
}
.movement-control {
display: grid;
grid-template-columns: 1fr 1fr 1fr;
gap: 10px;
margin: 10px 0;
}
.movement-row {
grid-column: 1 / -1;
display: grid;
grid-template-columns: 1fr 1fr 1fr;
gap: 10px;
}
.calibration-info {
background: rgba(255,255,0,0.2);
padding: 10px;
border-radius: 8px;
margin: 10px 0;
}
</style>
</head>
<body>
<divhljs-string">">
<h1>自平衡小车PID调试</h1>
<divhljs-string">">
<h3>实时状态</h3>
<div>当前角度: <span id="angle"hljs-string">">0.0</span>°</div>
<div>目标角度: <span id="target"hljs-string">">0.0</span>°</div>
<div>PID输出: <span id="output"hljs-string">">0</span></div>
<div>滤波输出: <span id="filtered-output">0</span></div>
<div>控制模式: <span id="mode"hljs-string">">自动</span></div>
<div>左轮PWM: <span id="motorLeftPwm">0</span> | 右轮PWM: <span id="motorRightPwm">0</span></div>
<div>移动速度: <span id="move-speed">0</span> | 转向速度: <span id="turn-speed">0</span></div>
<divhljs-string">">
<divhljs-string">" id="angle-bar"></div>
</div>
<divhljs-string">" id="debugInfo">角度计算正常</div>
</div>
<divhljs-string">" id="calibrationStatus"hljs-keyword">none;">
<h3>角度校准状态</h3>
<div id="calibrationStepInfo">校准步骤信息</div>
<div>直立角度: <span id="angle-upright">0.0</span>°</div>
<div>前倾角度: <span id="angle-forward">0.0</span>°</div>
<div>后倾角度: <span id="angle-backward">0.0</span>°</div>
<div>角度偏移: <span id="angle-offset-display">0.0</span>°</div>
</div>
<divhljs-string">">
<divhljs-string">">
<h3>比例系数 Kp</h3>
<divhljs-string">" id="kp-value">8.0</div>
<divhljs-string">">
<buttonhljs-string">" onclick="adjustPID('kp', 0.1)">Kp +0.1</button>
<buttonhljs-string">" onclick="adjustPID('kp', -0.1)">Kp -0.1</button>
</div>
</div>
<divhljs-string">">
<h3>积分系数 Ki</h3>
<divhljs-string">" id="ki-value">0.3</div>
<divhljs-string">">
<buttonhljs-string">" onclick="adjustPID('ki', 0.05)">Ki +0.05</button>
<buttonhljs-string">" onclick="adjustPID('ki', -0.05)">Ki -0.05</button>
</div>
</div>
<divhljs-string">">
<h3>微分系数 Kd</h3>
<divhljs-string">" id="kd-value">1.2</div>
<divhljs-string">">
<buttonhljs-string">" onclick="adjustPID('kd', 0.1)">Kd +0.1</button>
<buttonhljs-string">" onclick="adjustPID('kd', -0.1)">Kd -0.1</button>
</div>
</div>
</div>
<divhljs-string">">
<divhljs-string">">
<h3>左轮补偿系数</h3>
<divhljs-string">" id="compensation-left-value">1.00</div>
<divhljs-string">">
<buttonhljs-string">" onclick="adjustCompensation('left', 0.05)">左补偿 +0.05</button>
<buttonhljs-string">" onclick="adjustCompensation('left', -0.05)">左补偿 -0.05</button>
</div>
</div>
<divhljs-string">">
<h3>右轮补偿系数</h3>
<divhljs-string">" id="compensation-right-value">1.00</div>
<divhljs-string">">
<buttonhljs-string">" onclick="adjustCompensation('right', 0.05)">右补偿 +0.05</button>
<buttonhljs-string">" onclick="adjustCompensation('right', -0.05)">右补偿 -0.05</button>
</div>
</div>
<divhljs-string">">
<h3>死区调整</h3>
<div>左轮死区: <span id="deadzone-left-value">30</span></div>
<div>右轮死区: <span id="deadzone-right-value">30</span></div>
<divhljs-string">">
<buttonhljs-string">" onclick="adjustDeadzone('left', 1)">左死区 +1</button>
<buttonhljs-string">" onclick="adjustDeadzone('left', -1)">左死区 -1</button>
<buttonhljs-string">" onclick="adjustDeadzone('right', 1)">右死区 +1</button>
<buttonhljs-string">" onclick="adjustDeadzone('right', -1)">右死区 -1</button>
</div>
</div>
</div>
<divhljs-string">">
<divhljs-string">">
<h3>运动控制</h3>
<div>移动速度: <span id="move-speed-display">0</span></div>
<div>转向速度: <span id="turn-speed-display">0</span></div>
<divhljs-string">">
<divhljs-string">">
<buttonhljs-string">" onclick="movementControl('forward')">前进</button>
<buttonhljs-string">" onclick="movementControl('backward')">后退</button>
</div>
<buttonhljs-string">" onclick="movementControl('left')">左转</button>
<buttonhljs-string">" onclick="movementControl('right')">右转</button>
<buttonhljs-string">" onclick="movementControl('stop')">停止运动</button>
</div>
</div>
<divhljs-string">">
<h3>角度校准</h3>
<divhljs-string">" id="calibrationInstructions">
点击开始校准，按照步骤操作
</div>
<divhljs-string">">
<buttonhljs-string">" onclick="calibrationStep('start')">开始校准</button>
<buttonhljs-string">" onclick="calibrationStep('upright')">直立确认</button>
<buttonhljs-string">" onclick="calibrationStep('forward')">前倾确认</button>
<buttonhljs-string">" onclick="calibrationStep('backward')">后倾确认</button>
<buttonhljs-string">" onclick="calibrationStep('complete')">完成校准</button>
<buttonhljs-string">" onclick="calibrationStep('cancel')">撤销校准</button>
</div>
</div>
</div>
<divhljs-string">"hljs-string">">
<buttonhljs-string">" onclick="setMode('auto')">自动模式</button>
<buttonhljs-string">" onclick="setMode('manual')">手动模式</button>
<buttonhljs-string">" onclick="simpleCalibrate()">快速校准</button>
<buttonhljs-string">" onclick="stopMotors()">紧急停止</button>
</div>
<divhljs-string">">
<h3>当前参数</h3>
<div>PID: Kp=<span id="current-kp">8.0</span> | Ki=<span id="current-ki">0.3</span> | Kd=<span id="current-kd">1.2</span></div>
<div>补偿: 左轮=<span id="current-compensation-left">1.00</span> | 右轮=<span id="current-compensation-right">1.00</span></div>
<div>死区: 左轮=<span id="current-deadzone-left">30</span> | 右轮=<span id="current-deadzone-right">30</span></div>
<div>角度偏移: <span id="angle-offset">0.0</span>°</div>
</div>
</div>
<script>
function adjustPID(param, value) {
fetch(`/adjust?param=${param}&value=${value}`)
.then(response => response.text())
.then(data => {
updateDisplay();
});
}
function setMode(mode) {
fetch(`/mode?type=${mode}`)
.then(response => response.text())
.then(data => {
updateDisplay();
});
}
function simpleCalibrate() {
fetch('/simple_calibrate')
.then(response => response.text())
.then(data => {
alert('快速校准完成！当前角度已设为0度');
updateDisplay();
});
}
function stopMotors() {
fetch('/stop')
.then(response => response.text())
.then(data => {
updateDisplay();
});
}
function adjustDeadzone(motor, value) {
fetch(`/deadzone?motor=${motor}&value=${value}`)
.then(response => response.text())
.then(data => {
updateDisplay();
});
}
function adjustCompensation(motor, value) {
fetch(`/compensation?motor=${motor}&value=${value}`)
.then(response => response.text())
.then(data => {
updateDisplay();
});
}
function movementControl(action) {
fetch(`/movement?action=${action}`)
.then(response => response.text())
.then(data => {
updateDisplay();
});
}
function calibrationStep(step) {
fetch(`/calibration?step=${step}`)
.then(response => response.text())
.then(data => {
updateDisplay();
});
}
function updateDisplay() {
fetch('/status')
.then(response => response.json())
.then(data => {
document.getElementById('angle').textContent = data.angle.toFixed(1);
document.getElementById('target').textContent = data.target_angle.toFixed(1);
document.getElementById('output').textContent = data.output.toFixed(1);
document.getElementById('filtered-output').textContent = data.filtered_output.toFixed(1);
document.getElementById('mode').textContent = data.mode;
document.getElementById('motorLeftPwm').textContent = data.motor_left_pwm;
document.getElementById('motorRightPwm').textContent = data.motor_right_pwm;
document.getElementById('move-speed').textContent = data.move_speed;
document.getElementById('turn-speed').textContent = data.turn_speed;
document.getElementById('move-speed-display').textContent = data.move_speed;
document.getElementById('turn-speed-display').textContent = data.turn_speed;
document.getElementById('kp-value').textContent = data.kp.toFixed(2);
document.getElementById('ki-value').textContent = data.ki.toFixed(3);
document.getElementById('kd-value').textContent = data.kd.toFixed(2);
document.getElementById('current-kp').textContent = data.kp.toFixed(2);
document.getElementById('current-ki').textContent = data.ki.toFixed(3);
document.getElementById('current-kd').textContent = data.kd.toFixed(2);
document.getElementById('compensation-left-value').textContent = data.compensation_left.toFixed(2);
document.getElementById('compensation-right-value').textContent = data.compensation_right.toFixed(2);
document.getElementById('current-compensation-left').textContent = data.compensation_left.toFixed(2);
document.getElementById('current-compensation-right').textContent = data.compensation_right.toFixed(2);
document.getElementById('deadzone-left-value').textContent = data.deadzone_left;
document.getElementById('deadzone-right-value').textContent = data.deadzone_right;
document.getElementById('current-deadzone-left').textContent = data.deadzone_left;
document.getElementById('current-deadzone-right').textContent = data.deadzone_right;
document.getElementById('angle-offset').textContent = data.angle_offset.toFixed(1);
document.getElementById('debugInfo').textContent = data.debug_info;
document.getElementById('calibrationStatus').style.display = data.calibrating ? 'block' : 'none';
document.getElementById('calibrationStepInfo').textContent = data.calibration_step_info;
document.getElementById('angle-upright').textContent = data.angle_upright.toFixed(1);
document.getElementById('angle-forward').textContent = data.angle_forward.toFixed(1);
document.getElementById('angle-backward').textContent = data.angle_backward.toFixed(1);
document.getElementById('angle-offset-display').textContent = data.angle_offset.toFixed(1);
document.getElementById('calibrationInstructions').textContent = data.calibration_instructions;
let anglePercent = ((data.angle + 40) / 80) * 100;
anglePercent = Math.max(0, Math.min(100, anglePercent));
document.getElementById('angle-bar').style.width = anglePercent + '%';
});
}
setInterval(updateDisplay, 300);
updateDisplay();
</script>
</body>
</html>
)rawliteral";
// 函数声明
void initializeMPU6050();
void automaticControl();
void readMPU6050();
void stopMotors();
void setMotorSpeed(int left_pwm, int right_pwm, bool left_forward, bool right_forward);
void displayStatus();
void calculatePID();
void setupWebServer();
void startCalibration();
void processCalibrationStep(int step);
void setup() {
Serial.begin(115200);
delay(1000);
Serial.println("PID自平衡小车控制程序 - 使用MPU6050_tockn库");
Serial.println("MPU6050安装方向：X轴与车轮轴向平行，X箭头指向右轮");
// 初始化I2C
Wire.begin(i2c_sda, i2c_scl);
delay(100);
// 初始化MPU6050
initializeMPU6050();
// 设置电机引脚
pinMode(motor1_pwm, OUTPUT);
pinMode(motor1_dir, OUTPUT);
pinMode(motor2_pwm, OUTPUT);
pinMode(motor2_dir, OUTPUT);
// 初始停止电机
stopMotors();
// 设置WiFi为AP模式
WiFi.softAP(ssid, password);
IPAddress IP = WiFi.softAPIP();
Serial.print("AP IP address: ");
Serial.println(IP);
// 设置Web服务器
setupWebServer();
Serial.println("HTTP服务器已启动");
Serial.println("请连接WiFi: " + String(ssid));
Serial.println("密码: " + String(password));
Serial.println("然后在浏览器中访问: http://" + IP.toString());
Serial.println("使用MPU6050_tockn库，角度检测更准确");
Serial.println("重大提示：请先进行角度校准！点击'开始校准'按钮按照步骤操作。");
}
void initializeMPU6050() {
mpu6050.begin();
mpu_connected = true;
// 计算陀螺仪偏移（需要保持MPU6050静止）
Serial.println("计算陀螺仪偏移，请保持MPU6050静止...");
mpu6050.calcGyroOffsets(true);
Serial.println("陀螺仪偏移计算完成");
}
void setupWebServer() {
// 主页 - 返回HTML页面
server.on("/", []() {
server.send(200, "text/html", html_content);
});
// PID参数调整
server.on("/adjust", []() {
if (server.hasArg("param") && server.hasArg("value")) {
String param = server.arg("param");
float value = server.arg("value").toFloat();
if (param == "kp") {
kp += value;
kp = constrain(kp, 0.1, 20.0);
Serial.println("调整Kp，新值: " + String(kp, 2));
} else if (param == "ki") {
ki += value;
ki = constrain(ki, 0.01, 2.0);
Serial.println("调整Ki，新值: " + String(ki, 3));
} else if (param == "kd") {
kd += value;
kd = constrain(kd, 0.01, 5.0);
Serial.println("调整Kd，新值: " + String(kd, 2));
}
server.send(200, "text/plain", "OK");
} else {
server.send(400, "text/plain", "参数错误");
}
});
// 模式切换
server.on("/mode", []() {
if (server.hasArg("type")) {
String mode = server.arg("type");
if (mode == "auto") {
control_mode = AUTOMATIC;
integral = 0;
movement_active = false;
move_speed = 0;
turn_speed = 0;
filtered_output = 0;
last_output = 0;
Serial.println("切换到自动平衡模式");
} else if (mode == "manual") {
control_mode = MANUAL;
stopMotors();
integral = 0;
movement_active = false;
move_speed = 0;
turn_speed = 0;
filtered_output = 0;
last_output = 0;
Serial.println("切换到手动控制模式");
}
server.send(200, "text/plain", "OK");
} else {
server.send(400, "text/plain", "参数错误");
}
});
// 快速角度校准
server.on("/simple_calibrate", []() {
angle_offset = angleX;
Serial.println("快速角度校准完成，偏移量: " + String(angle_offset, 2));
server.send(200, "text/plain", "OK");
});
// 紧急停止
server.on("/stop", []() {
stopMotors();
integral = 0;
movement_active = false;
move_speed = 0;
turn_speed = 0;
filtered_output = 0;
last_output = 0;
Serial.println("紧急停止");
server.send(200, "text/plain", "OK");
});
// 死区调整
server.on("/deadzone", []() {
if (server.hasArg("motor") && server.hasArg("value")) {
String motor = server.arg("motor");
int value = server.arg("value").toInt();
if (motor == "left") {
deadzone_left += value;
deadzone_left = constrain(deadzone_left, 0, 100);
Serial.println("左轮死区: " + String(deadzone_left));
} else if (motor == "right") {
deadzone_right += value;
deadzone_right = constrain(deadzone_right, 0, 100);
Serial.println("右轮死区: " + String(deadzone_right));
}
server.send(200, "text/plain", "OK");
} else {
server.send(400, "text/plain", "参数错误");
}
});
// 补偿系数调整
server.on("/compensation", []() {
if (server.hasArg("motor") && server.hasArg("value")) {
String motor = server.arg("motor");
float value = server.arg("value").toFloat();
if (motor == "left") {
motor_compensation_left += value;
motor_compensation_left = constrain(motor_compensation_left, 0.5, 1.5);
Serial.println("左轮补偿系数: " + String(motor_compensation_left, 2));
} else if (motor == "right") {
motor_compensation_right += value;
motor_compensation_right = constrain(motor_compensation_right, 0.5, 1.5);
Serial.println("右轮补偿系数: " + String(motor_compensation_right, 2));
}
server.send(200, "text/plain", "OK");
} else {
server.send(400, "text/plain", "参数错误");
}
});
// 运动控制
server.on("/movement", []() {
if (server.hasArg("action")) {
String action = server.arg("action");
movement_active = true;
last_movement_time = millis();
if (action == "forward") {
move_speed = constrain(move_speed + speed_increment, -150, 150);
turn_speed = 0;
Serial.println("前进，移动速度: " + String(move_speed));
} else if (action == "backward") {
move_speed = constrain(move_speed - speed_increment, -150, 150);
turn_speed = 0;
Serial.println("后退，移动速度: " + String(move_speed));
} else if (action == "left") {
turn_speed = constrain(turn_speed - speed_increment, -100, 100);
Serial.println("左转，转向速度: " + String(turn_speed));
} else if (action == "right") {
turn_speed = constrain(turn_speed + speed_increment, -100, 100);
Serial.println("右转，转向速度: " + String(turn_speed));
} else if (action == "stop") {
move_speed = 0;
turn_speed = 0;
movement_active = false;
Serial.println("停止运动控制");
}
server.send(200, "text/plain", "OK");
} else {
server.send(400, "text/plain", "参数错误");
}
});
// 角度校准步骤
server.on("/calibration", []() {
if (server.hasArg("step")) {
String step = server.arg("step");
if (step == "start") {
startCalibration();
server.send(200, "text/plain", "开始校准流程");
} else if (step == "upright") {
processCalibrationStep(1);
server.send(200, "text/plain", "直立角度已记录");
} else if (step == "forward") {
processCalibrationStep(2);
server.send(200, "text/plain", "前倾角度已记录");
} else if (step == "backward") {
processCalibrationStep(3);
server.send(200, "text/plain", "后倾角度已记录");
} else if (step == "complete") {
processCalibrationStep(4);
server.send(200, "text/plain", "校准完成");
} else if (step == "cancel") {
calibration_step = 0;
calibrating = false;
server.send(200, "text/plain", "校准已撤销");
}
} else {
server.send(400, "text/plain", "参数错误");
}
});
// 获取状态数据 (JSON格式)
server.on("/status", []() {
// 计算实际使用的电机PWM（包含补偿系数）
int motor_left_pwm = 0, motor_right_pwm = 0;
if (control_mode == AUTOMATIC && movement_active) {
int balance_output = (int)filtered_output;
int speed_output = move_speed;
int turn_output = turn_speed;
// 应用补偿系数并限制在有效范围内
int left_pwm = constrain((balance_output + speed_output - turn_output) * motor_compensation_left, -motor_max, motor_max);
int right_pwm = constrain((balance_output + speed_output + turn_output) * motor_compensation_right, -motor_max, motor_max);
motor_left_pwm = abs(left_pwm);
motor_right_pwm = abs(right_pwm);
// 应用死区
if (motor_left_pwm < deadzone_left) motor_left_pwm = 0;
if (motor_right_pwm < deadzone_right) motor_right_pwm = 0;
} else if (control_mode == AUTOMATIC) {
// 应用补偿系数并限制在有效范围内
int left_pwm = constrain(filtered_output * motor_compensation_left, -motor_max, motor_max);
int right_pwm = constrain(filtered_output * motor_compensation_right, -motor_max, motor_max);
motor_left_pwm = abs(left_pwm);
motor_right_pwm = abs(right_pwm);
if (motor_left_pwm < deadzone_left) motor_left_pwm = 0;
if (motor_right_pwm < deadzone_right) motor_right_pwm = 0;
}
String debug_info = "角度计算正常";
if (!mpu_connected) {
debug_info = "MPU6050未连接";
} else if (angleX - angle_offset > max_angle || angleX - angle_offset < min_angle) {
debug_info = "角度超出安全范围";
}
String calibration_step_info = "未开始校准";
String calibration_instructions = "点击开始校准，按照步骤操作";
if (calibrating) {
switch (calibration_step) {
case 1:
calibration_step_info = "步骤1/4: 请将小车直立，然后点击'直立确认'";
calibration_instructions = "将小车直立，点击直立确认";
break;
case 2:
calibration_step_info = "步骤2/4: 请将小车前倾到底，然后点击'前倾确认'";
calibration_instructions = "将小车前倾到底，点击前倾确认";
break;
case 3:
calibration_step_info = "步骤3/4: 请将小车后倾到底，然后点击'后倾确认'";
calibration_instructions = "将小车后倾到底，点击后倾确认";
break;
case 4:
calibration_step_info = "步骤4/4: 校准完成，点击'完成校准'开始自动平衡";
calibration_instructions = "校准完成，点击完成校准开始平衡";
break;
}
}
String json = "{";
json += ""angle":" + String(angleX - angle_offset, 2) + ",";
json += ""target_angle":" + String(target_angle, 2) + ",";
json += ""output":" + String(output, 2) + ",";
json += ""filtered_output":" + String(filtered_output, 2) + ",";
json += ""motor_left_pwm":" + String(motor_left_pwm) + ",";
json += ""motor_right_pwm":" + String(motor_right_pwm) + ",";
json += ""move_speed":" + String(move_speed) + ",";
json += ""turn_speed":" + String(turn_speed) + ",";
json += ""kp":" + String(kp, 2) + ",";
json += ""ki":" + String(ki, 3) + ",";
json += ""kd":" + String(kd, 2) + ",";
json += ""compensation_left":" + String(motor_compensation_left, 2) + ",";
json += ""compensation_right":" + String(motor_compensation_right, 2) + ",";
json += ""angle_offset":" + String(angle_offset, 2) + ",";
json += ""deadzone_left":" + String(deadzone_left) + ",";
json += ""deadzone_right":" + String(deadzone_right) + ",";
json += ""calibrating":" + String(calibrating ? "true" : "false") + ",";
json += ""calibration_step_info":"" + calibration_step_info + "",";
json += ""calibration_instructions":"" + calibration_instructions + "",";
json += ""angle_upright":" + String(angle_upright, 2) + ",";
json += ""angle_forward":" + String(angle_forward, 2) + ",";
json += ""angle_backward":" + String(angle_backward, 2) + ",";
json += ""debug_info":"" + debug_info + "",";
json += ""mode":"" + String(control_mode == AUTOMATIC ? "自动" : "手动") + """;
json += "}";
server.send(200, "application/json", json);
});
server.begin();
}
void startCalibration() {
calibrating = true;
calibration_step = 1;
control_mode = MANUAL;
stopMotors();
Serial.println("开始角度校准流程");
Serial.println("步骤1: 请将小车直立，然后点击'直立确认'");
}
void processCalibrationStep(int step) {
switch (step) {
case 1:
angle_upright = angleX;
calibration_step = 2;
Serial.println("直立角度已记录: " + String(angle_upright, 2) + "°");
Serial.println("步骤2: 请将小车前倾到底，然后点击'前倾确认'");
break;
case 2:
angle_forward = angleX;
calibration_step = 3;
Serial.println("前倾角度已记录: " + String(angle_forward, 2) + "°");
Serial.println("步骤3: 请将小车后倾到底，然后点击'后倾确认'");
break;
case 3:
angle_backward = angleX;
calibration_step = 4;
Serial.println("后倾角度已记录: " + String(angle_backward, 2) + "°");
Serial.println("步骤4: 点击'完成校准'开始自动平衡");
break;
case 4:
angle_offset = angle_upright;
calibrating = false;
calibration_step = 0;
control_mode = AUTOMATIC;
Serial.println("角度校准完成！");
Serial.println("直立角度: " + String(angle_upright, 2) + "°");
Serial.println("前倾角度: " + String(angle_forward, 2) + "°");
Serial.println("后倾角度: " + String(angle_backward, 2) + "°");
Serial.println("角度偏移: " + String(angle_offset, 2) + "°");
Serial.println("已切换到自动平衡模式");
break;
}
}
void loop() {
server.handleClient();
// 读取MPU6050数据
readMPU6050();
// 根据当前模式执行相应控制
if (control_mode == AUTOMATIC && !calibrating) {
// 自动平衡模式 - 使用PID控制
calculatePID();
automaticControl();
} else {
// 手动模式或校准中 - 停止电机
stopMotors();
}
// 显示状态到串口
static unsigned long last_display = 0;
if (millis() - last_display > 1000) {
displayStatus();
last_display = millis();
}
delay(10);
}
void readMPU6050() {
// 使用MPU6050_tockn库更新数据
mpu6050.update();
// 获取角度和陀螺仪数据
angleX = mpu6050.getAngleX();
gyroX = mpu6050.getGyroX();
}
void calculatePID() {
// 使用校准后的角度
float calibrated_angle = angleX - angle_offset;
error = target_angle - calibrated_angle;
// 积分项 (防饱和)
integral += error;
if (integral > ki_limit) integral = ki_limit;
if (integral < -ki_limit) integral = -ki_limit;
// 微分项
derivative = -gyroX;
// PID输出
output = kp * error + ki * integral + kd * derivative;
// 严格限制输出范围在-255到255之间
output = constrain(output, -motor_max, motor_max);
// 输出变化率限制
float output_diff = output - last_output;
if (output_diff > max_output_change) {
output = last_output + max_output_change;
} else if (output_diff < -max_output_change) {
output = last_output - max_output_change;
}
last_output = output;
// 输出滤波
filtered_output = output_filter_alpha * filtered_output + (1 - output_filter_alpha) * output;
}
void automaticControl() {
float calibrated_angle = angleX - angle_offset;
// 检查角度是否超出安全范围
if (calibrated_angle > max_angle || calibrated_angle < min_angle) {
stopMotors();
integral = 0;
return;
}
// 检查运动控制是否超时
if (movement_active && (millis() - last_movement_time > movement_timeout)) {
movement_active = false;
move_speed = 0;
turn_speed = 0;
Serial.println("运动控制超时，返回纯平衡模式");
}
int left_pwm = 0, right_pwm = 0;
bool left_forward = true, right_forward = true;
if (movement_active) {
int balance_output = (int)filtered_output;
int speed_output = move_speed;
int turn_output = turn_speed;
// 应用补偿系数并严格限制在-255到255之间
int left_output = constrain((balance_output + speed_output - turn_output) * motor_compensation_left, -motor_max, motor_max);
int right_output = constrain((balance_output + speed_output + turn_output) * motor_compensation_right, -motor_max, motor_max);
left_forward = (left_output >= 0);
right_forward = (right_output >= 0);
left_pwm = abs(left_output);
right_pwm = abs(right_output);
} else {
// 应用补偿系数并严格限制在-255到255之间
int left_output = constrain(filtered_output * motor_compensation_left, -motor_max, motor_max);
int right_output = constrain(filtered_output * motor_compensation_right, -motor_max, motor_max);
left_forward = (left_output >= 0);
right_forward = (right_output >= 0);
left_pwm = abs(left_output);
right_pwm = abs(right_output);
}
// 应用电机控制
setMotorSpeed(left_pwm, right_pwm, left_forward, right_forward);
}
void stopMotors() {
analogWrite(motor1_pwm, 0);
analogWrite(motor2_pwm, 0);
digitalWrite(motor1_dir, LOW);
digitalWrite(motor2_dir, LOW);
move_speed = 0;
turn_speed = 0;
movement_active = false;
}
void setMotorSpeed(int left_pwm, int right_pwm, bool left_forward, bool right_forward) {
// 严格限制PWM值在0-255之间
left_pwm = constrain(left_pwm, 0, motor_max);
right_pwm = constrain(right_pwm, 0, motor_max);
// 应用死区
if (left_pwm < deadzone_left) {
left_pwm = 0;
}
if (right_pwm < deadzone_right) {
right_pwm = 0;
}
// 控制左电机
if (left_pwm == 0) {
analogWrite(motor1_pwm, 0);
digitalWrite(motor1_dir, LOW);
} else {
digitalWrite(motor1_dir, left_forward ? HIGH : LOW);
analogWrite(motor1_pwm, left_pwm);
}
// 控制右电机
if (right_pwm == 0) {
analogWrite(motor2_pwm, 0);
digitalWrite(motor2_dir, LOW);
} else {
digitalWrite(motor2_dir, right_forward ? HIGH : LOW);
analogWrite(motor2_pwm, right_pwm);
}
}
void displayStatus() {
float calibrated_angle = angleX - angle_offset;
Serial.print("角度(X轴): ");
Serial.print(calibrated_angle, 1);
Serial.print("° | 目标: ");
Serial.print(target_angle, 1);
Serial.print("° | 误差: ");
Serial.print(error, 2);
Serial.print(" | PID输出: ");
Serial.print(output, 2);
Serial.print(" | 滤波输出: ");
Serial.print(filtered_output, 2);
Serial.print(" | 积分: ");
Serial.print(integral, 2);
Serial.print(" | 微分: ");
Serial.print(derivative, 2);
Serial.print(" | PID参数: Kp=");
Serial.print(kp, 2);
Serial.print(" Ki=");
Serial.print(ki, 3);
Serial.print(" Kd=");
Serial.print(kd, 2);
Serial.print(" | 补偿系数: 左=");
Serial.print(motor_compensation_left, 2);
Serial.print(" 右=");
Serial.print(motor_compensation_right, 2);
Serial.print(" | 死区: 左=");
Serial.print(deadzone_left);
Serial.print(" 右=");
Serial.print(deadzone_right);
Serial.print(" | 运动控制: ");
if (movement_active) {
Serial.print("速度=");
Serial.print(move_speed);
Serial.print(" 转向=");
Serial.print(turn_speed);
} else {
Serial.print("关闭");
}
Serial.print(" | 模式: ");
Serial.print(control_mode == AUTOMATIC ? "自动" : "手动");
if (calibrated_angle > max_angle || calibrated_angle < min_angle) {
Serial.print(" | 状态: 超出安全范围!");
} else if (movement_active) {
Serial.print(" | 状态: 运动控制中");
} else if (filtered_output > 0) {
Serial.print(" | 状态: 正转");
} else if (filtered_output < 0) {
Serial.print(" | 状态: 反转");
} else {
Serial.print(" | 状态: 平衡");
}
Serial.println();
}mpu6050 模块安装方向为，X箭头方向与小车轮子轴向方向平行
© 版权声明
文章版权归作者所有，未经允许请勿转载。
相关文章
暂无评论...