//四川 绵阳 西南科技大学 信息工程学院 电气13级 刘其民 QQ:1203933924
//硬件参数:
//电池:1S/3.7V电池 推荐300-500mAh左右
//电机/桨:720空心杯/59MM桨
//MCU [email protected]
//陀螺仪加速度计:MPU-6050
//磁场传感计:HMC5883L
//无线芯片:NRF24L01
//电机驱动MOS管:AO3400
//MOS管保护用肖特基:BAT54ST
//下载口保护用瞬态抑制二极管:RCLAMP0524P
//机架尺寸:94mm*94mm
//标准PID控制公式:PID=P*e(n)+I*[(e(n)+e(n-1)+...+e(0)]+D*[e(n)-e(n-1)]
//数据定义说明:
//data 51单片机片内RAM最前面128字节RAM 用ACC读写,速度最快
//idata 片内RAM最前面256字节的RAM 包括data 用类似指针模式访问 适合用于指针操作
//pdata 外部扩展RAM的前256字节的RAM
//xdata 外部扩展RAM 用DPTR访问
#include <STC15W4K60S4.H>
#include <intrins.h>
#include <NRF24L01.H>
#include <MPU6050.H>
#include <math.h>
#include <STC15W4KPWM.H>
void Time0_Init(); //定时器初始化
void update(); //陀螺仪矫正
void Kalman_Filter(float Accel,float Gyro); //X轴卡尔曼滤波
void Kalman_Filter_Y(float Accel,float Gyro); //Y轴卡尔曼滤波
//*************************************自整定PID相关参数************************************************
//unsigned char xdata ERRORPID[512]={0};
//unsigned char xdata ZSYPID[100]={0};
//unsigned long int xdata ALLERROR[100]={0};
//unsigned long int TE=0,TZA=0,ADD=0,THEEND=0,LESTERROR=0;
//float BESTPID=0;
//******************************************************************************************************
unsigned char PID=0; //无线串口PID调整延时用
int ich1=0,ich2=0,ich3=0,ich4=0; //无线串口待发送数据
int speed0=0,speed1=0,speed2=0,speed3=0,V=0; //电机控制参数
int PWM0=0,PWM1=0,PWM2=0,PWM3=0; //电机控制参数
unsigned char TxBuf[20]={0};
unsigned char RxBuf[20]={0};
double g_x=0,g_y=0,g_z=0; //陀螺仪静差消除参数
double PID_x=0,PID_y=0,PID_z=0; //每根轴PID控制量输出
//******角度参数****************************************************************************************
double Angle_ax=0,Angle_ay=0,Angle_az=0; //由加速度计算的倾斜角度
double Angle_gy=0,Angle_gx=0,Angle_gz=0; //由角速度计算的倾斜角度
double AngleAx=0,AngleAy=0; //反三角函数处理后的角度值
double Angle=0,Angley=0; //最终倾斜角度
double Anglelate=0,Anglelatey=0,Anglezlate=0;//存储上一次角度数据
double Ax=0,Ay=0; //加入遥控器控制量后的角度
float idata dt=0.008; //系统周期
//******卡尔曼滤波参数-X轴******************************************************************************
float data Q_angle=0.001; //对加速度计的信任度
float data Q_gyro=0.003; //对陀螺仪的信任度
float data R_angle=0.5;
char data C_0 = 1;
float data Q_bias, Angle_err;
float data PCt_0, PCt_1, E;
float data K_0, K_1, t_0, t_1;
float data Pdot[4] ={0,0,0,0};
float data PP[2][2] = { { 1, 0 },{ 0, 1 } };
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt; //陀螺仪积分角度(测量值-陀螺仪偏差)*dt
//Angle相当于是系统的预测值
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // 先验估计误差协方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]= Q_gyro;
PP[0][0] += Pdot[0] * dt; // 先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;//估计值与预测值之间的偏差
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;//卡尔曼增益1 用于计算最优估计值
K_1 = PCt_1 / E;//卡尔曼增益2 用于计算最后估计的偏差
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //更新协方差矩阵
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err;//根据卡尔曼增益1算出最优角度
Q_bias += K_1 * Angle_err;//根据卡尔曼增益2算出预测值偏差
}
//******卡尔曼滤波参数-Y轴***************************************************************************
float data Q_angley=0.001;
float data Q_gyroy=0.003;
float data R_angley=0.5;
char data C_0y = 1;
float idata Q_biasy, Angle_erry;
float idata PCt_0y, PCt_1y, Ey;
float idata K_0y, K_1y, t_0y, t_1y;
float idata Pdoty[4] ={0,0,0,0};
float idata PPy[2][2] = { { 1, 0 },{ 0, 1 } };
void Kalman_Filter_Y(float Accely,float Gyroy)
{
Angley+=(Gyroy - Q_biasy) * dt;
Pdoty[0]=Q_angley - PPy[0][1] - PPy[1][0];
Pdoty[1]=- PPy[1][1];
Pdoty[2]=- PPy[1][1];
Pdoty[3]=Q_gyroy;
PPy[0][0] += Pdoty[0] * dt;
PPy[0][1] += Pdoty[1] * dt;
PPy[1][0] += Pdoty[2] * dt;
PPy[1][1] += Pdoty[3] * dt;
Angle_erry = Accely - Angley;
PCt_0y = C_0y * PPy[0][0];
PCt_1y = C_0y * PPy[1][0];
Ey = R_angley + C_0y * PCt_0y;
K_0y = PCt_0y / Ey;
K_1y = PCt_1y / Ey;
t_0y = PCt_0y;
t_1y = C_0y * PPy[0][1];
PPy[0][0] -= K_0y * t_0y;
PPy[0][1] -= K_0y * t_1y;
PPy[1][0] -= K_1y * t_0y;
PPy[1][1] -= K_1y * t_1y;
Angley += K_0y * Angle_erry;
Q_biasy += K_1y * Angle_erry;
}
//************姿态处理和PID**************************************************************************
float idata YP=4.5,YD=115.0,YI=0.05,ERRORX;
float idata XP=4.5,XD=115.0,XI=0.05,ERRORY;
float idata ZP=4.0,ZD=2.0;
void Angle_Calcu(void)
{
Angle_ax=(GetData(ACCEL_XOUT_H))/8192; //加速度处理
Angle_az=(GetData(ACCEL_ZOUT_H))/8192; //加速度量程 +-4g/S
Angle_ay=(GetData(ACCEL_YOUT_H))/8192; //转换关系8192LSB/g
Angle_gx=(GetData(GYRO_XOUT_H)-g_x)/65.5; //陀螺仪处理
Angle_gy=(GetData(GYRO_YOUT_H)-g_y)/65.5; //陀螺仪量程 +-500度/S
Angle_gz=(GetData(GYRO_ZOUT_H)-g_z)/65.5; //转换关系65.5LSB/度
//**************X轴指向*******************************************************************************
AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*180/3.141592657;
Anglelate=Ax;
//Angle=0.95*(Angle-Angle_gy*dt)+0.05*AngleAx;//一阶互补滤波 这边是-Angle_gy
Kalman_Filter(AngleAx,-Angle_gy); //卡尔曼滤波
Ax=Angle+((float)RxBuf[1]-128)/7;
ERRORX+=3*Ax; //加强静差积分强度但是不改变输出控制量的范围
if(ERRORX>1000){ERRORX=1000;}if(ERRORX<-1000){ERRORX=-1000;}
PID_y=Ax*XP+ERRORX*XI+(Ax-Anglelate)*XD;
speed0=0+PID_y,speed2=0-PID_y;
//**************Y轴指向*******************************************************************************
AngleAy=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*180/3.141592657;
Anglelatey=Ay;
//Angley=0.95*(Angley+Angle_gx*dt)+0.05*AngleAy;//一阶互补滤波 这边是+Angle_gx
Kalman_Filter_Y(AngleAy,Angle_gx); //卡尔曼滤波
Ay=Angley+((float)RxBuf[2]-128)/7-RxBuf[4]/20; //由于结构问题,油门越大,Y轴加速度计输出偏差越大,RxBuf[4]补偿之
ERRORY+=3*Ay; //加强静差积分强度但是不改变输出控制量的范围
if(ERRORY>1000){ERRORY=1000;}if(ERRORY<-1000){ERRORY=-1000;}
PID_x=Ay*YP+ERRORY*YI+(Ay-Anglelatey)*YD;
speed3=0+PID_x,speed1=0-PID_x;
//**************Z轴指向*******************************************************************************
Angle_gz+=(RxBuf[3]-128)/10;
PID_z=(Angle_gz)*ZP+(Angle_gz-Anglezlate)*ZD;
Anglezlate=Angle_gz;
speed0=speed0+PID_z,speed2=speed2+PID_z;
speed1=speed1-PID_z,speed3=speed3-PID_z;
//*****************无线串口相关***********************************************************************
ich1=Ax;
ich2=Ay;
ich3=AngleAy;
ich4=XD;
//**************速度更新******************************************************************************
if((1000-RxBuf[4]*4+speed0)>1000)PWM0=1000;
else if((1000-RxBuf[4]*4+speed0)<0)PWM0=0;
else PWM0=(1000-RxBuf[4]*4+speed0);
if((1000-RxBuf[4]*4+speed1)>1000)PWM1=1000;
else if((1000-RxBuf[4]*4+speed1)<0)PWM1=0;
else PWM1=(1000-RxBuf[4]*4+speed1);
if((1000-RxBuf[4]*4+speed2)>1000)PWM2=1000;
else if((1000-RxBuf[4]*4+speed2)<0)PWM2=0;
else PWM2=(1000-RxBuf[4]*4+speed2);
if((1000-RxBuf[4]*4+speed3)>1000)PWM3=1000;
else if((1000-RxBuf[4]*4+speed3)<0)PWM3=0;
else PWM3=(1000-RxBuf[4]*4+speed3);
if(RxBuf[4]>=
没有合适的资源?快使用搜索试试~ 我知道了~
STC-four-axis-platform.rar_STC_four_stc 四轴_stc 四轴_stc15四轴代码
共55个文件
lst:11个
obj:11个
c:10个
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
0 下载量 30 浏览量
2022-09-23
00:37:06
上传
评论
收藏 187KB RAR 举报
温馨提示
STC四轴源码,采用的是stc15w4k58s
资源详情
资源评论
资源推荐
收起资源包目录
STC-four-axis-platform.rar (55个子文件)
四轴-四元数测试版
四轴Rev7.hex 27KB
Timer.C 322B
四轴Rev7.0.uvgui_LQM.bak 78KB
MPU-6050.C 5KB
四轴Rev7.lnp 165B
四元数.LST 878B
STC15W4KPWM.C 4KB
四元数.OBJ 19KB
四轴Rev7.0.uvproj 15KB
四轴Rev7.0_uvproj.bak 15KB
EEPROM.LST 5KB
NRF24L01.LST 13KB
STC15W4KPWM.H 163B
STARTUP.LST 14KB
四轴Rev7.MAP 179KB
EEPROM.C 2KB
四轴Rev7.build_log.htm 837B
EEPROM.OBJ 15KB
USART.H 158B
MPU6050.H 1KB
四轴Rev7.0.C 10KB
四元数.c 66B
sys.h 0B
四轴Rev7.SBR 70KB
NRF24L01.H 620B
Timer.LST 1KB
四轴 Rev7.0.OBJ 35KB
STC15W4KPWM.LST 7KB
四轴Rev7.0.uvopt 9KB
IMU.H 150B
四轴Rev7.0 128KB
USART.OBJ 12KB
四轴Rev7.0.OBJ 36KB
四轴Rev7.0.LST 16KB
NRF24L01.OBJ 19KB
四轴Rev7.0_uvopt.bak 9KB
USART.LST 3KB
四轴Rev7.M51 59KB
STARTUP.A51 6KB
MPU-6050.LST 8KB
四轴 Rev7.0.LST 14KB
NRF24L01.C 9KB
MPU-6050.OBJ 17KB
IMU.LST 4KB
USART.C 1KB
Timer.H 91B
STC15W4KPWM.OBJ 15KB
四轴 Rev7.0.C 9KB
四轴Rev7.0.uvgui.LQM 78KB
STARTUP.OBJ 749B
IMU.OBJ 20KB
IMU.C 2KB
EEPROM.H 269B
STC15W4K60S4.H 13KB
Timer.OBJ 7KB
共 55 条
- 1
JaniceLu
- 粉丝: 79
- 资源: 1万+
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论0