两轮平衡小车心得（十二）FreeRTOS

1. 单片机实时操作系统RTOS；
2. 任务并发处理，包括了网页服务器功能；
3. 完整代码,目前按工程量已经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;                                                          //小车初始平衡角度,需要微调
unsigned long now = 0, past = 0, interval = 50;                                   //interval为小车调整姿态的时长,1000为一秒

// PID控制器对象
double angleKp = 5, angleKi = 0.1, angleKd = 0.15, angleInput = 0, angleOutput = 0;  //需要调整的角度环参数Kp,Kd
double speedKp = 3, speedKi = 0.3, speedKd = 0.2, speedInput = 0, speedOutput = 0;  //需要调整的速度环参数Kp,Ki
double turnKp = 0.1, turnKi = 0.01, turnKd = 0.02, turnInput = 0, turnOutput = 0;    //需要调整的转向环参数Kp,Ki
double angleSetpoint = -1;                                                        // 角度设定值
double speedSetpoint = 0;                                                         // 速度设定值,负为向前，正为向后，绝对值不要超过3
double turnSetpoint = 0;                                                          // 转向设定值
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);

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,                         // 传递给任务的参数
    1,                            // 任务优先级
    &TaskGetAngleXHandle          // 任务句柄
  );

  xTaskCreate(
    TaskGetSpeed,                 // 任务函数
    "GetSpeed",                   // 任务名称
    128,                          // 任务堆栈大小
    NULL,                         // 传递给任务的参数
    1,                            // 任务优先级
    &TaskGetSpeedHandle           // 任务句柄
  );

  xTaskCreate(
    TaskUpdateAndAdjustMotors,    // 任务函数
    "UpdateAndAdjustMotors",      // 任务名称
    128,                          // 任务堆栈大小
    NULL,                         // 传递给任务的参数
    1,                            // 任务优先级
    &TaskUpdateAndAdjustMotorsHandle // 任务句柄
  );

  xTaskCreate(
    TaskWebServer,                // 任务函数
    "WebServer",                  // 任务名称
    256,                          // 任务堆栈大小
    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 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);
}

// 任务函数：获取角度
void TaskGetAngleX(void *pvParameters) {
  for (;;) {
    getAngleX();
    vTaskDelay(pdMS_TO_TICKS(interval));  // 延迟一段时间
  }
}

// 任务函数：获取速度
void TaskGetSpeed(void *pvParameters) {
  for (;;) {
    getSpeed();
    vTaskDelay(pdMS_TO_TICKS(interval));  // 延迟一段时间
  }
}

// 任务函数：更新和调整电机
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;

  // 调整电机动作
  adjustLeftAction(leftOutput);
  adjustRightAction(rightOutput);
}

// 任务函数：Web服务器
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参数
            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));  // 延迟一段时间
  }
}