智能车制作

标题: s12电磁组舵机pid怎么写啊 [打印本页]

作者: △ABC≌△DEF 时间: 2014-4-25 16:38
标题: s12电磁组舵机pid怎么写啊
电磁组的pid算法怎么写啊，苦恼了很久了，想不出写的办法来呢，以下是自己的的程序，请指教
/////////////////////////////////////
////    电机 PID  定义
/////////////////////////////////////

int SetPoint=0; //设定目标 Desired Value
int  FeedBack=0;
float KKp=1.5; //比例常数 Proportional Const
float KKi=0;
float KKd=0.6; //微分常数 Derivative Const
signed int EE0=0;  //当前误差
signed int EE1=0; //Error[-1]
signed int EE2=0; //Error[-2]

signed int EError0=0,EError1=0;
signed int iiIncpid=0;
int sp=0;

///////////////////////////////
/// 舵机 PID
//////////////////////////////
//////////////////////////////////////
int duoji_pid(int ch1,int ch2 )

{
E0=ch1-ch2; //增量计算
Error0=E0-E1;
Error1=E1-E2;
iIncpid=(int)(Ki*Error0+Kp*E0+Kd*(Error0-Error1));
E1=E0;
E2=E1; //存储误差，用于下次计算
return iIncpid;
}

//-----PWM 初始化程序------//
void PWM_Init(void)
{
  PWME = 0x00;          //禁止
  PWMCTL = 0x70;          // no concatenation             控制寄存器设置01、23、45级联。
  PWMPRCLK = 0x00;       // 0000 0000 A = B = 24M       时钟预分频寄存器设置
  PWMSCLA = 12;          // SA = A/2/12 = 1000k          时钟设置
  PWMSCLB = 12;          // SB = B/2/12 = 1000K
  PWMCLK_PCLK1 = 1;       // PWM01-----SA             时钟源的选择
  PWMCLK_PCLK5 = 1;       // PWM45-----SA             时钟源的选择
  PWMCLK_PCLK3 = 1;       // PWM23-----SB             时钟源的选择
  PWMPOL_PPOL1 = 1;       // Duty=High Time             极性设置
  PWMPOL_PPOL5 = 1;       // Duty=High Time             极性设置
  PWMPOL_PPOL3 = 1;       // Duty=High Time             极性设置
  PWMCAE_CAE1 = 0;       // Left-aligned                对齐方式设置
  PWMCAE_CAE5 = 0;       // Left-aligned                对齐方式设置
  PWMCAE_CAE3 = 0;       // Left-aligned                对齐方式设置
  PWMPER01 = 20000;       //周期20ms    50HZ
  PWMPER45 = 1000;       //Frequency=SA/1000=1K          周期寄存器设置
  PWMPER23 = 1000;
  PWMDTY01 = 1580;          //占空比寄存器设置
  PWMDTY45 = 600;
  PWMDTY23 = 600;
  PWME = 0x3f;          //使能
}

//-----速度、角度控制-----//
uint jiaodu_ctrl(void)
{
  uint jiaodu;

  jiaodu=1580-duoji_pid(ATD0DR1L+ATD0DR2L,ATD0DR1L+ATD0DR2L) ;
  if (jiaodu>1780) jiaodu=1780;
  if (jiaodu<1380) jiaodu=1380;
  return jiaodu;
}

不知道这样可否大神求指教

作者: △ABC≌△DEF 时间: 2014-4-25 17:09
？？？？求助啊

欢迎光临智能车制作 (http://111.231.132.190/)