基于STC8A8K單片機(jī)的PID舵機(jī)小車控制程序源碼

ID:375898 · 發(fā)表于 2018-7-20 15:06

自己為電賽寫的沒用到~

單片機(jī)源程序如下:

#include<stc8.h>
#include<math.h>
#define PWMA1 P20
#define PWMA2 P21
#define PWMB1 P22
#define PWMB2 P23
#define SERVO P24 //舵機(jī)引腳
#define u8 unsigned char
#define T 0.156f
#define L 0.1445f
#define K 3.114f
#define PI 3.14159265
int Encoder_Left,Encoder_Right; //左右編碼器的脈沖計(jì)數(shù)
float Velocity,Velocity_Set,Angle,Angle_Set;
unsigned long count0,count1,count2,count3,Sensor;//編碼器脈沖計(jì)數(shù)
int Motor_A,Motor_B,Servo,Target_A,Target_B; //電機(jī)舵機(jī)控制相關(guān)
/**************************************************************************
函數(shù)功能：絕對(duì)值函數(shù)
入口參數(shù)：int
返回值：unsigned int
**************************************************************************/
int myabs(int a)//絕對(duì)值函數(shù)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
函數(shù)功能：電機(jī)舵機(jī)占空比
**************************************************************************/
void LmotorPWM(u8 D0,u8 D1)//左電機(jī)PWM占空比
{
P_SW2=0X80;
PWMCKS=0X09;//PWM時(shí)鐘為系統(tǒng)時(shí)鐘
PWMC=0XFD3F;//設(shè)置PWM周期為64831個(gè)脈沖,T=20MS,
PWM0T1=D0*648;
PWM1T1=D1*648;
PWM0CR=0X80;//使能PWM0輸出，且初始電平為低電平
PWM1CR=0X80;//使能PWM1輸出，且初始電平為低電平
P_SW2=0X00;
PWMCR=0X80;
}
void RmotorPWM(u8 D2,u8 D3)//右電機(jī)PWM占空比
{
P_SW2=0X80;
PWMCKS=0X0E;//PWM時(shí)鐘為系統(tǒng)時(shí)鐘
PWMC=0XFD3F;//設(shè)置PWM周期為12C0H個(gè)脈沖,T=200US
PWM2T1=D2*648;
PWM3T1=D3*648;
PWM2CR=0X80;//使能PWM2輸出，且初始電平為低電平
PWM3CR=0X80;//使能PWM3輸出，且初始電平為低電平
P_SW2=0X00;
PWMCR=0X80;
}
void DJPWM(u8 D4)//舵機(jī)PWM占空比
{
P_SW2=0X80;
PWMCKS=0X0E;//PWM時(shí)鐘為系統(tǒng)時(shí)鐘
PWMC=0XFD3F;//設(shè)置PWM周期為12C0H個(gè)脈沖,T=200US
PWM4T2=0x0380;//在0.5MS時(shí)賦值高電平
PWM4T1=0x0380+17.7*D4;
PWM4CR=0X80;//使能PWM2輸出，且初始電平為低電平
P_SW2=0X00;
PWMCR=0X80;
}
/**************************************************************************
函數(shù)功能：小車運(yùn)動(dòng)學(xué)分析
**************************************************************************/
void Analysis(float velocity,float angle)
{
Target_A=velocity*(1+T*tan(angle)/2/L);
Target_B=velocity*(1-T*tan(angle)/2/L); //后輪差速
Servo=9+angle*K; //舵機(jī)轉(zhuǎn)向
}
/**************************************************************************
函數(shù)功能：占空比設(shè)置
**************************************************************************/
void Set_Pwm(int motor_a,int motor_b,int servo)
{
if(motor_a<0) PWMA1=90,PWMA2=90+motor_a;
else PWMA2=90,PWMA1=90-motor_a;
if(motor_b<0) PWMB1=90,PWMB2=90+motor_b;
else PWMB2=90,PWMB1=90-motor_b;
SERVO=servo;
}
/**************************************************************************
函數(shù)功能：電機(jī)PI控制
**************************************************************************/
int PI_A (int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Target-Encoder; //計(jì)算偏差
Pwm+=0.015*(Bias-Last_bias)+0.015*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量輸出
}
int PI_B (int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Target-Encoder; //計(jì)算偏差
Pwm+=0.015*(Bias-Last_bias)+0.015*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量輸出
}
/**************************************************************************
函數(shù)功能：電機(jī)占空比限制幅度
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=90; //===PWM滿幅是90 限制在90
if(Motor_A<-Amplitude) Motor_A=-Amplitude;
if(Motor_A>Amplitude) Motor_A=Amplitude;
if(Motor_B<-Amplitude) Motor_B=-Amplitude;
if(Motor_B>Amplitude) Motor_B=Amplitude;
if(Servo<(0)) Servo=0; //舵機(jī)限幅
if(Servo>(120)) Servo=120; //舵機(jī)限幅
}
/**************************************************************************
函數(shù)功能：計(jì)算偏差角度
**************************************************************************/
void Get_RC(void)
{
static float Bias,Last_Bias;
Velocity=45; //電磁巡線模式下的速度
Bias=45-Sensor; //提取偏差
Angle=myabs(Bias)*Bias*0.0002+Bias*0.001+(Bias-Last_Bias)*0.05; //PI控制
Last_Bias=Bias; //上一次的偏差
}
/**************************************************************************
函數(shù)功能：編碼器計(jì)數(shù)
**************************************************************************/
void PCA_C()
{
count0=0;
count1=0;
CCON=0X00;
CMOD=0X09;//設(shè)定為系統(tǒng)時(shí)鐘，使能PCA溢出中斷
CL=0X00;
CH=0X00;
CCAPM0=0X21;//設(shè)置PCA0模塊為16位捕獲模式，且為上升沿捕獲
CCAPM1=0X21;//設(shè)置PCA1模塊為16位捕獲模式，且為上升沿捕獲
CCAPM2=0X21;//設(shè)置PCA2模塊為16位捕獲模式，且為上升沿捕獲
CCAPM3=0X21;//設(shè)置PCA3模塊為16位捕獲模式，且為上升沿捕獲
CCAP0L=0X00;
CCAP0H=0X00;
CCAP1L=0X00;
CCAP1H=0X00;
CCAP2L=0X00;
CCAP2H=0X00;
CCAP3L=0X00;
CCAP3H=0X00;
CR=1;//啟動(dòng)PCA
EA=1;
}
/**************************************************************************
函數(shù)功能：電機(jī)運(yùn)動(dòng)控制算法
**************************************************************************/
void control(void)
{
Encoder_Left=count0; //===讀取編碼器的值 //為了保證M法測(cè)速的時(shí)間基準(zhǔn)，首先讀取編碼器數(shù)據(jù)
Encoder_Right=count1; //===讀取編碼器的值
Get_RC();
Analysis(Velocity,Angle); //小車運(yùn)動(dòng)學(xué)分析
Motor_A=PI_A(Encoder_Left,Target_A); //===速度閉環(huán)控制計(jì)算電機(jī)A最終PWM
Motor_B=PI_B(Encoder_Right,Target_B); //===速度閉環(huán)控制計(jì)算電機(jī)B最終PWM
Xianfu_Pwm(); //===PWM限幅
Set_Pwm(Motor_A,Motor_B,Servo); //===賦值給PWM寄存器
}
void main()
{
while(1)
{
control();
LmotorPWM(PWMA1,PWMA2);
RmotorPWM(PWMB1,PWMB2);
DJPWM(Servo);//0-120度
PCA_c();
}
}
void PCA() interrupt 7 using 1//編碼器中斷
{
if(CCF0)
{
CCF0=0;
count0++;
}
if(CCF1)
{
CCF1=0;
count1++;
}
if(CCF2)
{
CCF2=0;
count1++;
}
if(CCF3)
{
CCF3=0;
count1++;
}
}

復(fù)制代碼

所有資料51hei提供下載:

STC8電機(jī)控制.zip (71.69 KB, 下載次數(shù): 117)

ID:487103 · 發(fā)表于 2019-4-22 00:23

很好的資料，加油

ID:587285 · 發(fā)表于 2019-7-20 11:20

51小萌新z 發(fā)表于 2019-4-22 00:23
很好的資料，加油

輸出口是哪個(gè)啊

ID:587285 · 發(fā)表于 2019-7-20 11:21

該怎么接單片機(jī)啊，求大佬解釋

ID:462629 · 發(fā)表于 2021-12-28 10:00

能補(bǔ)全電路原理圖,源碼嗎

帳號(hào)		自動(dòng)登錄	找回密碼
密碼			立即注冊(cè)

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

基于STC8A8K單片機(jī)的PID舵機(jī)小車控制程序源碼

評(píng)分