舵机用PD速度用PID，能让车子跑多快？

流水三千

帮我看看下面的程序舵机和速度分别采用的是什么算法？是位置式？or增量式？
程序很长，请看附件

流水三千

#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 通道  
}

流水三千

//*****************定时计数模块初始化**************//
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--);
}

流水三千

//************路径信息采集及舵机控制函数***********//  
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

楼上的这不是第四届烟台大学队的程序吗 ，请问你验证过吗...

lfllcy

pd，增量式的。我也看了 加我 qq1449863441一起讨论吧

草原

十分感谢你的慷慨！

草原

你能打包重发一份的吗？  你的程序错误太多

andongny

看看！

kejilou

速度位置式，舵机没给。