附录A下位机程序全文.docVIP

附录A 下位机程序全文 #include DSP28_Device.h #include ext_inf.h #include fir.h #include stdlib.h #define _const_length 0x400 #define _econst_length 0x400 struct ECAN_REGS ECanaShadow; //Create a shadow //////////存放A/D采集到的传感器数据///////// #define n 16 unsigned int ADLength[n],ADAngle[n],ADEncoder[n]; unsigned int ADCurrent[n],ADTorque[n],ADSpeed[n]; ////////////经过数字滤波后的数据///////////// unsigned int Length,Angle,Encoder,Angle2; unsigned int Current,Torque,Angle_motor; unsigned int Speed_motor,Speed_motor_fir; int AngelIntergral=90; //经过积分运算后得到的角度 int AngelInitial=90; //经过标定的初始角度 //////////一些参数//////////////////////// unsigned int a2=0; float a1[6]; float adclo=0.0; float * aa = (float *) 0 unsigned int CountAD=0; //AD采样的次数 unsigned int FinishAD=0; //采样完毕指示 unsigned int AngleRef=39115; //零角速率时A/D量 ////////////////与EV相关的一些参数///////////////// int Delta,T1CNT0,T1CNT1,T1CNT2,T2CNT0,T2CNT1,T2CNT2; unsigned int Pn=2000;//一周脉冲数 extern unsigned int Period=0xb72; //定时器一的采样 周期 float f0=200.0*((float)0xb72); //150000000/(2*128*0x16E3);//采样时间为10ms,采样频率为100HZ float pi=3.14,Delta_time; float speed=0,Theta=0,DeltaTheta=0,ThetaMech=0;//电机位置,速度,减速器的角度 int rot_direction=0,Position=0; //电机的旋转方向 unsigned int QEP_Flag1=0; // 捕获使能标志1 unsigned int QEP_Flag2=0; // 捕获使能标志2 unsigned long MotorSpeed_L=0; //电机1的速度 unsigned long MotorSpeed_R=0; //电机2的速度 ////////D/A输出量/////////////////////// unsigned int DAOUT=0; //////////与闭环控制有关的一些参数////// int SpeedRef=0,PositionRef=0; //速度，位置给定 int DeltaSpeed[3]={0,0,0},DeltaPosition[3]={0,0,0};//速度，位置误差表 int ControlValue=0,ControlValue_S=0,ControlValue_P=0; //电机的控制量 int DeltaSpeedTemp=0,DeltaPositionTemp=0;//速度,位置静差 float Kp=2.2,Kd=0,Ki=0.008;//PID参数 float KpTemp=0,KdTemp=0,KiTemp=0; float Kp1=1.9,Kd1=0.0,Ki1=0.01;//位置环PID 参数 float Kp2=1,Kd2=0,Ki2=0;//速度环PID参数 float DeltaControlValue=0,DeltaControlValue_S,DeltaControlValue_P; //位置，速度控制增量 #define PID_1; //选择PID算法 #define Control