装好小车后,最重要的部分就是PID算法了。
推荐一个讲PID算法的博文:点击打开链接
以及PID参数的调试步骤:点击打开链接
还有Arduino PID Library的官网:点击打开链接
目前我只实现了站立的功能,蓝牙遥控还没加上去,等春节后了吧。
代码中使用了两套PID参数进行控制,Kp,Ki,Kd是控制平衡速度的。也就是小车往前倾,加速;后倾,减速这个过程。
但是仅靠一个PID控制会出现一个问题:
因为小车的平衡角度Setpoint是手动设定的,所以会有一个微小的误差。当小车以Setpoint为基准进行调整时,可能有一个细微的倾斜,从而导致小车一直向一边跑。
于是引入另一套速度的PID参数:Sp,Si,Sd。 Inputs 为一个变量speedcount,每次中断检查车轮的转向,正向+1,反向-1。这样,当小车持续向一个方向移动时,speedcount会越来越大,从而产生的Outputs也更大。于是对车轮加一个反向的电压,使小车尽量停在原地。
实现的时候,把Outputs的作用加到第一套PID的目标平衡角度Setpoint上,就可以实现效果。
现在小车基本可以站立在一个小范围内了。
代码
#include "PID_v1.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu(0x68);
#define center 0x7F
char flag = 0;
double delta_v = 10.0;
char num = 0;
double time;
double balance_angle = 2.2;
signed int speeds = 0;
signed int oldspeed = 0;
byte inByte;
// MPU control/status vars
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
signed int speedcount = 0;
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float angle;
double Setpoint, Input, Output;
double kp = 17, ki = 100.0, kd = 0.5;//需要你修改的参数 kp = 25, ki = 185.0, kd = 0.29;
double Setpoints, Inputs, Outputs;
double sp = 20, si = 0, sd = 0.0;//需要你修改的参数
unsigned char dl = 50, count;
union{
signed int all;
unsigned char s[2];
}data;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
/*
Input: The variable we're trying to control (double)
Output: The variable that will be adjusted by the pid (double)
Setpoint: The value we want to Input to maintain (double)
Kp, Ki, Kd: Tuning Parameters. these affect how the pid will chage the output. (double>=0)
*/
PID myPID(&Input, &Output, &Setpoint, kp, ki, kd, DIRECT);
PID sPID(&Inputs, &Outputs, &Setpoints, sp, si, sd, DIRECT);
void motor(int v)
{
if (v>0)
{
v += dl;
digitalWrite(6, 0);
digitalWrite(7, 1);
digitalWrite(8, 1);
digitalWrite(9, 0);
analogWrite(10, v + delta_v);
analogWrite(11, v);
}
else if (v<0)
{
v = -v;
v += dl;
digitalWrite(6, 1);
digitalWrite(7, 0);
digitalWrite(8, 0);
digitalWrite(9, 1);
analogWrite(10, v + delta_v);
analogWrite(11, v);
}
else
{
analogWrite(10, 0);
analogWrite(11, 0);
}
}
void motort(int v)
{
if (v>0)
{
v += dl;
digitalWrite(8, 1);
digitalWrite(9, 0);
analogWrite(10, v);
}
else if (v<0)
{
v = -v;
v += dl;
digitalWrite(8, 0);
digitalWrite(9, 1);
analogWrite(10, v);
}
else
{
analogWrite(10, 0);
}
}
void setup()
{
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);
pinMode(11, OUTPUT);
digitalWrite(6, 0);
digitalWrite(7, 1);
digitalWrite(8, 1);
digitalWrite(9, 0);
analogWrite(10, 0);
analogWrite(11, 0);
Serial.begin(115200);
//Serial.begin(9600);
Wire.begin();
delay(100);
Serial.println("Initializing I2C devices...");
mpu.initialize();
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed!!!!!!");
delay(2);
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
if (devStatus == 0)
{
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println("DMP ready! Waiting for first interrupt...");
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
Serial.print("DMP Initialization failed !!!!!!(code ");
Serial.print(devStatus);
Serial.println(")");
}
Setpoint = balance_angle; // gogal value
Setpoints = 0.0; // gogal speed
myPID.SetTunings(kp, ki, kd);
myPID.SetOutputLimits(-245 + dl, 245 - dl);
myPID.SetSampleTime(5); // PID algorithm will evaluate each 5ms
myPID.SetMode(AUTOMATIC);
sPID.SetTunings(sp, si, sd);
sPID.SetOutputLimits(-60, 60);
sPID.SetSampleTime(200);
sPID.SetMode(AUTOMATIC);
attachInterrupt(1, speed, RISING); // 设置中断
}
void loop()
{
if (!dmpReady) return; // wait for MPU interrupt or extra packet(s) available
if (!mpuInterrupt && fifoCount < packetSize) return;
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
mpu.resetFIFO();
}
else if (mpuIntStatus & 0x02)
{
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //从DMP中取出Yaw、Pitch、Roll三个轴的角度,放入数组ypr。单位:弧度
angle = -ypr[2] * 180 / M_PI; //提取Roll,转化为角度制
}
Inputs = speedcount;
sPID.Compute();
Setpoint = balance_angle - Outputs;
Input = angle;
myPID.Compute();
if (angle>40 || angle<-40)
Output = 0;
//////////////
flag = 0;
if (flag) // flag == 1 , 转弯
{
motort(Output);
flag = 0;
}
else {
motor(Output);
}
if (Serial.available() > 0) {
inByte = Serial.read();
}
if (inByte == 'w'){
kp += 0.5;
}
else if (inByte == 'q'){
kp -= 0.5;
}
else if (inByte == 'r'){
ki += 10;
}
else if (inByte == 'e'){
ki -= 10;
}
else if (inByte == 'y'){
kd += 0.01;
}
else if (inByte == 't'){
kd -= 0.01;
}
else if (inByte == 'i'){
dl += 1;
}
else if (inByte == 'u'){
dl -= 1;
}
else if (inByte == 's'){
sp += 0.1;
}
else if (inByte == 'a'){
sp -= 0.1;
}
else if (inByte == 'f'){
si += 1;
}
else if (inByte == 'd'){
si -= 1;
}
else if (inByte == 'h'){
sd += 0.01;
}
else if (inByte == 'g'){
sd -= 0.01;
}
else if (inByte == 'v'){
Setpoints += 2;
}
else if (inByte == 'b'){
Setpoints -= 2;
}
else if (inByte == 'n'){ //转弯??
Setpoints += 2;
flag = 1;
}
else if (inByte == 'm'){ //转弯??
Setpoints -= 2;
flag = 1;
}
inByte = 'x';
sPID.SetTunings(sp, si, sd);
myPID.SetTunings(kp, ki, kd);
num++;
if (num == 20)
{
num = 0;
Serial.print("Kp: ");
Serial.print(kp);
Serial.print(",i: ");
Serial.print(ki);
Serial.print(",d: ");
Serial.print(kd);
Serial.print(",dl: ");
Serial.print(dl);
Serial.print(" Sp: ");
Serial.print(sp);
Serial.print(",i:");
Serial.print(si);
Serial.print(",d:");
Serial.print(sd);
Serial.print(", angle:");
Serial.println(angle);
Serial.print("Output: ");
Serial.print(Output);
Serial.print( " Outputs: ");
Serial.println(Outputs);
}
}
void speed()
{
if (digitalRead(6)){
speedcount += 5;
}
else
speedcount -= 5;
}