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

单片机实时操作系统RTOS；
任务并发处理，包括了网页服务器功能；
完整代码,目前按工程量已经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));  // 延迟一段时间
  }
}

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 216,001评论 6赞 498
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 92,210评论 3赞 392
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 161,874评论 0赞 351
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 58,001评论 1赞 291
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 67,022评论 6赞 388
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 51,005评论 1赞 295
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 39,929评论 3赞 416
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 38,742评论 0赞 271
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 45,193评论 1赞 309
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 37,427评论 2赞 331
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 39,583评论 1赞 346
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 35,305评论 5赞 342
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 40,911评论 3赞 325
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 31,564评论 0赞 21
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 32,731评论 1赞 268
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 47,581评论 2赞 368
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 44,478评论 2赞 352

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

推荐阅读更多精彩内容