- 單片機實時操作系統(tǒng)RTOS绵跷；
- 任務(wù)并發(fā)處理螺句，包括了網(wǎng)頁服務(wù)器功能勾笆；
- 完整代碼,目前按工程量已經(jīng)350行代碼阿宅！
#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.2; //小車初始平衡角度,需要微調(diào)
unsigned long now = 0, past = 0, interval = 50; //interval為小車調(diào)整姿態(tài)的時長,1000為一秒
// PID控制器對象
double angleKp = 5, angleKi = 0.1, angleKd = 0.15, angleInput = 0, angleOutput = 0; //需要調(diào)整的角度環(huán)參數(shù)Kp,Kd
double speedKp = 3, speedKi = 0.3, speedKd = 0.2, speedInput = 0, speedOutput = 0; //需要調(diào)整的速度環(huán)參數(shù)Kp,Ki
double turnKp = 0.1, turnKi = 0.01, turnKd = 0.02, turnInput = 0, turnOutput = 0; //需要調(diào)整的轉(zhuǎn)向環(huán)參數(shù)Kp,Ki
double angleSetpoint = -1; // 角度設(shè)定值
double speedSetpoint = 0; // 速度設(shè)定值,負為向前，正為向后笼蛛，絕對值不要超過3
double turnSetpoint = 0; // 轉(zhuǎn)向設(shè)定值
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);
// 任務(wù)句柄
TaskHandle_t TaskGetAngleXHandle = NULL;
TaskHandle_t TaskGetSpeedHandle = NULL;
TaskHandle_t TaskUpdateAndAdjustMotorsHandle = NULL;
TaskHandle_t TaskWebServerHandle = NULL;
// Web服務(wù)器實例
WiFiServer server(80);
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服務(wù)器
server.begin();
// 創(chuàng)建任務(wù)
xTaskCreate(
TaskGetAngleX, // 任務(wù)函數(shù)
"GetAngleX", // 任務(wù)名稱
128, // 任務(wù)堆棧大小
NULL, // 傳遞給任務(wù)的參數(shù)
1, // 任務(wù)優(yōu)先級
&TaskGetAngleXHandle // 任務(wù)句柄
);
xTaskCreate(
TaskGetSpeed, // 任務(wù)函數(shù)
"GetSpeed", // 任務(wù)名稱
128, // 任務(wù)堆棧大小
NULL, // 傳遞給任務(wù)的參數(shù)
1, // 任務(wù)優(yōu)先級
&TaskGetSpeedHandle // 任務(wù)句柄
);
xTaskCreate(
TaskUpdateAndAdjustMotors, // 任務(wù)函數(shù)
"UpdateAndAdjustMotors", // 任務(wù)名稱
128, // 任務(wù)堆棧大小
NULL, // 傳遞給任務(wù)的參數(shù)
1, // 任務(wù)優(yōu)先級
&TaskUpdateAndAdjustMotorsHandle // 任務(wù)句柄
);
xTaskCreate(
TaskWebServer, // 任務(wù)函數(shù)
"WebServer", // 任務(wù)名稱
256, // 任務(wù)堆棧大小
NULL, // 傳遞給任務(wù)的參數(shù)
1, // 任務(wù)優(yōu)先級
&TaskWebServerHandle // 任務(wù)句柄
);
// 啟動調(diào)度器
vTaskStartScheduler();
}
void loop() {
// 主循環(huán)不需要做任何事情洒放，F(xiàn)reeRTOS 會管理任務(wù)
}
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++;
} //左測速碼盤計數(shù)
void Code_right() {
count_right++;
} //右測速碼盤計數(shù)
void getSpeed() {
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;
}
void getAngleX() {
mpu6050.update();
angleX = mpu6050.getAngleX() - zeroAngle;
}
void adjustLeftAction(float leftOutput) {
runset(2, abs((int)leftOutput), leftOutput > 0 ? 1 : 0);
}
void adjustRightAction(float rightOutput) {
runset(1, abs((int)rightOutput), rightOutput > 0 ? 1 : 0);
}
// 任務(wù)函數(shù)：獲取角度
void TaskGetAngleX(void *pvParameters) {
for (;;) {
getAngleX();
vTaskDelay(pdMS_TO_TICKS(interval)); // 延遲一段時間
}
}
// 任務(wù)函數(shù)：獲取速度
void TaskGetSpeed(void *pvParameters) {
for (;;) {
getSpeed();
vTaskDelay(pdMS_TO_TICKS(interval)); // 延遲一段時間
}
}
// 任務(wù)函數(shù)：更新和調(diào)整電機
void TaskUpdateAndAdjustMotors(void *pvParameters) {
for (;;) {
updateAndAdjustMotors();
vTaskDelay(pdMS_TO_TICKS(interval)); // 延遲一段時間
}
}
void updateAndAdjustMotors() {
// 更新輸入
angleInput = angleX;
speedInput = speed;
turnInput = leftSpeed - rightSpeed;
// 計算PID輸出
anglePID.Compute();
speedPID.Compute();
turnPID.Compute();
// 計算電機輸出
leftOutput = angleOutput - speedOutput - turnOutput;
rightOutput = angleOutput - speedOutput + turnOutput;
// 調(diào)整電機動作
adjustLeftAction(leftOutput);
adjustRightAction(rightOutput);
}
// 任務(wù)函數(shù)：Web服務(wù)器
void TaskWebServer(void *pvParameters) {
WiFiClient client;
while (true) {
client = server.available();
if (client) {
Serial.println("New client");
String currentLine = "";
while (client.connected()) {
if (client.available()) {
char c = client.read();
Serial.write(c);
if (c == '\n') {
if (currentLine.length() == 0) {
client.println("HTTP/1.1 200 OK");
client.println("Content-type:text/html");
client.println();
client.println("<html><body>");
client.println("<h1>PID Controller Settings</h1>");
client.println("<form method='GET'>");
client.println("Angle Kp: <input type='text' name='angleKp' value='");
client.print(angleKp);
client.println("'><br>");
client.println("Angle Ki: <input type='text' name='angleKi' value='");
client.print(angleKi);
client.println("'><br>");
client.println("Angle Kd: <input type='text' name='angleKd' value='");
client.print(angleKd);
client.println("'><br>");
client.println("Speed Kp: <input type='text' name='speedKp' value='");
client.print(speedKp);
client.println("'><br>");
client.println("Speed Ki: <input type='text' name='speedKi' value='");
client.print(speedKi);
client.println("'><br>");
client.println("Speed Kd: <input type='text' name='speedKd' value='");
client.print(speedKd);
client.println("'><br>");
client.println("Turn Kp: <input type='text' name='turnKp' value='");
client.print(turnKp);
client.println("'><br>");
client.println("Turn Ki: <input type='text' name='turnKi' value='");
client.print(turnKi);
client.println("'><br>");
client.println("Turn Kd: <input type='text' name='turnKd' value='");
client.print(turnKd);
client.println("'><br>");
client.println("<input type='submit' value='Update'>");
client.println("</form>");
client.println("</body></html>");
break;
} else {
currentLine = "";
}
} else if (c != '\r') {
currentLine += c;
}
if (currentLine.endsWith("GET /")) {
// 處理根路徑請求
} else if (currentLine.startsWith("GET /?")) {
// 處理表單提交
String query = currentLine.substring(5, currentLine.indexOf(" HTTP/1.1"));
if (query.indexOf("angleKp=") != -1) {
angleKp = query.substring(query.indexOf("angleKp=") + 8, query.indexOf("&", query.indexOf("angleKp="))).toDouble();
}
if (query.indexOf("angleKi=") != -1) {
angleKi = query.substring(query.indexOf("angleKi=") + 8, query.indexOf("&", query.indexOf("angleKi="))).toDouble();
}
if (query.indexOf("angleKd=") != -1) {
angleKd = query.substring(query.indexOf("angleKd=") + 8, query.indexOf("&", query.indexOf("angleKd="))).toDouble();
}
if (query.indexOf("speedKp=") != -1) {
speedKp = query.substring(query.indexOf("speedKp=") + 8, query.indexOf("&", query.indexOf("speedKp="))).toDouble();
}
if (query.indexOf("speedKi=") != -1) {
speedKi = query.substring(query.indexOf("speedKi=") + 8, query.indexOf("&", query.indexOf("speedKi="))).toDouble();
}
if (query.indexOf("speedKd=") != -1) {
speedKd = query.substring(query.indexOf("speedKd=") + 8, query.indexOf("&", query.indexOf("speedKd="))).toDouble();
}
if (query.indexOf("turnKp=") != -1) {
turnKp = query.substring(query.indexOf("turnKp=") + 7, query.indexOf("&", query.indexOf("turnKp="))).toDouble();
}
if (query.indexOf("turnKi=") != -1) {
turnKi = query.substring(query.indexOf("turnKi=") + 7, query.indexOf("&", query.indexOf("turnKi="))).toDouble();
}
if (query.indexOf("turnKd=") != -1) {
turnKd = query.substring(query.indexOf("turnKd=") + 7, query.indexOf("&", query.indexOf("turnKd="))).toDouble();
}
// 更新PID參數(shù)
anglePID.SetTunings(angleKp, angleKi, angleKd);
speedPID.SetTunings(speedKp, speedKi, speedKd);
turnPID.SetTunings(turnKp, turnKi, turnKd);
// 重定向到根路徑
client.println("HTTP/1.1 302 Found");
client.println("Location: /");
client.println("Connection: close");
break;
}
}
}
delay(1);
client.stop();
Serial.println("Client disconnected");
}
vTaskDelay(pdMS_TO_TICKS(100)); // 延遲一段時間
}
}
