|
|
为适应四轴发展需要，我要从程序，从电路做起，有我们自己的代码，为此，写了下面的东东，希望众多高手支持、赐教
硬件：mega8+enc03+H桥
软件：我们坛子里的“标准的PID例程”，当小车朝一个方向偏离平衡点时，电机将反转以矫正小车的倾斜。电机控制没有用PWM，直接是通断信号
实验结果：抖动严重，并且只能平衡很短时间，小车会慢慢倒下。另外，比例p=1，I=1的效果与P=10，I=10的效果差不多一样
源程序：
#include <iom8v.h>
#include <macros.h>
#include "main.h"
#include "init.h"
unsigned int a=0,b=0;
signed int ad=0,adZ=0;
/*由于单片机的处理速度和ram资源的限制，一般不采用浮点数运算，而将所有参数全部用整数，运算到最后再除以一个2的N次方数据（相当于移位），
作类似定点数运算，可大大提高运算速度，根据控制精度的不同要求，当精度要求很高时，注意保留移位引起的"余数"，做好余数补偿。*/
#include <string.h>
#include <stdio.h>
/*====================================================================================================
PID Function
The PID (比例、积分、微分) function is used in mainly
control applications. PIDCalc performs one iteration of the PID
algorithm.
While the PID function works, main is just a dummy program showing
a typical usage.
=====================================================================================================*/
typedef struct PID {
double SetPoint; // 设定目标 Desired Value
double Proportion; // 比例常数 Proportional Const
double Integral; // 积分常数 Integral Const
double Derivative; // 微分常数 Derivative Const
double LastError; // Error[-1]
double PrevError; // Error[-2]
double SumError; // Sums of Errors
} PID;
/*====================================================================================================
PID计算部分
=====================================================================================================*/
double PIDCalc( PID *pp, double NextPoint )
{
double dError, Error;
Error = pp->SetPoint - NextPoint; // 偏差
pp->SumError += Error; // 积分
dError = pp->LastError - pp->PrevError; // 当前微分
pp->PrevError = pp->LastError;
pp->LastError = Error;
return (pp->Proportion * Error // 比例项
+ pp->Integral * pp->SumError // 积分项
+ pp->Derivative * dError // 微分项
);
}
/*====================================================================================================
Initialize PID Structure
=====================================================================================================*/
void PIDInit (PID *pp)
{
memset ( pp,0,sizeof(PID));
}
/*====================================================================================================
Main Program
=====================================================================================================*/
double sensor (void) // Dummy Sensor Function
{
for(a=0;a<60;a++)
{
ad+=read_adc(0);//-adZ;
}
ad/=60;
//ad-=read_adc(1);
return ad;
}
void actuator(double rDelta) // Dummy Actuator Function
{
if(ad>-10)
{
PORTB|=BIT(1);
PORTB&=~BIT(2);
PORTB|=BIT(7);
PORTB&=~BIT(6);
}
else if(ad<-15)
{
PORTB|=BIT(2);
PORTB&=~BIT(1);
PORTB&=~BIT(7);
PORTB&=~BIT(6);
}
else
{
PORTB&=~BIT(1);
PORTB&=~BIT(2);
PORTB|=BIT(6);
}
ad=0;
}
int main(void)
{
PID sPID; // PID Control Structure
double rOut; // PID Response (Output)
double rIn; // PID Feedback (Input)
init_devices();
PORTB|=BIT(6);
adZ=read_adc(0);
for(a=0;a<20;a++)
for(b=0;b<65535;b++); //soft time delay,and waiting for the power stable
PIDInit ( &sPID ); // Initialize Structure
sPID.Proportion = 1; // Set PID Coefficients
sPID.Integral = 1;
sPID.Derivative = 0;
sPID.SetPoint =934;//660;//read_adc(1);//934; // Set PID Setpoint
while(1)
{
rIn = sensor (); // Read Input
rOut = PIDCalc ( &sPID,rIn ); // Perform PID Interation
actuator ( rOut ); // Effect Needed Changes
}
return 0;
} |
阿莫论坛20周年了！感谢大家的支持与爱护！！
一只鸟敢站在脆弱的枝条上歇脚，它依仗的不是枝条不会断，而是自己有翅膀，会飞。
|
发表于 2012-2-24 21:09:25
楼主
