基于STC8A8K单片机的PID舵机小车控制程序源码 -

#include<stc8.h>
#include<math.h>
#define PWMA1 P20
#define PWMA2 P21
#define PWMB1 P22
#define PWMB2 P23
#define SERVO P24 //舵机引脚
#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; //左右编码器的脉冲计数
float Velocity,Velocity_Set,Angle,Angle_Set;
unsigned long count0,count1,count2,count3,Sensor;//编码器脉冲计数
int Motor_A,Motor_B,Servo,Target_A,Target_B; //电机舵机控制相关
/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回值：unsigned int
**************************************************************************/
int myabs(int a)//绝对值函数
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
函数功能：电机舵机占空比
**************************************************************************/
void LmotorPWM(u8 D0,u8 D1)//左电机PWM占空比
{
P_SW2=0X80;
PWMCKS=0X09;//PWM时钟为系统时钟
PWMC=0XFD3F;//设置PWM周期为64831个脉冲,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)//右电机PWM占空比
{
P_SW2=0X80;
PWMCKS=0X0E;//PWM时钟为系统时钟
PWMC=0XFD3F;//设置PWM周期为12C0H个脉冲,T=200US
PWM2T1=D2*648;
PWM3T1=D3*648;
PWM2CR=0X80;//使能PWM2输出，且初始电平为低电平
PWM3CR=0X80;//使能PWM3输出，且初始电平为低电平
P_SW2=0X00;
PWMCR=0X80;
}
void DJPWM(u8 D4)//舵机PWM占空比
{
P_SW2=0X80;
PWMCKS=0X0E;//PWM时钟为系统时钟
PWMC=0XFD3F;//设置PWM周期为12C0H个脉冲,T=200US
PWM4T2=0x0380;//在0.5MS时赋值高电平
PWM4T1=0x0380+17.7*D4;
PWM4CR=0X80;//使能PWM2输出，且初始电平为低电平
P_SW2=0X00;
PWMCR=0X80;
}
/**************************************************************************
函数功能：小车运动学分析
**************************************************************************/
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; //舵机转向
}
/**************************************************************************
函数功能：占空比设置
**************************************************************************/
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;
}
/**************************************************************************
函数功能：电机PI控制
**************************************************************************/
int PI_A (int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Target-Encoder; //计算偏差
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; //计算偏差
Pwm+=0.015*(Bias-Last_bias)+0.015*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
/**************************************************************************
函数功能：电机占空比限制幅度
**************************************************************************/
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; //舵机限幅
if(Servo>(120)) Servo=120; //舵机限幅
}
/**************************************************************************
函数功能：计算偏差角度
**************************************************************************/
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; //上一次的偏差
}
/**************************************************************************
函数功能：编码器计数
**************************************************************************/
void PCA_C()
{
count0=0;
count1=0;
CCON=0X00;
CMOD=0X09;//设定为系统时钟，使能PCA溢出中断
CL=0X00;
CH=0X00;
CCAPM0=0X21;//设置PCA0模块为16位捕获模式，且为上升沿捕获
CCAPM1=0X21;//设置PCA1模块为16位捕获模式，且为上升沿捕获
CCAPM2=0X21;//设置PCA2模块为16位捕获模式，且为上升沿捕获
CCAPM3=0X21;//设置PCA3模块为16位捕获模式，且为上升沿捕获
CCAP0L=0X00;
CCAP0H=0X00;
CCAP1L=0X00;
CCAP1H=0X00;
CCAP2L=0X00;
CCAP2H=0X00;
CCAP3L=0X00;
CCAP3H=0X00;
CR=1;//启动PCA
EA=1;
}
/**************************************************************************
函数功能：电机运动控制算法
**************************************************************************/
void control(void)
{
Encoder_Left=count0; //===读取编码器的值 //为了保证M法测速的时间基准，首先读取编码器数据
Encoder_Right=count1; //===读取编码器的值
Get_RC();
Analysis(Velocity,Angle); //小车运动学分析
Motor_A=PI_A(Encoder_Left,Target_A); //===速度闭环控制计算电机A最终PWM
Motor_B=PI_B(Encoder_Right,Target_B); //===速度闭环控制计算电机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++;
}
}

复制代码