四旋翼源碼第7版軟啟動+藍牙源碼（串級PID控制）

ID:328123 · 發表于 2018-5-12 11:21

這是我們調小四軸的源碼。主要用的串級PID控制。

STM32-DMP移植單片機源程序如下:

#include "delay.h"
#include "sys.h"
#include "usart.h"
#include "anbt_dmp_fun.h"
#include "anbt_i2c.h"
#include "anbt_dmp_mpu6050.h"
#include "anbt_dmp_driver.h"
#include "debug.h"
#include "PWM.h"
#define BYTE0(dwTemp) (*(char *)(&dwTemp))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))
#define q30 1073741824.0f
void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel);
void Send_Data(int16_t *Gyro,int16_t *Accel);
static void DMP_Delay ( __IO uint32_t nCount )
{
for ( ; nCount != 0; nCount -- );
}
int main(void)
{
int a = 0;
float error_pitch=0.0;
static float Pitch_I_out=0.0;
float error_pitch_old=0.0;
static float Pitch_I_shell_out=0.0;
float error_pitch_shell=0.0;
float pitch_shell_out=0.0;
float pitch_out=0.0;
float error_roll=0.0;
static float roll_I_out=0.0;
float error_roll_old=0.0;
static float roll_I_shell_out=0.0;
float error_roll_shell=0.0;
float roll_shell_out=0.0;
float roll_out=0.0;
static float yaw_I_out=0.0;
float error_yaw=0.0;
float error_yaw_old=0.0;
float yaw_out=0.0;
int i=1000000;
float m1=0,m2=0,m3=0,m4=0,m5=0.0;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;//陀螺儀存放數組，加速度存放數組，返回狀態量
unsigned char more;
long quat[4];//四元數存放數組
float Yaw=0.00,Roll,Pitch;//歐拉角
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;//計算姿態過程用到的變量
////
pitch_SET = 7;
P_pitch_shell =-25;//-55
P_pitch = -0.72; //-1.2
I_pitch= -0.015;// -0.032
D_pitch=-0.85;//-1.2
I_pitch_shell =-0.00012;//-0.0002
roll_SET = 9;
P_roll_shell= -25;//-55
P_roll = 0.72;//1.2
I_roll= 0.015;//0.032
D_roll= 0.85;//1.2
I_roll_shell = 0.00012;//0.0002
NVIC_Configuration();//設置NVIC中斷分組2:2位搶占優先級，2位響應優先級
uart_init(115200); //串口初始化為115200
//引用圓點博士的I2C程序，這里跟我們平常沒有什么區別
PWM_Init();
ANBT_I2C_Configuration(); //IIC初始化
// delay_ms(30);
DMP_Delay (300);//
AnBT_DMP_MPU6050_Init(); //6050DMP初始化
//TIM2_Int_Init(4999,7199);//10Khz的計數頻率，計數到5000為500ms
DMP_Delay (300);
while(i--)
{
TIM_SetCompare3(TIM3,200);
TIM_SetCompare4(TIM3,200);
TIM_SetCompare3(TIM4,200);
TIM_SetCompare4(TIM4,200);
}
//a = ReceiveOneByte[0];
while(1)
{
error_pitch_old = error_pitch;
error_roll_old = error_roll;
error_yaw_old = error_yaw;
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
if ( sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //感覺沒有價值，注掉
// if(Pitch>15||Pitch<-15||Roll>15||Roll<-15)//stop
// {
// TIM_SetCompare3(TIM3,0);
// TIM_SetCompare4(TIM3,0);
// TIM_SetCompare3(TIM4,0);
// TIM_SetCompare4(TIM4,0);
// while(1);
// }
//////////////////Pitch外環(PI)/////////////
error_pitch_shell = Pitch-pitch_SET;
Pitch_I_shell_out+=error_pitch_shell;
if(Pitch_I_shell_out>500)
{
Pitch_I_shell_out = 500;
}
if(Pitch_I_shell_out<-500)
{
Pitch_I_shell_out = -500;
}
pitch_shell_out = P_pitch_shell*error_pitch_shell+I_pitch_shell*Pitch_I_shell_out;
//------------Pitch內環PID gyro[1]---------------//
error_pitch = gyro[1]-pitch_shell_out;
Pitch_I_out+=error_pitch;
if(Pitch_I_out>500)
{
Pitch_I_out = 500;
}
if(Pitch_I_out<-500)
{
Pitch_I_out = -500;
}
pitch_out = P_pitch*error_pitch+I_pitch*Pitch_I_out+D_pitch*(error_pitch-error_pitch_old);
m1=pitch_out;
//////////////////roll外環(PI)gyro[0]/////////////
error_roll_shell = Roll - roll_SET;
roll_I_shell_out+=error_roll_shell;
if(roll_I_shell_out>1000)
{
roll_I_shell_out = 1000;
}
if(roll_I_shell_out<-1000)
{
roll_I_shell_out = -1000;
}
roll_shell_out = P_roll_shell*error_roll_shell+I_roll_shell*roll_I_shell_out;
//////////////////roll內環PID/////////
error_roll = gyro[0]-roll_shell_out;
roll_I_out+=error_roll;
if(roll_I_out>1000)
{
roll_I_out = 1000;
}
if(roll_I_out<-1000)
{
roll_I_out = -1000;
}
roll_out = P_roll*error_roll+I_roll*roll_I_out+D_roll*(error_roll-error_roll_old);
m2 = roll_out;
////////////////gyro[2] PD//////////
error_yaw = gyro[2];
yaw_out = 1.8*error_yaw+2.5*(error_yaw_old-error_yaw);
m3 = yaw_out;
/////////////////////yaw///////////
// error_yaw = Yaw;
// yaw_I_out+=error_yaw;
// if(yaw_I_out>200)
// {
// yaw_I_out = 200;
// }
// if(yaw_I_out<-200)
// {
// yaw_I_out = -200;
// }
// m4 = 0.0000025*error_yaw+0.000000025*yaw_I_out;
TIM_SetCompare3(TIM3,2500+m1+m2+m3+m4+m5);
TIM_SetCompare4(TIM3,2500+m1-m2-m3-m4+m5);
TIM_SetCompare3(TIM4,2500-m1-m2+m3+m4+m5);
TIM_SetCompare4(TIM4,2500-m1+m2-m3-m4+m5);
Push(1,(int)pitch_out);
Push(2,(int)Pitch);
Push(3,(int)gyro[0]);
Push(4,(int)gyro[1]);
Push(5,(int)gyro[2]);
Push(6,(int)Yaw);
SendDataToScope();
}
}
}
/*函數功能：根據匿名最新上位機協議寫的顯示姿態的程序
*具體原理看匿名的講解視頻
發送基本信息（姿態、鎖定狀態）
*/
void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel)
{
unsigned char i=0;
unsigned char _cnt=0,sum = 0;
unsigned int _temp;
u8 data_to_send[50];
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(Roll*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = 0-(int)(Pitch*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Yaw*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
//和校驗
for(i=0;i<_cnt;i++)
sum+= data_to_send[i];
data_to_send[_cnt++]=sum;
//串口發送數據
for(i=0;i<_cnt;i++)
AnBT_Uart1_Send_Char(data_to_send[i]);
}
//發送傳感器數據
void Send_Data(int16_t *Gyro,int16_t *Accel)
{
unsigned char i=0;
unsigned char _cnt=0,sum = 0;
// unsigned int _temp;
u8 data_to_send[50];
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復制代碼

全部資料51hei下載地址：

第7版軟啟動+藍牙.rar (396.1 KB, 下載次數: 58)

ID:405372 · 發表于 2018-11-30 17:22

下載試試看，最近在學

ID:70035 · 發表于 2019-8-29 12:57

最近在玩軟啟動，了解一下。

帳號		自動登錄	找回密碼
密碼			立即注冊

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

四旋翼源碼第7版軟啟動+藍牙源碼（串級PID控制）