- 全部工程项很大，总共超过500行，借助通义灵码才完成；
- pid参数不是想象的那么容易调，但借助详细的实时参数表会好很多；
- 全部代码：
#include <PID_v1.h>
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <Arduino_FreeRTOS.h>
#include <WiFiS3.h>
#include "arduino_secrets.h"
char ssid[] = SECRET_SSID; // your network SSID (name)
char pass[] = SECRET_PASS; // your network password (use for WPA, or use as key for WEP)
int keyIndex = 0; // your network key index number (needed only for WEP)
int STBY = 8; //使能端
int PWMA = 9; //右电机PWM输出控制脚
int AIN1 = 7; //右电机正极
int AIN2 = 6; //右电机负极
int PWMB = 10; //左电机PWM输出控制脚
int BIN1 = 13; //左电机正极
int BIN2 = 12; //左电机负极
#define PinA_left 4 //中断
#define PinA_right 2 //中断1
int count_right = 0, count_left = 0;
double leftSpeed = 0, rightSpeed = 0, speed = 0, angleX = 0;
double leftOutput = 0, rightOutput = 0;
double zeroAngle = 1; //小车初始平衡角度,需要微调
unsigned long now = 0, past = 0, interval = 40; //interval为小车调整姿态的时长,1000为一秒，40毫秒最佳
// PID控制器对象
double angleKp = 5, angleKi = 0.01, angleKd = 0.2, angleInput = 0, angleOutput = 0; //需要调整的角度环参数Kp,Kd
double speedKp = 5, speedKi = 0.4, speedKd = 0.1, speedInput = 0, speedOutput = 0; //需要调整的速度环参数Kp,Ki
double turnKp = 0.3, turnKi = 0.01, turnKd = 0.02, turnInput = 0, turnOutput = 0; //需要调整的转向环参数Kp,Ki
double angleSetpoint = 0; // 角度设定值
double speedSetpoint = 0; // 速度设定值,正为向前，负为向后，绝对值不要超过3
double turnSetpoint = 0; // 转向设定值,正为左，负为右，调整幅度±1
PID anglePID(&angleInput, &angleOutput, &angleSetpoint, angleKp, angleKi, angleKd, DIRECT);
PID speedPID(&speedInput, &speedOutput, &speedSetpoint, speedKp, speedKi, speedKd, DIRECT);
PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, DIRECT);
// MPU6050实例化
MPU6050 mpu6050(Wire);
// 任务句柄
TaskHandle_t TaskGetAngleXHandle = NULL;
TaskHandle_t TaskGetSpeedHandle = NULL;
TaskHandle_t TaskUpdateAndAdjustMotorsHandle = NULL;
TaskHandle_t TaskWebServerHandle = NULL;
// Web服务器实例
WiFiServer server(80);
bool webServerEnabled = true; // 默认情况下不启用网页服务器任务
void setup() {
pinMode(STBY, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PinA_left, INPUT); //测速码盘输入
pinMode(PinA_right, INPUT);
attachInterrupt(digitalPinToInterrupt(PinA_left), Code_left, CHANGE);
attachInterrupt(digitalPinToInterrupt(PinA_right), Code_right, CHANGE);
Serial.begin(9600);
Wire.begin();
mpu6050.begin();
mpu6050.calcGyroOffsets(true);
// 初始化PID控制器
anglePID.SetMode(AUTOMATIC);
anglePID.SetSampleTime(interval);
anglePID.SetOutputLimits(-254, 254);
speedPID.SetMode(AUTOMATIC);
speedPID.SetSampleTime(interval);
speedPID.SetOutputLimits(-254, 254);
turnPID.SetMode(AUTOMATIC);
turnPID.SetSampleTime(interval);
turnPID.SetOutputLimits(-254, 254);
// 连接Wi-Fi
WiFi.begin(ssid, pass);
while (WiFi.status() != WL_CONNECTED) {
delay(1000);
Serial.println("Connecting to WiFi...");
}
Serial.println("Connected to WiFi");
Serial.print("IP address: ");
Serial.println(WiFi.localIP());
// 启动Web服务器
server.begin();
// 创建任务
xTaskCreate(
TaskGetAngleX, // 任务函数
"GetAngleX", // 任务名称
128, // 任务堆栈大小
NULL, // 传递给任务的参数
4, // 任务优先级
&TaskGetAngleXHandle // 任务句柄
);
xTaskCreate(
TaskGetSpeed, // 任务函数
"GetSpeed", // 任务名称
128, // 任务堆栈大小
NULL, // 传递给任务的参数
3, // 任务优先级
&TaskGetSpeedHandle // 任务句柄
);
xTaskCreate(
TaskUpdateAndAdjustMotors, // 任务函数
"UpdateAndAdjustMotors", // 任务名称
128, // 任务堆栈大小
NULL, // 传递给任务的参数
2, // 任务优先级
&TaskUpdateAndAdjustMotorsHandle // 任务句柄
);
xTaskCreate(
TaskWebServer, // 任务函数
"WebServer", // 任务名称
512, // 任务堆栈大小
NULL, // 传递给任务的参数
1, // 任务优先级
&TaskWebServerHandle // 任务句柄
);
// 启动调度器
vTaskStartScheduler();
}
void loop() {
// 主循环不需要做任何事情，FreeRTOS 会管理任务
}
void runset(int motor, int speed, int direction) {
digitalWrite(STBY, 1);
if (motor == 1 && direction == 1) {
digitalWrite(AIN1, 1);
digitalWrite(AIN2, 0);
analogWrite(PWMA, speed);
}
if (motor == 2 && direction == 1) {
digitalWrite(BIN1, 1);
digitalWrite(BIN2, 0);
analogWrite(PWMB, speed);
}
if (motor == 1 && direction == 0) {
digitalWrite(AIN1, 0);
digitalWrite(AIN2, 1);
analogWrite(PWMA, speed);
}
if (motor == 2 && direction == 0) {
digitalWrite(BIN1, 0);
digitalWrite(BIN2, 1);
analogWrite(PWMB, speed);
}
}
void stop() {
digitalWrite(STBY, LOW);
}
void Code_left() {
count_left++;
} //左测速码盘计数
void Code_right() {
count_right++;
} //右测速码盘计数
void TaskGetAngleX(void *pvParameters) {
for (;;) {
mpu6050.update();
angleX = mpu6050.getAngleX() - zeroAngle;
vTaskDelay(pdMS_TO_TICKS(interval)); // 延迟一段时间
}
}
// 任务函数：获取速度
void TaskGetSpeed(void *pvParameters) {
for (;;) {
rightSpeed = (digitalRead(AIN1) == 1 && digitalRead(AIN2) == 0) ? count_right : -(count_right);
leftSpeed = (digitalRead(BIN1) == 1 && digitalRead(BIN2) == 0) ? count_left : -(count_left);
speed = (leftSpeed + rightSpeed) / 2;
count_left = 0;
count_right = 0;
vTaskDelay(pdMS_TO_TICKS(interval)); // 延迟一段时间
}
}
// 任务函数：更新和调整电机
void TaskUpdateAndAdjustMotors(void *pvParameters) {
for (;;) {
angleInput = angleX;
speedInput = speed;
turnInput = leftSpeed - rightSpeed;
// 计算PID输出
anglePID.Compute();
speedPID.Compute();
turnPID.Compute();
// 计算电机输出
if(abs(angleX)>10){
leftOutput = 0;
rightOutput = 0;
}
leftOutput = angleOutput - speedOutput - turnOutput;
leftOutput = constrain(leftOutput, -254, 254);
rightOutput = angleOutput - speedOutput + turnOutput;
rightOutput = constrain(rightOutput, -254, 254);
// 调整电机动作
// runset(2, abs((int)leftOutput), leftOutput > 0 ? 1 : 0);
// runset(1, abs((int)rightOutput), rightOutput > 0 ? 1 : 0);
vTaskDelay(pdMS_TO_TICKS(interval)); // 延迟一段时间
}
}
void TaskWebServer(void *pvParameters) {
while (1) {
WiFiClient client = server.available();
if (client) {
Serial.println("New client");
String request = "";
while (client.connected()) {
if (client.available()) {
char c = client.read();
request += c;
if (c == '\n') {
// 处理请求
if (request.indexOf("/debug") != -1) {
handleDebugPage(client);
} else if (request.indexOf("/update") != -1) {
handleControlPage(client, request);
} else {
handleControlPage(client, request);
}
break;
}
}
}
client.stop();
Serial.println("Client disconnected");
}
vTaskDelay(pdMS_TO_TICKS(100)); // 延迟0.1秒
}
}
void handleDebugPage(WiFiClient &client) {
sendPage(client, "Debug", "Go to Car Control", "/control", getDebugPageContent());
}
void handleControlPage(WiFiClient &client, const String &request) {
// 处理更新请求
if (request.indexOf("speedSetpoint=") != -1) {
int start = request.indexOf("speedSetpoint=") + 14;
int end = request.indexOf(" HTTP/1.1");
String speedStr = request.substring(start, end);
float newSpeedSetpoint = speedStr.toFloat();
if (newSpeedSetpoint >= -30 && newSpeedSetpoint <= 30) { // 调整范围
speedSetpoint = newSpeedSetpoint;
Serial.println("Updated speedSetpoint: " + String(speedSetpoint));
} else {
Serial.println("Invalid speedSetpoint value: " + speedStr);
}
}
if (request.indexOf("angleSetpoint=") != -1) {
int start = request.indexOf("angleSetpoint=") + 14;
int end = request.indexOf(" HTTP/1.1");
String angleStr = request.substring(start, end);
float newAngleSetpoint = angleStr.toFloat();
if (newAngleSetpoint >= -30 && newAngleSetpoint <= 30) { // 调整范围
angleSetpoint = newAngleSetpoint;
Serial.println("Updated angleSetpoint: " + String(angleSetpoint));
} else {
Serial.println("Invalid angleSetpoint value: " + angleStr);
}
}
// 重新发送页面以显示更新后的值
sendPage(client, "Car Control", "Go to Debug", "/debug", getControlPageContent());
}
void sendPage(WiFiClient &client, const String &title, const String &linkText, const String &linkHref, const String &content) {
client.println("HTTP/1.1 200 OK");
client.println("Content-type:text/html; charset=utf-8"); // 设置字符编码为UTF-8
client.println();
client.println("<html><head><title>" + title + "</title></head><body>");
client.println("<h1>" + title + " Page</h1>");
client.println(content);
client.println("</body></html>");
}
String getControlPageContent() {
return "<style>"
"body { font-family: Arial, sans-serif; display: flex; justify-content: center; align-items: center; height: 100vh; background-color: #f0f0f0; }"
".container { text-align: center; }"
".button-container { display: flex; justify-content: center; align-items: center; flex-direction: column; }"
".button {"
" display: inline-block;"
" width: 100px;"
" height: 100px;"
" border-radius: 50%;"
" background-color: #4CAF50;"
" color: white;"
" text-align: center;"
" line-height: 100px;"
" font-size: 24px;"
" margin: 10px;"
" text-decoration: none;"
" transition: background-color 0.3s;"
"}"
".button:hover { background-color: #45a049; }"
".button:active { background-color: #3e8e41; }"
"</style>"
"<div class='container'>"
"<h1>Car Control Page</h1>"
"<h2>Angle Setpoint: <span id='angleSetpoint'>" + String(angleSetpoint) + "</span></h2>"
"<h2>Speed Setpoint: <span id='speedSetpoint'>" + String(speedSetpoint) + "</span></h2>"
"<div class='button-container'>"
"<a href='/update?speedSetpoint=" + String(speedSetpoint + 3) + "' class='button'>↑<br>上</a>"
"<div style='display: flex;'>"
"<a href='/update?angleSetpoint=" + String(angleSetpoint + 3) + "' class='button'>←<br>左</a>"
"<a href='/update?speedSetpoint=" + String(speedSetpoint - 3) + "' class='button'>↓<br>下</a>"
"<a href='/update?angleSetpoint=" + String(angleSetpoint - 3) + "' class='button'>→<br>右</a>"
"</div>"
"</div>"
"<br><a href=\"/debug\">Go to Debug</a>"
"</div>";
}
String getDebugPageContent() {
return "<h1>Debug Page</h1>"
"<p>This is the debug page.</p>"
"<br><a href=\"/control\">Go to Car Control</a>";
}
- 小车运行后生成内网网页服务器，有两个页面，一个是手操小车运行，一个是内参debug;
然后开始玩吧！
