智能车制作

标题: 跪求PID控制程序的注释 [打印本页]

作者: lixiangdada 时间: 2012-3-9 11:06
标题: 跪求PID控制程序的注释
求frespeed、prespeed、freservospeed、presservopeed、preserved、temppwm、Speed代表的含义
以及速度和舵机控制分别用了那种具体的PID算法
衷心感谢！！！

#include "Judgement.H"
#include <math.h>
unsigned short PWM;
#define maxpwm 18000
int count =0;
int tempmid=0;
unsigned short frespeed=0;
unsigned short prespeed=0;
unsigned short freservospeed=0;
unsigned short presservopeed=0;
unsigned short preserved[80]={0};
int mortorPwm[16]={16000,13000,11000,9000,8000,6000,6000,6000,6000,6000,8000,6000,4000,6000,4000,3000};
extern "C" _declspec(dllexport)
int speedPid(int temppwm,int frespeed,int Speed,int prespeed)
{
double retVal=10.9*Speed+17.02*(temppwm-10.9*Speed)+9*10.9*(Speed-frespeed)+10.9*(frespeed-prespeed);
if (retVal>65535)
  retVal=65535;
if (retVal<0)
  retVal=0;
return retVal;
return retVal;
}
int servoPid(int temppwm,int frespeed,int prespeed)
{
double retVal=temppwm+0.8*(temppwm-frespeed)+0.32*(frespeed-prespeed);
  if (retVal>65535)
  retVal=65535;
if (retVal<0)
  retVal=0;
return retVal;
}
void Judge( unsigned short Speed,
unsigned char* SensorData, unsigned short SensorCount,
unsigned char* CCDData, int CCDWidth, int CCDHeight,
unsigned short* MotorPWM, unsigned short* SteerPWM)
{
/*PWM += 1;
*MotorPWM = 0;
*SteerPWM = 16384;*/
int i=0;
int j=0;
int k=0;
int m=0;
double mid=0;
int loop=0;
double avgpwm=0;
double interlapce=0;
double angle=0;
double temppwm;
double linek=0;
double lineb=0;
double dkd=0;
double offset=0;
  for(i=0;i<CCDWidth*CCDHeight;i++)
{
  if(CCDData[i]==0&&CCDData[i-1]==255)
j=i;
  if(CCDData[i]==0&&CCDData[i+1]==255)
k=i;
  m=j/32;
  j=j%32;
  k=k%32;
  mid=(j+k)/2+0.5;
  preserved[m]=(unsigned short)mid;
}
linek=(preserved[79]-preserved[0])/79;
lineb=preserved[0];
dkd=sqrt(linek*linek+1);
for(i=0;i<80;i++)
{
  offset+=(preserved[i]-linek*i-lineb)/dkd;
}
for(i=CCDWidth*CCDHeight;i>0;i--)
{
  if(CCDData[i]==0&&CCDData[i-1]==255)
j=i;
  if(CCDData[i]==0&&CCDData[i+1]==255)
k=i;
}
m=j/32;
j=j%32;
k=k%32;
mid=(int)((j+k)/2);
if(j==0)
{
  /*if(frePWM[count]>0)
temppwm=32768*(1-frePWM[count]/(3.14/6));
  else
temppwm=32768*(1+frePWM[count]/(3.14/6));
  *SteerPWM=temppwm;
  *MotorPWM=500;*/
  return;
}
*MotorPWM=18000;
if((int)mid==CCDWidth/2)
{
  prespeed=Speed;
  frespeed=Speed;
  *MotorPWM=speedPid(18000,frespeed,Speed,prespeed);
  *SteerPWM=32768;
}
else
{

  interlapce=(CCDWidth/2-mid);

  temppwm=32768+65535/32*interlapce;
  if(temppwm>65535)
temppwm=65535;
  else if(temppwm<0)
temppwm=0;

  *SteerPWM=servoPid(temppwm,freservospeed,presservopeed);
  presservopeed=freservospeed;
  freservospeed=*SteerPWM;

  int interspeed=0;
  if(interlapce>0)
interspeed=16-interlapce;
  else
interspeed=16+interlapce;
  temppwm=mortorPwm[abs(interlapce)];

  *MotorPWM=speedPid(temppwm,frespeed,Speed,prespeed);
  prespeed=frespeed;
  frespeed=Speed;
}
}
extern "C" _declspec(dllexport)
void Reset()
{
PWM = 0;
}

作者: lixiangdada 时间: 2012-3-12 08:20
各位大哥大姐，拜托啦

作者: ily_M_T_I_C_S 时间: 2012-3-12 16:40
我也想要 709733624@qq.com 我的邮箱麻烦大神给我发分

作者: jun33 时间: 2012-3-13 16:32
哪位大神有也给小弟一份呗
jun198933@163.com

作者: usstwhw 时间: 2012-3-23 16:47
菜鸟求学

作者: usstwhw 时间: 2012-3-23 16:47
740126966 饿一份

作者: 借你一生 时间: 2012-4-9 22:02
#include <hidef.h> /* common defines and macros */
#include <MC9S12XS128.h> /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
//==================================================
//定时中断函数
//定时中断0 实现500MS 定时
//author:yangtze
//time:2009/5/2
//==================================================
#define PITTIME 5000//设定为50MS 定时
unsigned int vTmpPIT=0;
void pllclk(void)//24MHz，外部时钟为16MHz
{
CLKSEL=0X00;
PLLCTL=0xe1;
SYNR=2;//PLLCLK =2*OSCCLK*(SYNR + 1)/(REFDV + 1)
REFDV=1;
PLLCTL=0X60;
asm NOP;
asm NOP;
asm NOP;
while((CRGFLG&0X08)==0);//时钟校正同步
CLKSEL=0X80;
}
void initIOBoutput(void)//IO 口初始化，B 口为输出
{
DDRB=0XFF;
PORTB=0XFF;
}
void initPIT(void)//定时中断初始化函数50MS 定时中断设置
{
PITCFLMT_PITE=0; //定时中断通道0 关
PITCE_PCE0=1;//定时器通道0 使能
PITMTLD0=240-1;//8 位定时器初值设定。240 分频，在24MHzBusClock 下，为0.1MHz。
即
10us.
PITLD0=PITTIME-1;//16 位定时器初值设定。PITTIME*0.01MS
PITINTE_PINTE0=1;//定时器中断通道0 中断使能
PITCFLMT_PITE=1;//定时器通道0 使能
}
void main(void)
{
pllclk();
initIOBoutput();
initPIT();
EnableInterrupts;
for(;;) {} /* wait forever */
/* please make sure that you never leave this function */
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 PIT0(void)
{
vTmpPIT++;
if(vTmpPIT==10)
{
PORTB=~PORTB;//输出取反
vTmpPIT=0;
}
PITTF_PTF0=1;//清中断标志位
}

作者: songcol 时间: 2012-4-10 13:35
这都是啥，这么乱

作者: xufang 时间: 2012-4-12 13:43
看看

作者: naruto7 时间: 2012-4-12 16:56
各种寄存器啊，还穿插汇编

作者: 精晶 时间: 2012-5-18 20:08
这是什么啊？哪是PID

作者: banguiz 时间: 2013-4-9 16:17
偶也想知道frespeed、prespeed等的注释，求解啦

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