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,0char *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*转化为stringpid_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;
}