|
|
主函數- int main(void)
- {
- delay_init(); //延時函數初始化
- NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//設置中斷優先級分組為組2：2位搶占優先級，2位響應優先級
- uart_init(115200); //串口初始化為115200
- IIC_Init();
- TIM3_PWM_Init(19999,71); //PA6,PA7 PWM 舵機輸出 20ms
- PWMA=1600;
- PWMB=1600;
- delay_ms(500);
- MPU6050_initialize(); //=====MPU6050初始化
- DMP_Init();
- // Init_HMC5883();
- TIM2_Getsample_Int(4999,71); //5ms定時中斷
- while(1)
- {
- delay_ms(50);
- ///////////////////////////////////////////////////////
- }
- }
- int MPU6050_PID(float pitch,float Target)
- {
- static float kp=66,kd=-20,Ki=0.04;
- static float LastError,SumError;
- float Error,dError;
- int PWM;
- char flag;
- //求偏差
- Error = pitch-Target; //
- printf("\n Error= %.2f \n",Error);
- //積分
- SumError+=Error;
- //積分限幅
- if(SumError>500) SumError=500;
- else if(SumError<-500) SumError=-500;
- //積分分離
- if(fAbs(Error)<3) flag=1;
- else flag=0;
- //微分
- dError=LastError-Error;
- LastError=Error;
- //計算PID
- PWM = kp*Error+kd*dError+flag*Ki*SumError; //
- return PWM/100;
- }
|
-
-
舵機PID控制.zip
2018-4-23 15:11 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
452.93 KB, 下載次數: 322, 下載積分: 黑幣 -5
評分
-
查看全部評分
|
[復制鏈接]
