ESP32使用mpu6050以及pid调参

pid

//pid参考教程  https://www.xpstem.com/article/10120
#include <MPU6050_tockn.h>
#include <Wire.h>

MPU6050 mpu6050(Wire);

// pid相关参数
unsigned long lastTime;
double Input, Output, Setpoint;
double ITerm, lastInput;
double kp, ki, kd;
int SampleTime = 1000; //1秒
double outMin,outMax;

void setup() {
  Serial.begin(115200);
  delay(5000);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  // pid初始化
  Setpoint = 0; //设置目标值
  SetTunings(15,0.3,0);  //设置kp ki kd 参数
  SetSampleTime(50);//设置采样时间  毫秒
  SetOutputLimits(-255,255); //设置输出的最大最小值
}

void loop() {
  mpu6050.update();
  // Serial.print("angleX : ");
  // Serial.print(mpu6050.getAngleX());
  Serial.print("\tangleY : ");
  Serial.println(mpu6050.getAngleY());
  // Serial.print("\tangleZ : ");
  // Serial.println(mpu6050.getAngleZ());
  Input = mpu6050.getAngleY(); //设置 输入值
  Compute();//计算
  Serial.println(Output);//输出
}

//pid计算 Output输出值的范围在 -255--255
void Compute()
{
   /*How long since we last calculated*/
   unsigned long now = millis();
   int timeChange = (now-lastTime);
   if(timeChange >= SampleTime)
   {
     double error = Setpoint - Input;
     ITerm += (ki*error);
     if(ITerm > outMax) ITerm = outMax;
     else if(ITerm < outMin) ITerm = outMin;

     double dInput =(Input-lastInput);

    Output = kp * error + ITerm - kd * dInput;
    if(Output > outMax) Output = outMax;
    else if(Output < outMin) Output = outMin;

    lastInput = Input;
    lastTime = now;
   } 
}
// 设置kp ki kd参数值
void SetTunings(double Kp, double Ki, double Kd)
{
  double SampleTimeInSec = ((double)SampleTime)/1000;
   kp = Kp;
   ki = Ki*SampleTimeInSec;
   kd = Kd/SampleTimeInSec;
}
// 设置采样时间
void SetSampleTime(int NewSampleTime)
{
  if(NewSampleTime >0)
  {
    double ratio = (double)NewSampleTime/(double)SampleTime;
    ki *= ratio;
    kd /= ratio;
    SampleTime = (unsigned long)NewSampleTime;
  }
}
// 设置输出值的范围
void SetOutputLimits(double Min,double Max)
{
  if(Min >Max) return;
  outMin = Min;
  outMax = Max;

  if(Output >outMax ) Output = outMax;
  else if (Output <outMin) Output = outMin;

  if(ITerm >outMax) ITerm = outMax;
  else if(ITerm < outMin) ITerm = outMin;
}


/*
#include <MPU6050_tockn.h>
#include <Wire.h>

MPU6050 mpu6050(Wire);

long timer = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  delay(5000);
}

void loop() {
  mpu6050.update();

  if(millis() - timer > 1000){
    
    Serial.println("=======================================================");
    Serial.print("temp : ");Serial.println(mpu6050.getTemp());
    Serial.print("accX : ");Serial.print(mpu6050.getAccX());
    Serial.print("\taccY : ");Serial.print(mpu6050.getAccY());
    Serial.print("\taccZ : ");Serial.println(mpu6050.getAccZ());
  
    Serial.print("gyroX : ");Serial.print(mpu6050.getGyroX());
    Serial.print("\tgyroY : ");Serial.print(mpu6050.getGyroY());
    Serial.print("\tgyroZ : ");Serial.println(mpu6050.getGyroZ());
  
    Serial.print("accAngleX : ");Serial.print(mpu6050.getAccAngleX());
    Serial.print("\taccAngleY : ");Serial.println(mpu6050.getAccAngleY());
  
    Serial.print("gyroAngleX : ");Serial.print(mpu6050.getGyroAngleX());
    Serial.print("\tgyroAngleY : ");Serial.print(mpu6050.getGyroAngleY());
    Serial.print("\tgyroAngleZ : ");Serial.println(mpu6050.getGyroAngleZ());
    
    Serial.print("angleX : ");Serial.print(mpu6050.getAngleX());
    Serial.print("\tangleY : ");Serial.print(mpu6050.getAngleY());
    Serial.print("\tangleZ : ");Serial.println(mpu6050.getAngleZ());
    Serial.println("=======================================================\n");
    timer = millis();
    
  }

}
*/

串口调参

#include <Arduino.h>
//串口输入字符串的模式  10,0.01,0   中间使用,间隔
double pid_array[] ={10,0.01,0};  //申明一个数组  分别存放 kp  ki  kd 的值

void setup() {
  Serial.begin(115200);
}

void loop() {
  String inString="";
  while(Serial.available()>0){
    inString += char(Serial.read());
    delay(10);      // 延时函数用于等待字符完全进入缓冲区
  }
  // 检查是否接收到数据，如果接收到数据，则输出该数据
  if(inString!=""){
    Serial.print("Input String:");
    Serial.println(inString);       //Input String:10,0.01,0
    char *ptr = strtok(const_cast<char*>(inString.c_str()),",");   // string转化为char* 使用   const_cast<char*>(inString.c_str())
    int i = 0;
    while(ptr != NULL){
      Serial.println(ptr);
      String tmp_str = ptr;    //char*转化为string
      pid_array[i] = tmp_str.toDouble();
      i++;
      ptr = strtok(NULL,",");
    }
    SetTunings(pid_array[0],pid_array[1],pid_array[2]);
  }
}

// 设置kp ki kd参数值
void SetTunings(double Kp, double Ki, double Kd)
{
  Serial.print("kp=");
  Serial.print(Kp);
  Serial.print("\t ki=");
  Serial.print(Ki);
  Serial.print("\t kd=");
  Serial.println(Kd);    //kp=10.00	 ki=0.01	 kd=0.00
  // double SampleTimeInSec = ((double)SampleTime)/1000;
  //  kp = Kp;
  //  ki = Ki*SampleTimeInSec;
  //  kd = Kd/SampleTimeInSec;
}