杂项——舵机云台的PID控制

 

云台的PID算出的数据不能直接作为PWM加载到舵机上。应当适当的处理

control函数 

        pwm_x=Position_PIDx (red_x,target_x);       
        pwm_y=Position_PIDy (red_y,70);



        Limit(&pwm_x,&pwm_y);
        //Servo_SetAngle1(pwm_y);               //上下移动，为Y轴       0为最上方，170为最下方            -90最上方，90最下方
        //Servo_SetAngle2(pwm_x);               //左右移动，为X轴       0为最右边，170为最左边             -90最右边，90最左边


//对于Y轴的控制
        if(pwm_y!=0)                //说明有误差，那就用输出的PWM作为角度
        {
            last_angle_y=angle_y;           //保存上一个角度
            angle_y=angle_y+pwm_y;          //不断累加角度
        }

        if(pwm_y==0)            //没误差了，那就继续等于上一个角度
        {
         angle_y=last_angle_y;
        }

        if(angle_y>90)              //限幅
        {
           angle_y=90;     
        }

          if(angle_y<-90)           //限幅
        {
           angle_y=-90;     
        }

         Servo_SetAngle1(angle_y);               //上下移动，为Y轴       0为最上方，170为最下方            -90最上方，90最下方


// //对于X轴的控制
//         if(pwm_x!=0)                //说明有误差，那就用输出的PWM作为角度
//         {
//             last_angle_x=angle_x;           //保存上一个角度
//             angle_x=angle_x+pwm_x;          //不断累加角度
//         }

//         if(pwm_x==0)            //没误差了，那就继续等于上一个角度
//         {
//          angle_x=last_angle_x;
//         }

//         if(angle_x>90)              //限幅
//         {
//            angle_x=90;     
//         }

//           if(angle_x<-90)           //限幅
//         {
//            angle_x=-90;     
//         }

//          Servo_SetAngle2(angle_x);             //左右移动，为X轴       0为最右边，170为最左边             -90最右边，90最左边

 

 舵机函数

float b=0;
void Servo_SetAngle1(float Angle)           //angle范围-90到90        Y轴
{
     b=((Angle / 180) * 200) + 150;                  
    
 
	DL_TimerA_setCaptureCompareValue(PWM_Servo_INST, b, DL_TIMER_CC_0_INDEX);
}

void Servo_SetAngle2(float Angle)           //X轴
{
     b=((Angle / 180 )* 200) + 150;
    
 
	DL_TimerA_setCaptureCompareValue(PWM_Servo_INST, b, DL_TIMER_CC_1_INDEX);
}



//PID控制舵机1               //传入参数：目标X值和实际X值             X轴PID
int Position_PIDx (int True,int Target)
{ 	
	 float Position_KP=0.1,Position_KI=0,Position_KD=0;
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target-True;                                  //计算偏差
	 Integral_bias+=Bias;	                                 //求出偏差的积分
	 Pwm=-(Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias));       //位置式PID控制器
	 Last_Bias=Bias;                                       //保存上一次偏差 
	 return Pwm;                                           //增量输出

}



//PID控制舵机2               //传入参数：目标Y值和实际Y值           Y轴PID
int Position_PIDy (int True,int Target)
{ 	
	 float Position_KP=0.06,Position_KI=0.01,Position_KD=0;             //
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target-True;                                  //计算偏差
	 Integral_bias+=Bias;	                                 //求出偏差的积分
	 Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);       //位置式PID控制器
	 Last_Bias=Bias;                                       //保存上一次偏差 
	 return Pwm;                                           //增量输出

}