求助PID输出恒为0
求助，我刚开始用PID，借鉴论坛里面的程序写了个用角度传感器来控制小车走直线和斜线的PID，但发现输出恒为0，求指点一二。/*
Name: Sketch_work.ino
Created: 2018/5/5 13:48:51
Author: Qi
*/
#include <FlexiTimer2.h>
#include <Servo.h>
#include <PID_v1.h>
Servo Vacum_c;//定义真空控制单位
#define Red1 26//定义红外传感器数字接口
#define Red2 28
#define Red3 30
#define Red4 32
#define L1 2 //定义L298N驱动板数字接口
#define L2 3
#define R1 4
#define R2 5
#define Vacum 6//真空泵数字接口
#define Vo A0 //角度传感器模拟接口
double setpointF=420, setpointL=480, setpointR=340;//设置直线行走角度设定值，左侧行走角度设定值，右侧行走角度设定值
double outptF, outputL, outputR;//设置直线行走输出值,左侧行走输出值
double Kp = 4, Ki = 0, Kd = 0;//设置PID常量
double inputF, inputL, inputR;//设置直线行走时候角度角度输入值，左侧行走角度输入值，右侧行走角度输入值
int pwmR = 220;//设置右侧电机固定PWM值
int angle; //角度读取模拟值
PID PIDF(&inputF, &outptF, &setpointF, Kp, Ki,Kd, DIRECT);//设置直线行驶PID
PID PIDL(&inputL, &outputL, &setpointL, Kp, Ki, Kd,DIRECT);//设置左侧行驶PID
PID PIDR(&inputR, &outputR, &setpointR, Kp, Ki, Kd, DIRECT);//设置右侧行驶PID
void setup() {
Serial.begin(9600);
Vacum_c.attach(Vacum, 500, 2500);
Vacum_c.writeMicroseconds(800);
delay(1000);
Vacum_c.writeMicroseconds(2000);
foword();
pinMode(Red1, INPUT);
pinMode(Red2, INPUT);
pinMode(Red3, INPUT);
pinMode(Red4, INPUT);
pinMode(Vo, INPUT);
//FlexiTimer2::set(200, Rangle);
//FlexiTimer2::start();
PIDF.SetSampleTime(200);
PIDF.SetOutputLimits(0, 255);
PIDF.SetMode(AUTOMATIC);
PIDL.SetSampleTime(200);
PIDL.SetOutputLimits(0, 255);
PIDL.SetMode(AUTOMATIC);
PIDR.SetSampleTime(200);
PIDR.SetOutputLimits(0, 255);
PIDR.SetMode(AUTOMATIC);
}
void Rangle()
{
angle = analogRead(Vo);
inputF = inputL = inputR = angle;
}
void foword() //直线前进
{
setpointF = 420;
PIDF.Compute();
digitalWrite(L1, outptF);
digitalWrite(L2, LOW);
digitalWrite(R1, pwmR);
digitalWrite(R2, LOW);
}
void reverse()//直线后退
{
setpointF = 420;
PIDF.Compute();
digitalWrite(L1, LOW);
digitalWrite(L2, outptF);
digitalWrite(R1, LOW);
digitalWrite(R2, pwmR);
}
void brake()//停止
{
digitalWrite(L1, LOW);
digitalWrite(L2, LOW);
digitalWrite(R1, LOW);
digitalWrite(R2, LOW);
}
void upturnR()//上方右转
{
do{
digitalWrite(L1, HIGH);
digitalWrite(L2, LOW);
digitalWrite(R1, LOW);
digitalWrite(R2, HIGH);
} while (angle > 340);
}
void upturnL()//上方左转
{
do{
digitalWrite(L1, LOW);
digitalWrite(L2, HIGH);
digitalWrite(R1, HIGH);
digitalWrite(R2, LOW);
} while (angle<480);
}
void dow_turnL()//下方右转
{
do{
digitalWrite(L1, LOW);
digitalWrite(L2, HIGH);
digitalWrite(R1, HIGH);
digitalWrite(R2, LOW);
} while (angle<480);
}
void dow_turnR() //下方左转
{
do{
digitalWrite(L1, LOW);
digitalWrite(L2, HIGH);
digitalWrite(R1, HIGH);
digitalWrite(R2, LOW);
} while (angle > 340);
}
void line_L()//左侧直线
{
setpointL = 345;
PIDL.Compute();
digitalWrite(L1, LOW);
digitalWrite(L2, outputL);
digitalWrite(R1, LOW);
digitalWrite(R2, pwmR);
}
void line_R()//右侧直线
{
setpointR = 475;
PIDR.Compute();
digitalWrite(L1, LOW);
digitalWrite(L2, outputR);
digitalWrite(R1, LOW);
digitalWrite(R2, pwmR);
}
void loop() {
Rangle();
boolean I1, I2, I3, I4;
I1 = digitalRead(Red1);
I2 = digitalRead(Red2);
I3 = digitalRead(Red3);
I4 = digitalRead(Red4);
if (I1 == 1)
{
brake();
delay(30);
reverse();
delay(500);
upturnR();
line_L();
}
if (I2 == 1)
{
brake();
delay(50);
reverse();
delay(500);
upturnR();
line_L();
}
if (I3 == 1)
{
brake();
delay(50);
foword();
delay(500);
//dow_turnL();
foword();
}
if (I4 == 1)
{
brake();
delay(50);
foword();
delay(500);
//dow_turnR();
foword();
}
if (I1 = 1 && I2 == 1)
{
brake();
delay(50);
reverse();
delay(500);
upturnR();
line_L();
}
if (I3 == 1 && I4 == 1)
{
brake();
delay(50);
foword();
}
Serial.print("inputF=");
Serial.println(inputF);
Serial.print("angle=");
Serial.println(angle);
Serial.print("outputF=");
Serial.println(outptF);
}
页:
[1]