步進電機轉到指定位置
單片機源程序如下:
- #include "led.h"
- #include "delay.h"
- #include "key.h"
- #include "sys.h"
- #include "beep.h"
- #include "encoder.h"
- #include "timer.h"
- int main(void)
- {
- vu8 key=0;
- delay_init(); //延時函數初始化
- uart_init(9600); //=====串口初始化
- Encoder_Init_TIM2(); //=====編碼器接口
- TIM6_Int_Init(99,7199); //10MS進入一次中斷
- TIM3_PWM_Init(7199,0); //不分頻。PWM頻率=72000000/900=80Khz
- while(1)
- {
-
- key=KEY_Scan(0); //得到鍵值
- if(key)
- {
- switch(key)
- {
- case WKUP_PRES: //控制蜂鳴器
- LED0=!LED0;
- break;
- case KEY2_PRES: //控制LED0翻轉
- LED0=!LED0;
- break;
- case KEY1_PRES: //控制LED1翻轉
- LED1=!LED1;
- break;
- case KEY0_PRES: //同時控制LED0,LED1翻轉
- LED0=!LED0;
- LED1=!LED1;
- break;
- }
- }else delay_ms(10);
- }
- }
- #include "control.h"
- #define PI 3.141592653
- #include "sys.h"
- int Encoder,Target_position=10280; //Encoder編碼器的脈沖計數，相當于實際速度 Target_velocity目標速度 一周1040脈沖 初始位置10000在encoder.c中設置
- int Moto; //電機PWM變量 應是Motor的 向Moto致敬
-
- void TIM6_IRQHandler(void) //TIM6中斷 函數實現了編碼器角速度線速度的計算 10MS進入一次中斷
- {
- if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) //檢查TIM3更新中斷發生與否
- {
- TIM_ClearITPendingBit(TIM6, TIM_IT_Update ); //清除TIMx更新中斷標志
- Encoder=Read_Encoder(2);
- printf("位置%d\r\n",Encoder);
- Moto=Position_PID(Encoder,Target_position); //===位置PID控制器
- Xianfu_Pwm(); //===PWM限幅
- Set_Pwm(Moto); //===賦值給PWM寄存器
- }
- }
- /**************************************************************************
- 函數功能：賦值給PWM寄存器
- 入口參數：PWM
- 返回 值：無
- **************************************************************************/
- void Set_Pwm(int moto1)
- {
- if(moto1<0) IN2=1, IN1=0; //換向引腳
- else IN2=0, IN1=1;
- TIM_SetCompare2(TIM3,myabs(moto1));
- }
- /**************************************************************************
- 函數功能：限制PWM賦值
- 入口參數：無
- 返回 值：無
- **************************************************************************/
- void Xianfu_Pwm(void)
- {
- int Amplitude=5000; //===PWM滿幅是7200 限制在7100
- if(Moto<-Amplitude) Moto=-Amplitude;
- if(Moto>Amplitude) Moto=Amplitude;
- }
- /**************************************************************************
- 函數功能：絕對值函數
- 入口參數：int
- 返回 值：unsigned int
- **************************************************************************/
- int myabs(int a)
- {
- int temp;
- if(a<0) temp=-a;
- else temp=a;
- return temp;
- }
- /**************************************************************************
- 函數功能：增量PI控制器
- 入口參數：編碼器測量值，目標速度
- 返回 值：電機PWM
- 根據增量式離散]]PID公式
- pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
- e(k)代表本次偏差
- e(k-1)代表上一次的偏差 以此類推
- pwm代表增量輸出
- 在我們的速度控制閉環系統里面，只使用PI控制
- pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
- **************************************************************************/
- int Position_PID (int Encoder,int Target)
- {
- float Position_KP=10,Position_KI=0.1,Position_KD=40;
- static float Bias,Pwm,Integral_bias,Last_Bias;
- Bias=Encoder-Target; //計算偏差
- Integral_bias+=Bias; //求出偏差的積分
- Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
- Last_Bias=Bias; //保存上一次偏差
- return Pwm; //增量輸出
- }
所有資料51hei提供下載:
PID精準控制電機轉到指定位置.7z
(191.75 KB, 下載次數: 311)
2019-4-23 21:59 上傳
點擊文件名下載附件
|
[復制鏈接]
