智能车制作

标题: 舵机用PD速度用PID，能让车子跑多快？ [打印本页]

作者: 流水三千 时间: 2010-6-5 22:04
标题: 舵机用PD速度用PID，能让车子跑多快？
帮我看看下面的程序舵机和速度分别采用的是什么算法？是位置式？or增量式？
程序很长，请看附件

作者: 流水三千 时间: 2010-6-5 22:06
#include <hidef.h>    /* common defines and macros */
#include <MC9S12XS128.h>    / * derivative information */
#pragma  LI NK_INFO DERIVATIVE "mc9s12xs128"
#pragma  CODE_ SEG __NEAR_SEG NON_BANKED
//*********************宏定义*******************//
#define DLT1    21 20000
#define DLT2    15 0
#define INTETIME  1536       // 中断时间常数
#define N       10
#define NSP    11
#define MAXANGLE  1 560       // 舵机最大右偏角对应的PWM 值
#define MINANGLE  1 220       // 舵机最大左偏角对应的PWM 值
#define P1       3          //舵机PD 控制P 常量
#define D1       7          //舵机PD 控制D 常量
#define P2       80          //速度PD 控制P 常量
#define I2       5
#define D2       60          //速度PD 控制D 常量
#define MAXSUME 600       / /积分饱和上限
#define MINSUME -60 0       // 积分饱和下限
//********************定义结构体*****************//
struct PIDROUTE                //路径控制结构体
{ int derror;                   //微分变量
  int sume;                      //积分变量
}; struct PIDV                      //速度控制结构体
{
  float  set;                   //设定值
  float  now;                   //当前值
  float  error;                //当前偏差
  float  laste;                //上次偏差
  float  derror;                //微分变量
  float  sume;                   //积分变量
};
struct str1                      //路径信息结构体1
{
  int rout_ero;                //路径偏差
  unsigned char sensors,change;
}; struct str2                      //路径信息结构体1
{ unsigned char change,blarnum;
  unsi gned char area[5][3];
  unsi gned char blmiddle;
}; struct filter                      //滤波结构体
{ unsi gned char senava[12];
I

  unsigned char blamef,blnmef,blmef;
}; struct speed                         //速度结构体
{ unsi gned int vwl,vwb,vwo;
  unsigned int vnl,vnb,vno;
}; //*******************定义全局变量*****************//
unsigned char state=0;
unsigned char sensor19,sensor0;
unsigned char bend=100;
unsigned char inte=0;
unsigned int  stopt=0;
unsigned char stop=0;
unsigned int  eng=0;
unsigned char startln;
unsigned char enterln=0;          //识别进直道延时
unsigned char enterna,enterwi;    //识别进窄道、宽道延时
unsigned char chantim=0,chanti=0;
unsigned char spn1=0,spn2;
struct filter sensta;             //定义滤波结构体变量
struct PIDROUTE routec;
unsigned char rout_o[12];          //路径原始信息
int arr0[12]={-60,-50,-45,-30,-10,-5,5,10,30,45,50,60};
unsigned int rou_out;             //舵机PWM 输出
struct PIDV speedc;
float  sp_out=0;
unsigned int v_arr[NSP];
struct speed spsel[7];
struct speed spset;                //设定速度
void MCUInit(void)
{    CLKSEL &= 0X7F;             //暂选OSCCLK 为系统时钟源
PLLCTL &= 0XBF;
SYNR = 0X01;                //设定倍频因子
REFDV = 0X00;
PLLCTL |= (1<<6);             //打开PLL 电路
w hile ((CRGFLG&0X08) == 0X00); //等待PLLCLK 稳定
CLKSEL |= (1<<7);
COPCTL = 0X00;                //关闭看门狗
} //******************IO 端口初始化*******************//
void IOInit(void)
{    DDRA =0X00;                   //设置A 口为输入口  读光电管信息
DDRH =0X00;
DDRB =0X00;
PUCR |=0X02;                //设置B 口上拉
PTM &=0XF8;                   //M 口低三位初始为低
PTP |=(1<<5); //***       //P 口第五位初始为高

} //******************PWM 模块初始化******************//
void PWMInit(void)
{
PWMCTL |=0XB0;
PWMPRCLK =0X05;
PWMCLK =0X00;    //分频因子
PWMCAE &=0X75;    //PWM 输出左对齐
PWMPER01 =20000;
PWMDTY01 =1390;    //设定01 通道初始占空比摆正舵机
PWMPER23 =3200;
PWMDTY23 =0;       //设定23 通道初始占空比
PWMPER67 =3200;
PWMDTY67 =0;       //设定67 通道初始占空比
PWME |=0X8A;       //开启所选PWM 通道
}

作者: 流水三千 时间: 2010-6-5 22:07
//*****************定时计数模块初始化**************//
void TIMERInit(void)
{    PACTL =0X50;
TSCR2=0X80;       //选择总线时钟，开启溢出中断
TSCR1=0X80;       //开启计数器
} //******************相关信息初始化*****************//
void DataInit(void)
{ un signed char i;
speedc.laste=0;
speedc.sume=0;
for(i=0;i<12;i++)
sensta.senava[i]=1;          //设置初始时光电管全有效

spsel[0].vwl=160;             //设置预选速度  下同
spsel[0].vwb=140;
spsel[0].vwo=90;
spsel[0].vnl=140;
spse l[0].vnb=100;
spse l[0].vno=80;

spsel[1].vwl=160;
spsel[1].vwb=140;
spsel[1].vwo=90;
spsel[1].vnl=140;
spse l[1].vnb=100;
spse l[1].vno=80;

spsel[2].vwl=210;
spsel[2].vwb=150;
spsel[2].vwo=1 10;
spsel[2].vnl=150;
spse l[2].vnb=110;
spse l[2].vno=80;


spsel[3].vwl=250;
spsel[3].vwb=150;
spsel[3].vwo=110;
spsel[3].vnl=160;
spse l[3].vnb=120;
spse l[3].vno=80;

spsel[4].vwl=270;
spsel[4].vwb=170;
spsel[4].vwo=1 10;
spsel[4].vnl=180;
spse l[4].vnb=120;
spse l[4].vno=80;

  } void kg()
{    un signed char b012=0;
b012 =PORTB;             //读B 口
b012 &=0X07;             //取低三位
if(PO RTB_PB3==1)
{
sw itch(b012)
{
   case 7:spset=spsel[3]; // 正常速度
         break;
   case 6:
   case 5:
   case 3:
   case 4:
   case 2:
   case 1:spset=spsel[1]; // 速度低二
         break;
   case 0:spset=spsel[0]; // 速度低三
         break;
}
}
  else
{
sw itch(b012)
{
   case 7:spset=spsel[3]; // 正常速度
         break;
   case 6:
   case 5:
   case 3:spset=spsel[4]; // 速度高一
         break;
   case 4:
   case 2:

   case 1:spset=spsel[5]; // 速度高二
         break;
   case 0:spset=spsel[6]; // 速度高三
         break;
}
}
} //*****************求绝对值函数*******************//
char absl(char i)
{
  if(i>0)    return i;
  else
return (-i);
} //*******************延迟函数**********************//
void delay(unsigned long int  a)
{
unsigned long int i;
for(i=a;i>0;i--);
}

作者: 流水三千 时间: 2010-6-5 22:07
//************路径信息采集及舵机控制函数***********//
void routectr()
{    u nsigned char i,j,pth,middle;
in t lj1;
in t temp1;
un signed int temp2;
//*路径信息采集*//
pt h=PTH;
{
PTM =0x01;                   //第一组光电管工作
delay(DLT2);                //延时
ro ut_o[0] =((pth &0X08)>>3);
ro ut_o[3] =PORTA_PA2;
ro ut_o[6] =((pth &0x04)>>2);
ro ut_o[9] =PORTA_PA3;
PTM =0x02;                   //第二组光电管工作
delay(DLT2);                //延时
ro ut_o[1] =((pth &0X02)>>1);
ro ut_o[4] =PORTA_PA4;
ro ut_o[7] =(pth &0x01);
ro ut_o[10]=PORTA_PA5;
PTM =0x04;                   //第三组光电管
delay(DLT2);                //延时
ro ut_o[2] =PORTA_PA0;
ro ut_o[5] =PORTA_PA6;
ro ut_o[8] =PORTA_PA1;
ro ut_o[11] =PORTA_PA7;
PTM=0x00;

} */
ro ut_o[0] =((pth &0X08)>>3);
ro ut_o[1] =((pth &0X02)>>1);
ro ut_o[2] =PORTA_PA0;
ro ut_o[3] =PORTA_PA2;
ro ut_o[4] =PORTA_PA4;
ro ut_o[5] =PORTA_PA6;
ro ut_o[6] =((pth &0X04)>>2);
ro ut_o[7] =(pth &0X01);
ro ut_o[8] =PORTA_PA1;
ro ut_o[9] =PORTA_PA3;
rout_o[10]=PORTA_PA5;
ro ut_o[11]=PORTA_PA7;

//*路径信息加工处理*//
for(i=N-1;i>0;i--)
   data1[i]=data1[i-1];             //平移结构体数组
for(i=0 ,data1[0].sensors=0,middle=0;i<12;i++)//计算sensors 值
{
if(rout_o[ i]==0)
{
   da ta1[0].sensors++;
   if(i>= 3&&i<=8)
   middle++;
}
}
for(i=0;i<5;i++)                   //计算data2 结构体变量有关值
   data2.area[i][0]=0;
da ta2.area[0][1]=0;
for(i=1 ,data2.change=0;i<12;i++)
{
if(rout_o[ i]!=rout_o[i-1])
{
   dat a2.area[data2.change][2]=i-1;
   data2.change++;
   data2.area[data2.change][1]=i;
}
}
da ta2.area[data2.change][2]=11;
data1[0].change=data2.change;
sw itch(data2.change)
{
c ase 0:if(rout_o[0]==0)
         {
         data2.blarnum=1;
         data2.blnum=12;
         data2.blmiddle=6;
         }
         else
         {
         data2.blarnum=0;
         data2.blnum=0;

         data2.blarnum=12;
         }
         break;
c ase 1:data2.blarnum=1;
         if(rout_o[0]==0)
         {
         data2.blnum=data2.area[0][0];
         data2.blmiddle=data2.area[0][2]>>1;
         }
         else
         {
         data2.blnum=data2.area[1][0];
         data2.blmiddle=(data2.area[1][1]+11)>>1;
         }
         break;
c ase 2:if(rout_o[0]==0)
         {
         data2.blarnum=2;
         if(data2.area[0][0]>data2.area[2][0])
            dat a2.blnum=data2.area[0][0];
         else
            dat a2.blnum=data2.area[2][0];
         }
         else
         {
         data2.blarnum=1;
         data2.blnum=data2.area[1][0];
         }
         break;
c ase 3:data2.blarnum=2;
         if(rout_o[0]==0)
         {
         if(data2.area[0][0]>data2.area[2][0])
            dat a2.blnum=data2.area[0][0];
         else
            dat a2.blnum=data2.area[2][0];
         }
         else
         {
         if(data2.area[1][0]>data2.area[3][0])
            dat a2.blnum=data2.area[1][0];
         else
            dat a2.blnum=data2.area[3][0];
         }
         break;
c ase 4:if(rout_o[0]==0)
         {
         data2.blarnum=3;
         if(data2.area[0][0]>data2.area[2][0])
            dat a2.blnum=data2.area[0][0];
         else
            dat a2.blnum=data2.area[2][0];

         if(data2.blnum<data2.area[4][0])
            dat a2.blnum=data2.area[4][0];
         }
         else
         {
         data2.blarnum=2;
         if(data2.area[1][0]>data2.area[3][0])
            dat a2.blnum=data2.area[1][0];
         else
            dat a2.blnum=data2.area[3][0];
         }
         break;
default:break;
}
{
if(c hanti<3)
   chanti+ +;
else
{
   chantim=20;
   chanti=0;
}
}
else
{
if(chantim>0)
   chantim--;
ch anti=0;
}

fo r(i=0,lj1=0,sensta.blamef=0,sensta.blnmef=0;i<12;i++) {
if(rout_o[ i]==0&&sensta.senava[i]==1)
{
   sensta.blamef++;
   se nsta.blnmef +=i;
   lj1 +=arr0[i];
}
}
if(senst a.blamef>0)
{
data1[0].rout_ero=(int)(lj1/sensta.blamef); //路径偏差
for(i=0;i<12;i++)
   sensta.senava[i]=0;
}
switch(sensta.blamef)                      //确定等效黑线
{
c ase 0:j=1;
         for(i=0;i<6;i++)

         if(j)
         sensta.blmef=data2.blmiddle;    //改变等效黑线位置
         break;

c ase 1:sensta.blmef=sensta.blnmef;
         break;
case 2:
case 4:sensta.blmef=(unsigned char)(sensta.blnmef/sensta.blamef);
         if(sensta.blmef<10)
         sensta.senava[sensta.blmef+2]=1;
         break;
case 3:sensta.blmef=(unsigned char)(sensta.blnmef/3);
         break;
default: break;
}
sensta.senava[sensta.blmef]=1;          //确定有效区域
if(sensta.bl mef>0)
   sensta.senava[sensta.blmef-1]=1;       //确定有效区域
if(sensta.bl mef<11)
   sens ta.senava[sensta.blmef+1]=1;       // 确定有效区域
   if(startln==1)                         //连续识别两次
   {
   if(state==0)                         //首次过起始线
   state=1;
   else if(state==7)                   //第二次过起始线
   state=5;
   }
   else
   startln=1;
}
else
{
   {
   if(en terna>=20&&sensor19>sensor0)
      state=3;
   else
   {
      if(enterwi==0)
         sensor1 9=data1[0].sensors;
      if(enterwi==19)
         sensor0=data1[0].sensors;
      enterna++;
   }
   }
   else
   {
      if(e nterwi>=20&&sensor19<sensor0)
      state=4;
      else
      {
      if(enterwi==0)
         sensor1 9=data1[0].sensors;
      if(enterwi==19)
         sensor0=data1[0].sensors;
      enterwi++;
      }


   }
}
}
else                //正常路段
{
   switch(state)
   {
   case 1:state=7; //首次过起始线后，进入宽道
         break;
   case 5:state=6; //第二次过起始线后，跑完一圈
         break;
   case 3:state=8; //过进窄道标志后，进入窄道
         wina=1;
         break;
   case 4:state=7; //过出窄道标志后，进入宽道
         wina=0;
         break;
   default:break;
   }
   start ln=0;
   enterna=0;
   enterwi=0;
}
if(data1[0].sensors==0&&(se nsta.blmef==0||sensta.blmef==11))
{
   if(be nd<200)
   bend++;
}
else
{
   if(sensta.blmef<4||sensta.blmef>7)    //弯道数值待定
   {
   lnbn=1;
   enterln=0;
   if(b end<200)
      bend++;
   }
   else
   if(sensta.blmef>4&&sensta.blmef<7)    //直道
   {
   if(enterln<30)                      //延时识别  待定
      enterln++;
   else
      lnbn=0;
   if(be nd>0)
      bend--;
   }
}
if(data1[0].sensors==0&&(se nsta.blmef==0||sensta.blmef==11))
{

                  speedc.set=spset.vno;
         break;
   default:
         break;
   }
}
//*当前速度检测*//
sp n2=spn1+1;
if(spn 2>=NSP)
   spn2 -=NSP;
if(v_arr[ spn1]<v_arr[spn2])
   spe edc.now=(65536-v_arr[spn2])+v_arr[spn1];
else
   spe edc.now=v_arr[spn1]-v_arr[spn2];
sp eedc.now *=10;
if(spn 1>=NSP)
   spn1=0;
else spn1++;
//*速度控制*//
speedc.laste=speedc.error;                            //上次偏差
speedc.error =speedc.set-speedc.now;                   //计算当前偏差
speedc.sume +=speedc.error;                            //计算积分变量
speedc.derror=speedc.error-speedc.laste;                //计算微分变量
if(PO RTB_PB6==1&&bend<100&&lnbn==1)
{
sp _out=-2500;
}
else
{
sp_ out=P2*speedc.error+I2*speedc.sume+D2*speedc.derror;  //PID 控制
if(sp_out>3000)                                           sp_out=3000;
else if(sp_out<-2500)                                     sp_out=-2500;
}
if(sp_ out>0)
{
PWMD TY23=(unsigned int)sp_out;
PWMDTY67=0;
}
else
{
PWMDTY23=0;
PWMDTY67=(unsigned int)(-sp_out);
}

} //*********************中断处理函数*********************//
__interrupt 16 void TIMERintr()
{    asm("sei");       //关中断
inte=1;          //函数调用使能
TFLG2 |=0X80;    //清中断标志

作者: lfllcy 时间: 2010-6-6 01:06
楼上的这不是第四届烟台大学队的程序吗，请问你验证过吗...

作者: lfllcy 时间: 2010-6-6 01:09
pd，增量式的。我也看了加我 qq1449863441一起讨论吧

作者: 草原 时间: 2010-6-7 19:56
十分感谢你的慷慨！

作者: 草原 时间: 2010-6-7 20:04
你能打包重发一份的吗？你的程序错误太多

作者: andongny 时间: 2010-7-11 10:22
看看！

作者: kejilou 时间: 2010-7-13 09:18
速度位置式，舵机没给。

作者: qvsx100 时间: 2010-9-19 22:47
不了解，还是顶一下

作者: weichuiweicao 时间: 2011-7-2 18:23
我也想用PD控制舵机，但总是控制的不好，现在只用P控制，PD控制有啥好处啊？还不太清楚，求赐教！

作者: JAMESKY 时间: 2011-7-7 00:09
不错

作者: kgly 时间: 2011-7-7 15:07
顶一下！

作者: baiyawen 时间: 2011-7-8 20:21
位置式

作者: liuxuesong 时间: 2011-7-11 21:40
2.7m/s

作者: 海彬 时间: 2012-5-15 10:36
路过

作者: 冰蚕 时间: 2014-4-23 20:18

:lol

欢迎光临智能车制作 (http://www.znczz.com/)