pid.c

本人s12的一部分机器人巡线程序

#define Position 9
#define MAX_Speed 50
#define MIX_Speed 10
#define Init_Speed 30 //初始给定速度
char e0,e1,e2;
float Mid_Speed,Mid_SpeedV;
float Left_SpeedV,Right_SpeedV;
//const float P=Kp*(1+T/Ti+Td/T),I=Kp*(1+2*Td/T),D=Kp*Td/T;
const float Kp=0.870,Ki=0.550,Kd=0.0;
const float P=0.5,I=0.0,D=0.0;//P=Kp+Ki+Kd,I=Kp+2*Kd,D=Kd;
/**************************PID方式计算出给定下位机的速度****************************************/
void PID_Speed(uchar positions)
  {
      e0=Position-positions;//计算位置偏差
      Mid_Speed=(float)(P*e0-I*e1+D*e2);//PID计算
      e2=e1;//保存状态
      e1=e0;
      if((Mid_Speed>255)||(Mid_Speed<-255))Mid_Speed=255;//如果计算结果超过8位数值则取8位数最大值
      Mid_SpeedV=abs(Mid_Speed/2);//平分输出偏差
      if((e0==0)&&(e1==0))//如果三次等于0偏差则调整到30的最初给定速度,消除积分取整偏差
        {
          LeftSpeed=RightSpeed=Init_Speed;//回复初始给定速度
        }
      if(positions<9)//位置值小于9,左侧传感器在白线,要求左慢右快
        {
          Left_SpeedV=Init_Speed-Mid_SpeedV;//左侧电机速度减去半PID偏差
          if(Left_SpeedV<=MIX_Speed)LeftSpeed=MIX_Speed;//如果相减结果小于最小值,左侧等于最小值
          else LeftSpeed=(uchar)Left_SpeedV;//否则速度等于前次给定速度加半PID偏差
          
          Right_SpeedV=Init_Speed+Mid_SpeedV;//右侧电机速度加上半PID偏差
          if(Right_SpeedV>=MAX_Speed)RightSpeed=MAX_Speed;//如果速度超过设定上限,右侧速度等于上限
          else RightSpeed=(uchar)Right_SpeedV;//否则速度等于前次给定速度加PID半偏差
        }
      else  //位置大于等于9,右侧传感器在白线,要求左快右慢
        {
          Left_SpeedV=Init_Speed+Mid_SpeedV;
          if(Left_SpeedV>=MAX_Speed)LeftSpeed=MAX_Speed;
          else LeftSpeed=(uchar)Left_SpeedV;
          Right_SpeedV=Init_Speed-Mid_SpeedV;          
          if(Right_SpeedV<=MIX_Speed)RightSpeed=MIX_Speed;
          else RightSpeed=(uchar)Right_SpeedV;
        }
  }

/*****************************用对称平分误差输出方式计算给定速度*************************************************/
#define MID 9  //给定的中间位置
float Midspeed;  //给定的中间速度
float E;//误差
float E1;// 前次误差
float dE;//误差变化
float B1;//中间变量1
float B2;//中间变量2
float OUT1;//计算输出结果
float OUT_L,OUT_R;//左右电机输出的最终结果
uchar var_x;
float coe_speed;//coefficient系数
ulong leftspeedcount,rightspeedcount;
void Calculat_E(void)
  {
    E=e_f-MID;//为负值表示当前白线在左排传感器上,左电机减速,右电机加速
//    if(((E1==-9)&&((E!=-8)||(E!=-7)))||((E1==9)&&((E!=8)||(E!=7))))
//      {
//        E=E1;
//      }
    if(Midspeed==0)coe_speed=100;
    else coe_speed=100.0/Midspeed;//比例因子计算公式
    
    if((E>9)||(E<-9)||(E1>9)||(E1<-9)){E=E1=0;} //主要是防止在启动的时候读取到的随机值扰乱数据
    if((E<-4)||(E>4))dE=1*E-E1;
    else if((E>=-4)&&(E<=4))dE=0*E-E1;
    
    B1=E+dE;
    B2=E*E+5*dE*dE;
    
    if(B1==0)OUT1=0;
    else OUT1=B2/(B1*coe_speed);//根据基准速度高低调节比例因数
    if(OUT1>Midspeed){OUT_L=Midspeed+OUT1;OUT_R=0;}
    else if(OUT1<(0-Midspeed)){OUT_L=0;OUT_R=Midspeed-OUT1;}
    else{OUT_L=Midspeed+OUT1;OUT_R=Midspeed-OUT1;}
    
    LeftSpeed=OUT_L;
    RightSpeed=OUT_R;
    E1=E;
  }

/*****************************用查表方式计算出给定下位机的速度***************************************************/
void Calculate_Speed(void)
          {
            if(direction==0x08)//正转
              {
                LeftSpeed=leftforward[e_f][e_fL];//查规则表获取左边电机的速度
                RightSpeed=rightforward[e_f][e_fL];//查规则表获取右边电机的速度
                e_fL=e_f;    
              }
            else if(direction==0x04)//反转
              {
                LeftSpeed=leftforward[e_b][e_bL];//查规则表获取左边电机的速度
                RightSpeed=rightforward[e_b][e_bL];//查规则表获取右边电机的速度
                e_bL=e_b;  
              }
            else if(direction==0)//发现停车请求使速度等于0
              {
                LeftSpeed=0;
                RightSpeed=0;
              }
          }      
          
/******************************向从机发送计算出来的速度值*******************************************/ 
           
void Speed_Comm(void)
           {
            leftspeedcount+=LeftSpeed;
            rightspeedcount+=RightSpeed;
            if(((RightSpeedL!=RightSpeed)&&(LeftSpeedL!=LeftSpeed))||(equ_count==5)) //equ_countR==5时即使速度相同也补发信号，防止从机数据丢失或者上次通信失败
              {
                STUTS=Left_speed_sent(LeftSpeed,directionL);
                STUTS=Right_speed_sent(RightSpeed,directionR);
                equ_count=0;
              }
            else equ_count++;   
            LeftSpeedL=LeftSpeed;               //记录当前左边电机的速度作为下次参考
            RightSpeedL=RightSpeed;             //记录当前右边电机的速度作为下次参考
          }
/*******************************与电脑通讯显示采集数据部分,在真正比赛时候可以不使用*******/ 
void Cpu_Commu(void)
         {
             /*******和电脑通信显示数据,便于调试*******/    
                Comm_time++;
                if((Comm_time==5)&&(key_Comm==FALSE))
                  {
                    Comm_time=0;
                    key_Comm=TRUE;//这里可以使用消息邮箱方式
                    //OSTaskResume(14);//恢复通讯任务
                  }
        }

/*******************************数线函数,实验阶段,可能放在单独任务中比较好*******/
/*
void Lines_Counter(void)         
         {
             //if(direction==0x08)Follow_Stuts=Line_Count(AD_bData0,7);
             //              else Follow_Stuts=Line_Count(AD_bData1,7);
             if(AD_bData1[0]>180)Follow_Stuts=TRUE;//判断数线传感器是否数到线
             if(Follow_Stuts==TRUE)
              {
                Line_a++;//数到白线则加1
                Follow_Stuts=FALSE;
              }
             if(Line_a>=2)//当数线加1到大于等于2的时候算是正确的白线
              {
                RightSpeed=0;//数到白线的时候使电机速度为0
                LeftSpeed=0;
                Line_a=0;//清除数线变量
                Follow_Num--;
                Follow_Start=FALSE;//循线任务关闭
                Key_TaskStart_EN=TRUE;//查找地图下一步打开
             }
          }     
*/