PID.rar_DSP 步进电机_dsp 28035_dsp pid_步进电机 PID_步进电机PID

// //########################################################################### // // Ver | dd mmm yyyy | Who | Description of changes // =====|=============|======|=============================================== // 0.55| 06 May 2002 | L.H. | EzDSP Alpha Release // 0.56| 20 May 2002 | L.H. | No change // 0.57| 27 May 2002 | L.H. | No change //########################################################################### #include "DSP28_Device.h" //--------------------------------------------------------------------------- // InitGpio: //--------------------------------------------------------------------------- // This function initializes the Gpio to a known state. // void PID_ini(void) { unsigned int i; for(i=0;i<3;i++) { x[i]=0; //对误差进行初始化 y[i]=0; } P_gain=175; I_gain=3000; D_gain=0; T=0.001; // 采样周期 x_CCD=0; y_CCD=0; x_set=360; //考虑到CCD分的像素是720 X 576 则设定值取在其中心 y_set=288; X_Error_crl=0; Y_Error_crl=0; } void PID_cal(void) { // 增量式PID算法控制公式 u= A e + B e(k-1) + C e(k-2) A = P_gain*(1+T/I_gain+D_gain/T); B = P_gain*(1+2*D_gain/T); C = P_gain*D_gain/T; x[0]=x_set-x_CCD; //当前误差 y[0]=y_set-y_CCD; //增量算法计算值 用于控制转速 X_Error_crl=A*x[0]+B*x[1]+C*x[2]; //因为只有一维运算 Y_Error_crl=A*y[0]+B*y[1]+C*y[2]; //x[0]用于存放当前误差 x[1]用于存放E(K-1), x[2]用于存放E(K-2) x[2]=x[1]; //保存误差以用于下次计算 x[1]=x[0]; y[2]=y[1]; //保存误差以用于下次计算 y[1]=y[0]; } /****************************************************************************** 试验中发现： 当方向信号给予低电平时，步进电机顺时针方向旋转（即水平方向向右移动,而显示在CCD 观看图像上的向左移动） 则高电平，为逆时针方向旋转（即水平方向向左移动，与上相反） 似乎二维的运转台和一维运转台的方向不一致 *****************************************************************************8*/ void X_PID_ctrl(void) { X_ctrl_value_1 = X_Error_crl; //将误差计算值传递给ctrl_value X_Error_direction = X_ctrl_value_1 - X_ctrl_value_2; if(-2<= x[0] <=2 || -700<= X_ctrl_value_1 <=700) //允许有多大误差 { X_motor_stop; x_stop_y=1; //用于Y轴电机的条件编译 } if( x[0] > 2 ) { X_direction_high; EvaRegs.T1PR=60000; //如果偏离太远，直接采用比较大的电机转速 EvaRegs.T1CMPR=EvaRegs.T1PR/2; X_motor_start; } if( x[0] < -2) { X_direction_low; EvaRegs.T1PR=60000; //如果偏离太远，直接采用比较大的电机转速 EvaRegs.T1CMPR=EvaRegs.T1PR/2; X_motor_start; } /* if( -100<=x[0]<-5 ) //实行分阶段控制，以免速度太低，电机振动,分 { X_direction_high; x_ctrl_num=0.7; EALLOW; EvaRegs.T1PR=(int)(x_ctrl_num*(65536+X_ctrl_value_1)); //注意这里X_ctrl_value_1为负值 EvaRegs.T1CMPR=EvaRegs.T1PR/2; EDIS; X_motor_start; } if( x[0] < -100 ) { X_direction_high; EALLOW; EvaRegs.T1PR=10000; //如果偏离太远，直接采用比较大的电机转速 EvaRegs.T1CMPR=EvaRegs.T1PR/2; EDIS; X_motor_start; } if(X_ctrl_value_1<-700) { // X_motor_stop; //先停止产生PWM，进行设置 X_direction_high; // EvaRegs.T1PR=65536+15*X_ctrl_value_1; //注意这里X_ctrl_value_1为负值 EvaRegs.T1PR=(int)(65536+X_ctrl_value_1); //注意这里X_ctrl_value_1为负值 EvaRegs.T1CMPR=EvaRegs.T1PR/2; X_motor_start; x_stop_y=0; if(X_Error_direction>0) { // X_motor_stop; X_direction_low; EvaRegs.T1PR=(int)(65536+X_ctrl_value_1); //注意数据格式不统一 EvaRegs.T1CMPR=EvaRegs.T1PR/2; // X_motor_start; } } if( 5<x[0]<=100 ) { X_direction_low; x_ctrl_num=0.7; EALLOW; EvaRegs.T1PR=(int)(x_ctrl_num*(65536-X_ctrl_value_1)); //注意这里X_ctrl_value_1为负值 EvaRegs.T1CMPR=EvaRegs.T1PR/2; EDIS; X_motor_start; } if( x[0]>100 ) { X_direction_low; EALLOW; EvaRegs.T1PR=10000; //如果偏离太远，直接采用比较大的电机转速 EvaRegs.T1CMPR=EvaRegs.T1PR/2; EDIS; X_motor_start; } if(X_ctrl_value_1>700) { // X_motor_stop; X_direction_low; EvaRegs.T1PR=(int)(65536-X_ctrl_value_1); EvaRegs.T1CMPR=EvaRegs.T1PR/2; X_motor_start; //如果发现过程中误差逐渐增大，则认为走反了，立即反向 if(X_Error_direction>0) { // X_motor_stop; X_direction_high; EvaRegs.T1PR=(int)(65536-X_ctrl_value_1); EvaRegs.T1CMPR=EvaRegs.T1PR/2; // X_motor_start; } x_stop_y=0; //如果外界有干扰，恢复Y值 } */ X_ctrl_value_2=X_ctrl_value_1; //存储误差，以判断后期方向 } void Y_PID_ctrl(void) { //0.75的系数主要是为了适应CCD的宽高比 Y_ctrl_value_1 = 0.75*Y_Error_crl; //将误差计算值传递给ctrl_value Y_Error_direction = Y_ctrl_value_1 - Y_ctrl_value_2; if(-3<= y[0] <=3 || -787.5<= Y_ctrl_value_1 <=787.5) //允许有多大误差 { Y_motor_stop; y_stop_x=1; } if( Y_ctrl_value_1<-787.5) { // Y_motor_stop; //先停止产生PWM，进行设置 Y_direction_high; EvaRegs.T2PR=(int)(65536+Y_ctrl_value_1); //注意这里Y_ctrl_value_1为负值 EvaRegs.T2CMPR=EvaRegs.T2PR/2; Y_motor_start; y_stop_x=0; /* if(Y_Error_direction>0) { // Y_motor_stop; Y_direction_low; EvaRegs.T2PR=(int)(65536+Y_ctrl_value_1); EvaRegs.T2CMPR=EvaRegs.T2PR/2; // Y_motor_start; } */ } if(Y_ctrl_value_1>787.5) { // Y_motor_stop; Y_direction_low; EvaRegs.T2PR=(int)(65536-Y_ctrl_value_1); EvaRegs.T2CMPR=EvaRegs.T2PR/2; Y_motor_start; /* //如果发现过程中误差逐渐增大，则认为走反了，立即反向 if(Y_Error_direction>0) { // Y_motor_stop; Y_direction_high; EvaRegs.T2PR=(int)(65536-Y_ctrl_value_1); EvaRegs.T2CMPR=EvaRegs.T2PR/2; // Y_motor_start; } */ y_stop_x=0; } Y_ctrl_value_2=Y_ctrl_value_1; //存储误差，以判断后期方向 } //=========================================================================== // No more. //===========================================================================