速度闭环.docVIP

速度闭环 #include hidef.h /* common defines and macros */ #include derivative.h /* derivative-specific definitions */ #define MOTOR_LEFT_SPEED_POSITIVE (g_fLeftMotorOut 0) #define MOTOR_RIGHT_SPEED_POSITIVE (g_fRightMotorOut 0) /*void AD_Init(void); void AD_GetValue(); void AD_GetValue_Gyro(); int Left1,Right1,Gyro;*/ signed char X_value,Y_value,Z_value; signed char X_value_final,Y_value_final,Z_value_final; //三轴加速度的最终值 int Left1,Right1,Gyro; int MOTOR_OUT_DEAD_VAL,MOTOR_OUT_MAX,MOTOR_OUT_MIN,Gyro_final,Acc_final,g_lnCarAngleSigma,g_nCarAngle,GyroV; float g_fLeftMotorOut,g_fRightMotorOut,g_fAngleControlOut,g_fSpeedControlOut; int nLeftVal, nRightVal, a,b,c,d,m,n,aa,bb; float fLeftVal, fRightVal,vol; float g_fGravityAngle,g_fGyroscopeAngleSpeed,g_fCarAngle,g_fGyroscopeAngleIntegral,fspeed; float g_fSpeedControlIntegral,g_fSpeedControlOutOld,g_fSpeedControlOutNew,g_fCarSpeed; double g_fLeftVoltageSigma,g_fRightVoltageSigma,g_fDirectionControlOutOld,g_fDirectionControlOutNew; double g_fDirectionControlOut,g_nDirectionControlPeriod; int g_nLeftMotorPulseSigma,g_nRightMotorPulseSigma,g_nSpeedControlPeriod,g_n1MSEventCount,g_nSpeedControlCount; word num1,num0,i,j; uchar flag,ch1,ch0; int Counter,g_nDirectionControlCount; float GyroSum,AccSum,DerictiontempSub,DerictiontempSum; int g_nLeftMotorPulse,g_nRightMotorPulse; void Pll_Init(void) { CLKSEL_PLLSEL=0; //不使能锁相环 PLLCTL_PLLON=1; //锁相环电路允许 SYNR=0X41; //VCO_CLOCK=2*osc_clock*(1+SYNR)/(1+REFDV)=80MHz //VCOFRQ[1:0]=1:1 REFDV=0XC0; POSTDIV=0X00; //VCO_CLOCK=PLL_CLOCK _asm(nop); _asm(nop); while(!(CRGFLG_LOCK==1)) //等待时钟频率稳定 { ; } CLKSEL_PLLSEL=1; //使能锁相环时钟 } void AD_Init(void){ ATD0DIEN=0x00; // 禁止数字输入 ATD0CTL1=0x00; // 8位精度，不放电 ATD0CTL2=0x60; // 启动A/D, 快速清零, 无等待模式, 禁止外部触发, 中断禁止 ATD0CTL3=0xC0; // 转换序列长度为8, No FIFO,