伺服电机控制代码

给出了伺服电机PWM控制的PID调节算法实现，本算法已应用与高速绘图仪，仅作参考，弱用于商业用于，不承担任何责任。

#include “pid_x.h“

/*** Beginheader speed_relay*/
 void speed_relay_X（void）
{
   float Out_PWM;  /*继电器的PWM输出*/
/*********计算临时变量****************/
   static float k1801w1801;
   static float c1 c2 f_relay Current_Speed_Value;  
   static float fl_temp;
    c1=0.392699;
    c2=6.283185;

    f_relay=speed_relay;
    Current_Speed_Value=（float）errspeed_x;      /*得到当前的速度值*/
    
    if （Current_Speed_Value<0.0）  /*如果当前测量的速度值小于速度的稳态值steady_v*/
	Out_PWM=0-f_relay;      /*继电器的输出为Out_PWMsteady_c*/
    else                               /*如果当前测量的速度值大于等于速度的稳态值*/
	Out_PWM=0+f_relay;      /*继电器的输出为Out_PWMsteady_c*/
 /*   	
    if（Out_PWM>10）
    	Out_PWM=10;
    if（Out_PWM<0）
    	Out_PWM=0;
     speedmv=Out_PWM*100.0/10;
    speedsetpoint=speedpv;
*/

    /*anaOutVolts（Out_PWM）;                 输出PWM值移植到ARM9时需要
    */
    
   /*继电器的控制输出*/
   input_x= （int）Out_PWM;
   if （（Out_PWM<=0.0）&&（speed_flag==1））//steady_c
                    speed_flag=0;
    if （（Out_PWM>0.0）&&（speed_flag==0））//steady_c
            {
			       speed_crossover++;
			       speed_flag=1;
			        if（speed_crossover==1）
	 		          {
	   			        speed_pv_max=Current_Speed_Value;
	   			        speed_pv_min=Current_Speed_Value;
	   			        speed_measure=1;     
	  		           }
	 		        if（speed_crossover==2）      /*speed_crossover等于2时表示一个震荡周期结束要完成临界值的计算*/
	 		         {
	   			        speed_crossover=0;
	   			        speed_flag=1;
			   			speed_measure=0;
						fl_temp=speed_pv_max-speed_pv_min;
			 			fl_temp=fl_temp*c1;
			 			fl_temp=fl_temp/f_relay;
			 			k1801=fl_temp;
			 			fl_temp=（float）speed_relay_counter;
						fl_temp=fl_temp/time;        /*为什么除以10 ?*/
			 			fl_temp=c2/fl_temp;
			 			w1801=fl_temp;
						speed_relay_counter=0;
			 			speed_relay_finish1++;   
						if（speed_relay_finish1==speed_relay_finsh） /*speed_relay_finish1表示震荡完成的次数完成四次后可以看作开始作等幅震荡*/
			 			  {   speed_k180=k1801;
			   			      speed_w180=w1801;
			   		       }
			 		   }
	        }
    if（speed_measure==1）
        {
			if（Current_Speed_Value>speed_pv_max）
				speed_pv_max=Current_Speed_Value;
			if（Current_Speed_Value				speed_pv_min=Current_Speed_Value;
			speed_relay_counter++;
		}
}  

static void speed_auto_tuning（void）
{

	   speed_relay_X（）;
		if（speed_relay_finish1==speed_relay_finsh） /*speed_relay_finish1表示震荡完成的次数完成四次后可以看作开始作等幅震荡*/

		 {	/*speed_pid_tuning=1;
			  speedmode=1;
			*/
			PID_Relay_Flag=0;
			speed_relay_finish1=0;	
		/*		
			speedKp_X=cos（3.1415926*0.25）/（speed_k180*2.0）;
			speedTd_X=（tan（3.1415926*0.25）+sqrt（tan（3.1415926*0.25）*tan（3.1415926*0.25）+1 ））/（speed_w180*2）;
			speedTi_X=speedTd_X*4.0;
		*/	
			speedKp_X=540.1899;    //test
			speedTd_X=57.6351;
			speedTi_X=230.5404;	

	
		 }	
	}
	
void PID_X（void）
{       
  
 float control_output;
  
  current_speed=pulse_error_x;
  
  speed_I=speed_last_I+（0+speedKp_X）*Sample_pe

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      16334  2007-05-21 17:29  motor\MotionControl--beifen.C

     文件       4322  2007-05-21 18:32  motor\pid_x.c

     文件       2193  2007-05-17 10:10  motor\pid_x.h

     文件       4245  2007-05-21 14:41  motor\pid_y .c

     文件       2375  2007-05-17 10:11  motor\pid_y.h

     目录          0  2007-07-22 16:41  motor

----------- ---------  ---------- -----  ----

                29469                    6