基于STC15W的风力摆控制系统设计带视频与程序（电赛测控题） -

//========================================================================
// 城建学院电子实验室
// 修改日期2017.07.30
// 版本V1.0
//========================================================================
//特别声明：此程序
//
//
//
// MCU工作频率:24MHz
//=========================================================================
#include <STC15F2K60S2.h> //STC15系列通用头文件
#include <intrins.h> //STC特殊命令符号声明
#include <MPU6050.H> //MPU6050数字陀螺仪
#include <STC15W4KPWM.H> //单片机所有IO口初始化-PWM口初始化
#include <IMU.H>
#include <math.H>
bit busy,flag=0;
int ich1=0,ich2=0,ich3=0,ich4=0,ich5=0,ich6=0; //匿名科技上位机
//==================================================//
// PID算法变量
//==================================================//
int speed1=0,speed2=0,speed3=0,speed4=0;//电机速度参数
int PWM1=0,PWM2=0,PWM3=0,PWM4=0; //加载至PWM模块的参数
float Angle=0;
char mode=1;
//*****************角度参数*************************************************
float Last_Angle_gx=0; //外上一次陀螺仪数据
float Last_Angle_gy=0; //上一次陀螺仪数据
double Gyro_y=0,Gyro_x=0,Gyro_z=0; //Y轴陀螺仪数据暂存
double Accel_x=0,Accel_y=0,Accel_z=0; //X轴加速度值暂存
double Angle_ax=0,Angle_ay=0,Angle_az=0; //由加速度计算的加速度(弧度制)
double Angle_gy=0,Angle_gx=0,Angle_gz=0; //由角速度计算的角速率(角度制)
float data AngleX=0,AngleY=0; //四元数解算出的欧拉角
double Ax=0,Ay=0; //加入遥控器控制量后的角度
double Vx=0,Vy=0; //加入遥控器控制量后的角度
float ierrorx=0,ierrory=0; //误差积分积分根据情况，目前测试情况不需要积分
//****************姿态处理和PID*********************************************
float FR1=0,FR2=0; //xy轴期望角度
float data PID_Output; //PID最终输出量
float xdata Last_Ax=0;
float xdata Last_Ay=0;
float xdata Last_vx=0;
float xdata Last_vy=0;
float R=0.20f;//摆幅半长（m）
unsigned int time=0;
float A1=0,A2=0;
sbit key1=P2^0;
sbit key2=P2^1;
sbit key3=P2^2;
sbit key4=P2^3;
//==================================================//
// PID 手动微调参数值
//==================================================//
// P值适当调，D值可以给很大!!! P值过大系统就会荡
#define Out_XP 4.5f
#define Out_XI 0.0f //I根据情况，目前不需要I
#define Out_XD 2200.0f
#define Out_YP Out_XP
#define Out_YI Out_XI
#define Out_YD Out_XD
#define PI 3.15459
#define T 1601//单摆周期(ms) 根据单摆公式用杆长计算
//--------------------------------------------------//
// PID算法飞控自平衡函数
//--------------------------------------------------//
void Flight(void)interrupt 1
{
time+=10;
Gyro_x = GetData(GYRO_XOUT_H);//读出 X轴陀螺仪数据
Gyro_y = GetData(GYRO_YOUT_H);//读出 Y轴陀螺仪数据
Gyro_z = GetData(GYRO_ZOUT_H);//读出 Z轴陀螺仪数据
Accel_y= GetData(ACCEL_YOUT_H);//读出 X轴加速度数据
Accel_x= GetData(ACCEL_XOUT_H);//读出 Y轴陀螺仪数据
Accel_z= GetData(ACCEL_ZOUT_H);//读出 Z轴加速度数据
//姿态数据算法（借鉴STC官方算法）
Last_Angle_gx=Angle_gx; //储存上一次角速度数据
Last_Angle_gy=Angle_gy; //储存上一次角速度数据
Angle_ax=(Accel_x)/8192; //加速度处理
Angle_az=(Accel_z)/8192; //加速度量程 +-4g/S
Angle_ay=(Accel_y)/8192; //转换关系 8192LSB/g
Angle_gx=(Gyro_x)/65.5+4; //陀螺仪处理
Angle_gy=(Gyro_y)/65.5+3; //陀螺仪量程 +-500度/S
Angle_gz=(Gyro_z)/65.5; //转换关系65.5LSB/度
//***********************************四元数解算***********************************
IMUupdate(Angle_gx*0.0174533,Angle_gy*0.0174533,Angle_gz*0.0174533,Angle_ax,Angle_ay,Angle_az);//0.174533为PI/180 目的是将角度转弧度
//****平衡算法***********************************************************************
FR1=A1*sin(2*PI*time/T); //X轴控制
if(mode==3) FR2=A2*cos(2*PI*time/T); //Y轴控制
else if(mode==4) FR2=A2*cos(2*PI*time/(T/2)); //Y轴控制
else FR2=A2*sin(2*PI*time/T); //Y轴控制
if(time >= 1601)time=0;
//************** MPU6050 X轴指向 ***********************************************************
Ax=FR1-(AngleX+2); //X轴偏差
ich1=Ax;
ich2=Ay;
ich3=0;
ich4=0;
ich5=0;
ich6=0;
ierrorx +=Last_Ax-Ax;
PID_Output = Ax*Out_XP +Out_XI*ierrorx+(Last_Ax-Ax)*Out_XD; //PID
if(PID_Output > 1000) PID_Output = 1000; //输出量限幅
if(PID_Output < -1000) PID_Output = -1000;
speed1 =0+ PID_Output; speed3 =0- PID_Output;
Last_Ax=Ax;
//**************MPU6050 Y轴指向**************************************************
Ay=FR2-(AngleY); //Y轴偏差
ierrory +=Last_Ay-Ay;
PID_Output = Ay*Out_YP +Out_YI*ierrory+(Last_Ay-Ay)*Out_XD; //PID
if(PID_Output > 1000) PID_Output = 1000; //输出量限幅
if(PID_Output < -1000) PID_Output = -1000;
speed2 =0+ PID_Output; speed4 = 0- PID_Output;
Last_Ay=Ay;
//************************PID控制数值***************************************************
//**************将速度参数加载至PWM模块*************************************************
//速度参数控制，防止超过PWM参数范围0-1000
if(speed1>1000)speed1=1000;else if(speed1<0)speed1=0;PWM1=(speed1); // 5
if(speed2>1000)speed2=1000;else if(speed2<0)speed2=0;PWM2=(speed2); // 3
if(speed3>1000)speed3=1000;else if(speed3<0)speed3=0;PWM3=(speed3); // 4
if(speed4>1000)speed4=1000;else if(speed4<0)speed4=0;PWM4=(speed4); // 2
PWM(1000-PWM1,1000-PWM2,1000-PWM3,1000-PWM4);
}
//--------------------------------------------------//
// 时间延时函数
//--------------------------------------------------//
void Delay(unsigned int x)
{
unsigned int i,j;
for(i=0;i<x;i++)
for(j=0;j<250;j++);
}
//--------------------------------------------------//
// 定时器0 初始化函数 V2.0
//--------------------------------------------------//
void Timer0Init(void) //10毫秒@24.000MHz
{
TR0 = 0;
AUXR &= 0x7F; //定时器时钟12T模式
TMOD &= 0xF0; //设置定时器模式
IE = 0x82;
TL0 = 0xE0; //设置定时初值
TH0 = 0xB1; //设置定时初值
TF0 = 0; //清除TF0标志
TR0 = 1; //定时器0开始计时
}
//--------------------------------------------------//
// 上位机串口初始化函数 V2.0
// 波特率115200,24MHZ
//--------------------------------------------------//
void Usart_Init() //波特率115200,24MHZ
{
SCON = 0x50;
AUXR |= 0x40;
AUXR &= 0xFE;
TMOD &= 0x0F;
TL1 = 0xcc;
TH1 = 0xFF;
ET1 = 0;
TR1 = 1;
}
void SendData(unsigned char dat)
{
while (busy);
busy = 1;
SBUF = dat;
}
void Send(int Ax,int Ay,int Az,int Gx,int Gy,int Gz)
{
unsigned char sum=0;
ES = 1;
SendData(0xAA);
SendData(0xAA);
SendData(0x02);
SendData(12);
SendData(Ax>>8);
SendData(Ax);
SendData(Ay>>8);
SendData(Ay);
SendData(Az);
SendData(Az>>8);
SendData(Gx);
SendData(Gx>>8);
SendData(Gy);
SendData(Gy>>8);
SendData(Gz);
SendData(Gz>>8);
sum+=0xAA;sum+=0xAA;sum+=0x02;sum+=12;
sum+=Ax>>8;sum+=Ax;sum+=Ay>>8;sum+=Ay;sum+=Az>>8;sum+=Az;
sum+=Gx>>8;sum+=Gx;sum+=Gy>>8;sum+=Gy;sum+=Gz>>8;sum+=Gz;
SendData(sum);
ES = 0;
}
void mode1()
{
A1=0;
A2=0;
}
void mode2()
{
A1=(atan(R*cos(PI/180*Angle)/0.75))* 57.2957795f;//根据摆幅算出角度A 0.75m摆长
A2=(atan(R*sin(PI/180*Angle)/0.75))* 57.2957795f;//根据摆幅算出角度A
/*if(0==key3)
{
Delay(10);
if(key3==0) Angle+=15;
if(Angle>=360) Angle=0;
while(!key1);
}
if(key4==0)
{
Delay(10);
if(key4==0) Angle-=15;
if(Angle<=-360) Angle=0;
while(!key4);
}*/
}
void mode3()
{
A1=(atan(R*cos(PI/180*Angle)/0.75))* 57.2957795f;//根据摆幅算出角度A 0.75m摆长
/*if(key3==0)
{
Delay(10);
if(key3==0) flag=~flag;
while(!key3);
}*/
if(!flag) A2=A1;
else A2=0-A1;
}
void mode4()
{
A1=(atan(R*cos(PI/180*Angle)/0.75))* 57.2957795f;//根据摆幅算出角度A 0.75m摆长
A2=A1/2;
}
//--------------------------------------------------//
// 程序主函数
//--------------------------------------------------//
void main(void)
{
PWMGO(); //初始化PWM
Usart_Init();//串口初始化
Delay(10); // 延时 100
InitMPU6050(); //初始化MPU-6050
Delay(100); // 延时 100
Timer0Init(); //初始化定时器
EA = 1;
while(1)
{
Send(ich1,ich2,ich3,ich4,ich5,ich6); //串口发送数据
Delay(1);
/*if(key1==0)
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码

//========================================================================
// 城建学院电子实验室
// 修改日期2017.07.30
// 版本V1.0
//========================================================================
//特别声明：此程序
//
//
//
// MCU工作频率:24MHz
//=========================================================================
#include <STC15F2K60S2.h> //STC15F2K60S2 STC15系列通用头文件
#include <intrins.h>
#include <MPU6050.H>
sbit SCL=P3^5; //IIC时钟引脚定义
sbit SDA=P3^4; //IIC数据引脚定义
typedef unsigned char uchar;
typedef unsigned char uint;
void InitMPU6050(); //初始化MPU6050
void Delay2us();
void I2C_Start();
void I2C_Stop();
bit I2C_RecvACK();
void I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
uchar Single_ReadI2C(uchar REG_Address); //读取I2C数据
void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C写入数据
//I^C时序中延时设置，具体参见各芯片的数据手册 6050推荐最小1.3us 但是会出问题，这里延时实际1.9us左右
void Delay2us()
{
unsigned char i;
i = 11;
while (--i);
}
//**************************************
//I2C起始信号
//**************************************
void I2C_Start()
{
SDA = 1; //拉高数据线
SCL = 1; //拉高时钟线
Delay2us(); //延时
SDA = 0; //产生下降沿
Delay2us(); //延时
SCL = 0; //拉低时钟线
}
//**************************************
//I2C停止信号
//**************************************
void I2C_Stop()
{
SDA = 0; //拉低数据线
SCL = 1; //拉高时钟线
Delay2us(); //延时
SDA = 1; //产生上升沿
Delay2us(); //延时
}
//**************************************
//I2C接收应答信号
//**************************************
bit I2C_RecvACK()
{
SCL = 1; //拉高时钟线
Delay2us(); //延时
CY = SDA; //读应答信号
SCL = 0; //拉低时钟线
Delay2us(); //延时
return CY;
}
//**************************************
//向I2C总线发送一个字节数据
//**************************************
void I2C_SendByte(uchar dat)
{
uchar i;
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1; //移出数据的最高位
SDA = CY; //送数据口
SCL = 1; //拉高时钟线
Delay2us(); //延时
SCL = 0; //拉低时钟线
Delay2us(); //延时
}
I2C_RecvACK();
}
//**************************************
//从I2C总线接收一个字节数据
//**************************************
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1; //使能内部上拉,准备读取数据,
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1;
SCL = 1; //拉高时钟线
Delay2us(); //延时
dat |= SDA; //读数据
SCL = 0; //拉低时钟线
Delay2us(); //延时
}
return dat;
}
//**************************************
//向I2C设备写入一个字节数据
//**************************************
void Single_WriteI2C(uchar REG_Address,uchar REG_data)
{
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //内部寄存器地址，
I2C_SendByte(REG_data); //内部寄存器数据，
I2C_Stop(); //发送停止信号
}
//**************************************
//从I2C设备读取一个字节数据
//**************************************
uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //发送存储单元地址，从0开始
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号
REG_data=I2C_RecvByte(); //读出寄存器数据
SDA = 1; //写应答信号
SCL = 1; //拉高时钟线
Delay2us(); //延时
SCL = 0; //拉低时钟线
Delay2us(); //延时
I2C_Stop(); //停止信号
return REG_data;
}
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07); //陀螺仪125hz
Single_WriteI2C(CONFIG, 0x04); //21HZ滤波延时A8.5ms G8.3ms 此处取值应相当注意，延时与系统周期相近为宜
Single_WriteI2C(GYRO_CONFIG, 0x08); //陀螺仪500度/S 65.5LSB/g
Single_WriteI2C(ACCEL_CONFIG, 0x08);//加速度+-4g 8192LSB/g
}
//**************************************
//合成数据
//**************************************
int GetData(uchar REG_Address)
{
char H,L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return (H<<8)+L; //合成数据
}

复制代码