求助,求助自平衡小车的问题
#include <PID_v1.h>/*陀螺仪使用的定义变量*/
unsigned char Re_buf,counter=0;
unsigned char sign=0;
/*电机使用定义的变量*/
int motor_a1=4;
int motor_a2=5;
int motor_b1=6;
int motor_b2=7;
int ena=10;
int enb=11;
/*定义PID库使用的参数*/
double Setpoint, Input, Output;
double kp,ki,kd;
/*定义PID程序*/
PID myPID(&Input, &Output, &Setpoint,kp,ki,kd, DIRECT);
/*初始化*/
void setup() {
Serial.begin(115200); //初始化串口速率
/*配置电机针脚为输出*/
pinMode(motor_a1,OUTPUT);
pinMode(motor_a2,OUTPUT);
pinMode(motor_b1,OUTPUT);
pinMode(motor_b2,OUTPUT);
myPID.SetMode(AUTOMATIC); //配置PID为自动模式
myPID.SetSampleTime(100); //配置PID采样率为100毫秒
Setpoint = -1; //设置小车平衡点角度
}
/*主函数*/
void loop() {
if (sign==0)
return; //sign为数据更新标志,每隔10ms更新一次,也就是说以下代码每隔10ms控制一次
sign=0;
//getkpkikd(&kp,&ki,&kd); //获取kp,ki,kd的值
kp=(double)analogRead(0)/1024*200;
ki=(double)analogRead(1)/10240;
kd=(double)analogRead(2)/1024*400;
myPID.Compute(); //PID运算
SetMotor(Output); //控制电机
//cout2serial() ; //串口输出需要查看的数据
Serial.print("angle:");
Serial.print(Input);Serial.print(" ");
Serial.print("output:");
Serial.print(Output);Serial.print(" ");
Serial.print("kp:");
Serial.print(kp);Serial.print(" ");
Serial.print("ki:");
Serial.print(ki);Serial.print(" ");
Serial.print("kd:");
Serial.print(kd);Serial.println(" ");
}
/*获取kp,ki,kd的值的函数*/
/*void getkpkikd(&kp,&ki,&kd){
kp=(double)analogRead(0)/1024*200;
ki=(double)analogRead(1)/10240;
kd=(double)analogRead(2)/1024*400;
}*/
/*控制电机运行的函数*/
void SetMotor(float Output)
{
if (Output>=0){
pinMode(motor_a1,HIGH);
pinMode(motor_a2,LOW);
pinMode(motor_b1,HIGH);
pinMode(motor_b2,LOW);
analogWrite(ena,Output);
analogWrite(enb,Output);
}
else if (Output<0) {
pinMode(motor_a1,LOW);
pinMode(motor_a2,HIGH);
pinMode(motor_b1,LOW);
pinMode(motor_b2,HIGH);
analogWrite(ena,abs(Output));
analogWrite(enb,abs(Output));
}
}
/*输出串口数据的函数*/
/*void cout2serial() {
Serial.print("angle:");
Serial.print(Input);Serial.print(" ");
Serial.print("output:");
Serial.print(Output);Serial.print(" ");
Serial.print("kp:");
Serial.print(kp);Serial.print(" ");
Serial.print("ki:");
Serial.print(ki);Serial.print(" ");
Serial.print("kd:");
Serial.print(kd);Serial.println(" ");
}*/
void serialEvent()
{
while (Serial.available()) {
Re_buf=(unsigned char)Serial.read();
if(counter==0&&Re_buf!=0x55) return; //第0号数据不是帧头
counter++;
if(counter==11) //接收到11个数据
{
counter=0; //重新赋值,准备下一帧数据的接收
switch(Re_buf )
{
case 0x51:
break;
case 0x52:
break;
case 0x53:
Input= double(short(Re_buf <<8| Re_buf ))/32768*180;
sign=1;
break;
}
}
}
}不知道为什么Output 为零 本帖最后由 immortal 于 2015-4-18 23:16 编辑
/**********************************************************************************************
* Arduino PID Library - Version 1.0.1
* by Brett Beauregard <[email protected]> brettbeauregard.com
*
* This Library is licensed under a GPLv3 License
**********************************************************************************************/
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <PID_v1.h>
/*Constructor (...)*********************************************************
* The parameters specified here are those for for which we can't set up
* reliable defaults, so we need to have the user set them.
***************************************************************************/
PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int ControllerDirection)
{
myOutput = Output;
myInput = Input;
mySetpoint = Setpoint;
inAuto = false;
PID::SetOutputLimits(0, 255); //default output limit corresponds to
//the arduino pwm limits
SampleTime = 100; //default Controller Sample Time is 0.1 seconds
PID::SetControllerDirection(ControllerDirection);
PID::SetTunings(Kp, Ki, Kd);
lastTime = millis()-SampleTime;
}
/* Compute() **********************************************************************
* This, as they say, is where the magic happens.this function should be called
* every time "void loop()" executes.the function will decide for itself whether a new
* pid Output needs to be computed.returns true when the output is computed,
* false when nothing has been done.
**********************************************************************************/
bool PID::Compute()
{
if(!inAuto) return false;
unsigned long now = millis();
unsigned long timeChange = (now - lastTime);
if(timeChange>=SampleTime)
{
/*Compute all the working error variables*/
double input = *myInput;
double error = *mySetpoint - input;
ITerm+= (ki * error);
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
double dInput = (input - lastInput);
/*Compute PID Output*/
double output = kp * error + ITerm- kd * dInput;
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
*myOutput = output;
/*Remember some variables for next time*/
lastInput = input;
lastTime = now;
return true;
}
else return false;
}
/* SetTunings(...)*************************************************************
* This function allows the controller's dynamic performance to be adjusted.
* it's called automatically from the constructor, but tunings can also
* be adjusted on the fly during normal operation
******************************************************************************/
void PID::SetTunings(double Kp, double Ki, double Kd)
{
if (Kp<0 || Ki<0 || Kd<0) return;
dispKp = Kp; dispKi = Ki; dispKd = Kd;
double SampleTimeInSec = ((double)SampleTime)/1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;
if(controllerDirection ==REVERSE)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
}
/* SetSampleTime(...) *********************************************************
* sets the period, in Milliseconds, at which the calculation is performed
******************************************************************************/
void PID::SetSampleTime(int NewSampleTime)
{
if (NewSampleTime > 0)
{
double ratio= (double)NewSampleTime
/ (double)SampleTime;
ki *= ratio;
kd /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}
/* SetOutputLimits(...)****************************************************
* This function will be used far more often than SetInputLimits.while
*the input to the controller will generally be in the 0-1023 range (which is
*the default already,)the output will be a little different.maybe they'll
*be doing a time window and will need 0-8000 or something.or maybe they'll
*want to clamp it from 0-125.who knows.at any rate, that can all be done
*here.
**************************************************************************/
void PID::SetOutputLimits(double Min, double Max)
{
if(Min >= Max) return;
outMin = Min;
outMax = Max;
if(inAuto)
{
if(*myOutput > outMax) *myOutput = outMax;
else if(*myOutput < outMin) *myOutput = outMin;
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
}
}
/* SetMode(...)****************************************************************
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
* when the transition from manual to auto occurs, the controller is
* automatically initialized
******************************************************************************/
void PID::SetMode(int Mode)
{
bool newAuto = (Mode == AUTOMATIC);
if(newAuto == !inAuto)
{/*we just went from manual to auto*/
PID::Initialize();
}
inAuto = newAuto;
}
/* Initialize()****************************************************************
* does all the things that need to happen to ensure a bumpless transfer
*from manual to automatic mode.
******************************************************************************/
void PID::Initialize()
{
ITerm = *myOutput;
lastInput = *myInput;
if(ITerm > outMax) ITerm = outMax;
else if(ITerm < outMin) ITerm = outMin;
}
/* SetControllerDirection(...)*************************************************
* The PID will either be connected to a DIRECT acting process (+Output leads
* to +Input) or a REVERSE acting process(+Output leads to -Input.)we need to
* know which one, because otherwise we may increase the output when we should
* be decreasing.This is called from the constructor.
******************************************************************************/
void PID::SetControllerDirection(int Direction)
{
if(inAuto && Direction !=controllerDirection)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
controllerDirection = Direction;
}
/* Status Funcions*************************************************************
* Just because you set the Kp=-1 doesn't mean it actually happened.these
* functions query the internal state of the PID.they're here for display
* purposes.this are the functions the PID Front-end uses for example
******************************************************************************/
double PID::GetKp(){ returndispKp; }
double PID::GetKi(){ returndispKi;}
double PID::GetKd(){ returndispKd;}
int PID::GetMode(){ returninAuto ? AUTOMATIC : MANUAL;}
int PID::GetDirection(){ return controllerDirection;}
immortal 发表于 2015-4-18 23:19 static/image/common/back.gif
这是PID库函数 不懂!可能是PId问题 没看见你的PID调节值 suoma 发表于 2015-4-19 18:30 static/image/common/back.gif
没看见你的PID调节值
我不太懂你的意思,PID调节值的相应程序呢,该怎样改呢。谢谢
页:
[1]