国产精品TV在线麻豆,久青草国产在视频在线观看,日韩一区二区三区精品视频

//四川綿陽西南科技大學信息工程學院電氣13級
//本人只提供有限技術支持,截止時間：2015年3月1號,過時不候，請勿打擾。
//本人今年大二上期，由于技術有限提問時請不要超過本程序及本硬件的范圍，什么捷聯(lián)慣導技術，擴展卡爾曼，梯度下降濾波
//就不要問了，我也不懂，最近在自學自動控制原理，還是個小渣渣，連STM32都不會，做個四軸都還是只能苦逼地用51單片機
//本程序屬于完全開源程序，允許用于進行二次開發(fā)，但是不能做細小改動后就聲明自己擁有版權，
//對于抄襲后又聲明自己有版權是自己開發(fā)的我只想說我QNMLGB。
//使用本程序應遵循GNUGPL協(xié)議！
//本程序屬于本硬件的最終版本，不會繼續(xù)升級，但是會不定期的發(fā)布修復Bug后的程序。
//特別鳴謝在研發(fā)過程中給予支持的朋友及團體：
// 西南科技大學電氣09級劉暢
// 西南科技大學嵌入式技術實驗室何科君
// 西南科技大學航空航天學社暨空氣動力學實驗室
//硬件參數(shù)：
//電池:1S/3.7V電池推薦300-650mAh左右 500mah以上的電池推薦安裝在背面
//電機/槳:720空心杯/59MM槳
// 特別聲明：本程序測試的空心杯為淘寶店鋪深圳杰盛電機的720空心杯其他廠家的電機不適用于本程序，除非自行調整PID
// 本程序測試的螺旋槳為淘寶店鋪虎虎平價店的59MM直徑 1mm孔徑螺旋槳其他廠家的螺旋槳不適用于本程序，除非自行調整PID
//MCU IAP15W4K61S4@28.000MHZ (B版！A版單片機絕對不適合！)
//特別說明，本程序目前只適合IAP系列的單片機，非IAP單片機使用須修改EEPROM讀寫地址
//陀螺儀加速度計：MPU-6050
//無線芯片:NRF24L01
//電機驅動MOS管:AO3400
//升壓方案:BL8530
//3.3V穩(wěn)壓方案:ME6219C-33-M5G
//下載口保護:1K電阻
//機架尺寸:94mm*94mm
//設計失誤的地方：
//MOS管保護用的肖特基放錯了位置，不過完全不影響使用，這種小四軸不加肖特基保護都行。
//軟件參數(shù)：
//姿態(tài)解算：四元數(shù)
//濾波：互補濾波（From 德國開源四軸）
//PID：串級PID 外環(huán)PI,內環(huán)PID
//數(shù)據定義說明：
//data 51單片機片內RAM最前面128字節(jié)RAM 用ACC讀寫，速度最快
//idata 片內RAM最前面256字節(jié)的RAM 包括data 用類似指針模式訪問適合用于指針操作
//pdata 外部擴展RAM的前256字節(jié)的RAM 不要用會大姨媽！
//xdata 外部擴展RAM 用DPTR訪問
#include <STC15W4K60S4.H>
#include <intrins.h>
#include <NRF24L01.H>
#include <MPU6050.H>
#include <math.h>
#include <STC15W4KPWM.H>
#include <Timer.h>
#include <EEPROM.h>
#include <USART.h>
#include <IMU.H>
//******************************************************************************************************
float XE=0,YE=0; //角度人為修正，但是四軸漂移一般是硬件造成的，故不將此值寫入EEPROM，這個只是應急使用，發(fā)現(xiàn)漂移應
//連至上位機檢查電機軸是否發(fā)生彎曲，發(fā)現(xiàn)問題電機及時更換
float YM=0; //油門變化速度控制，不這樣做的話快速變化油門時四軸會失速翻轉并GG
int ich1=0,ich2=0,ich3=0,ich4=0,ich5=0,ich6=0; //無線串口/串口相關
int speed0=0,speed1=0,speed2=0,speed3=0,V=0; //電機速度參數(shù)
int PWM0=0,PWM1=0,PWM2=0,PWM3=0; //加載至PWM模塊的參數(shù)
int g_x=0,g_y=0,g_z=0; //陀螺儀矯正參數(shù)
char a_x=0,a_y=0; //角度矯正參數(shù)
unsigned char TxBuf[20]={0};
unsigned char RxBuf[20]={0};
double PID_x=0,PID_y=0,PID_z=0; //PID最終輸出量
float FR1=0,FR2=0,FR3=0; //將char數(shù)據轉存為float型
//*****************角度參數(shù)*************************************************
double Gyro_y=0,Gyro_x=0,Gyro_z=0; //Y軸陀螺儀數(shù)據暫存
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; //由角速度計算的角速率(角度制)
double AngleAx=0,AngleAy=0; //三角函數(shù)解算出的歐拉角
double Angle=0,Angley=0; //四元數(shù)解算出的歐拉角
double Anglezlate=0; //Z軸相關
double Ax=0,Ay=0; //加入遙控器控制量后的角度
//****************姿態(tài)處理和PID*********************************************
float Out_PID_X=0,Last_Angle_gx=0;//外環(huán)PI輸出量上一次陀螺儀數(shù)據
float Out_XP=35,Out_XI=0.01,ERRORX_Out=0;//外環(huán)P 外環(huán)I 外環(huán)誤差積分
float In_XP=0.4,In_XI=0.01,In_XD=9,ERRORX_In=0;//內環(huán)P 內環(huán)I 內環(huán)D 內環(huán)誤差積分
float Out_PID_Y=0,Last_Angle_gy=0;
float Out_YP=35,Out_YI=0.01,ERRORY_Out=0;
float In_YP=0.4,In_YI=0.01,In_YD=9,ERRORY_In=0;
float ZP=5.0,ZD=4.0;//自旋控制的P D
int lastR0=0,ZT=0; //上一次RxBuf[0]數(shù)據(RxBuf[0]數(shù)據在不斷變動的) 狀態(tài)標識
int i=0;
void Angle_Calculate() interrupt 1
{
//if(YM<RxBuf[4]&&(RxBuf[4]-YM)<=2){YM++;YM++;}
//else if(YM>RxBuf[4]&&(YM-RxBuf[4])<=2){YM--;YM--;} //防止油門變化過快而失速
//else {YM=RxBuf[4];}
if(RxBuf[1]>FR1){FR1+=0.2;}else if(RxBuf[1]<FR1){FR1-=0.2;}
if(RxBuf[2]>FR2){FR2+=0.2;}else if(RxBuf[2]<FR2){FR2-=0.2;}
YM=(float)RxBuf[4]*3/4;
if(YM>100)//如果油門大于100 即開始起飛
{
if(RxBuf[0]==lastR0)//如果RxBuf[0]的數(shù)據沒有收到即矢聯(lián)
{
ZT++; //狀態(tài)標識+1
if(ZT>128){ZT=128;} //狀態(tài)標識大于128即1秒沒有收到數(shù)據，失控保護
}
else{ZT=0;}
}
else{ZT=0;} //收到信號退出失控保護
if(ZT==128){YM=101;RxBuf[1]=128;RxBuf[2]=128;} //觸發(fā)失控保護油門為1半少一點，緩慢下降，俯仰橫滾方向舵歸中
lastR0=RxBuf[0];
i++;
if(i==130){i=129;}
Accel_y= GetData(ACCEL_YOUT_H); //讀取6050數(shù)據
Accel_x= GetData(ACCEL_XOUT_H);
Accel_z= GetData(ACCEL_ZOUT_H);
Gyro_x = GetData(GYRO_XOUT_H)-g_x;
Gyro_y = GetData(GYRO_YOUT_H)-g_y;
Gyro_z = GetData(GYRO_ZOUT_H)-g_z;
Last_Angle_gx=Angle_gx; //儲存上一次角速度數(shù)據
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; //陀螺儀處理
Angle_gy=(Gyro_y)/65.5; //陀螺儀量程 +-500度/S
Angle_gz=(Gyro_z)/65.5; //轉換關系65.5LSB/度
//***********************************四元數(shù)解算***********************************
IMUupdate(Angle_gx*0.0174533,Angle_gy*0.0174533,Angle_gz*0.0174533,Angle_ax,Angle_ay,Angle_az);
//0.174533為PI/180 目的是將角度轉弧度
//********************三角函數(shù)直接解算以供比較四元數(shù)解算精準度********************
AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*57.2957795f; //后面的數(shù)字是180/PI 目的是弧度轉角度
AngleAy=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*57.2957795f;
//**************X軸指向***********************************************************
FR1=((float)RxBuf[1]-128)/7; //char類型轉存為float以便除法運算
Ax=Angle-FR1-a_x; //角度控制量加載至角度
if(YM>20)
{
ERRORX_Out+=Ax;//外環(huán)積分(油門小于某個值時不積分)
}
else
{
ERRORX_Out=0; //油門小于定值時清除積分值
}
if(ERRORX_Out>500){ERRORX_Out=500;}
else if(ERRORX_Out<-500){ERRORX_Out=-500;}//積分限幅
Out_PID_X=Ax*Out_XP+ERRORX_Out*Out_XI;//外環(huán)PI
if(YM>20)
{
ERRORX_In+=(Angle_gy-Out_PID_X); //內環(huán)積分(油門小于某個值時不積分)
}
else
{
ERRORX_In=0; //油門小于定值時清除積分值
}
if(ERRORX_In>500){ERRORX_In=500;}
else if(ERRORX_In<-500){ERRORX_In=-500;}//積分限幅
PID_x=(Angle_gy+Out_PID_X)*In_XP+ERRORX_In*In_XI+(Angle_gy-Last_Angle_gy)*In_XD;//內環(huán)PID
if(PID_x>1000){PID_x=1000;} //輸出量限幅
if(PID_x<-1000){PID_x=-1000;}
speed0=0-PID_x,speed2=0+PID_x;
//**************Y軸指向**************************************************
if(RxBuf[2]>=143||RxBuf[2]<=113);else{RxBuf[2]=128;}
FR2=((float)RxBuf[2]-128)/7; //char類型轉存為float以便除法運算
Ay=Angley+FR2-a_y; //角度控制量加載至角度
if(YM>20)
{
ERRORY_Out+=Ay;//外環(huán)積分(油門小于某個值時不積分)
}
else
{
ERRORY_Out=0; //油門小于定值時清除積分值
}
if(ERRORY_Out>500){ERRORY_Out=500;}
else if(ERRORY_Out<-500){ERRORY_Out=-500;}//積分限幅
Out_PID_Y=Ay*Out_YP+ERRORY_Out*Out_YI;//外環(huán)PI
if(YM>20)
{
ERRORY_In+=(Angle_gx-Out_PID_Y); //內環(huán)積分(油門小于某個值時不積分)
}
else
{
ERRORY_In=0; //油門小于定值時清除積分值
}
if(ERRORY_In>500){ERRORY_In=500;}
else if(ERRORY_In<-500){ERRORY_In=-500;}//積分限幅
PID_y=(Angle_gx+Out_PID_Y)*In_YP+ERRORY_In*In_YI+(Angle_gx-Last_Angle_gx)*In_YD;//內環(huán)PID
if(PID_y>1000){PID_y=1000;} //輸出量限幅
if(PID_y<-1000){PID_y=-1000;}
speed3=0+PID_y,speed1=0-PID_y;//加載到速度參數(shù)
//**************Z軸指向(Z軸隨便啦，自旋控制沒必要上串級PID)*****************************
FR3=((float)RxBuf[3]-128)*1.5;
Angle_gz-=FR3;
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=AngleAx; //此處可發(fā)送6個數(shù)據至上位機，需要發(fā)送什么數(shù)據在此處修改即可
ich4=AngleAy;
ich5=0;
ich6=0;
//**************將速度參數(shù)加載至PWM模塊*************************************************
PWM0=(1000-YM*4+speed0);
if(PWM0>1000){PWM0=1000;} //速度參數(shù)控制，防止超過PWM參數(shù)范圍0-1000
else if(PWM0<0){PWM0=0;}
PWM1=(1000-YM*4+speed1);
if(PWM1>1000){PWM1=1000;}
else if(PWM1<0){PWM1=0;}
PWM2=(1000-YM*4+speed2);
if(PWM2>1000){PWM2=1000;}
else if(PWM2<0){PWM2=0;}
PWM3=(1000-YM*4+speed3);
if(PWM3>1000){PWM3=1000;}
else if(PWM3<0){PWM3=0;}
if(YM>=10)
{PWM(PWM1,PWM2,PWM0,PWM3);}//1203
else
{PWM(1000,1000,1000,1000);}
}
void main()
{
PWMGO();//初始化PWM
IAPRead();//讀取陀螺儀靜差
InitMPU6050();//初始化MPU-6050
Usart_Init();//初始化串口
Time0_Init();//初始化定時器
RxBuf[1]=128;
RxBuf[2]=128;
RxBuf[3]=128;
RxBuf[4]=0;
while(1)
{
Delay(500);
nRF24L01_RxPacket(RxBuf);
if(RxBuf[5]==1&&i>128)
{
IAP_Gyro();
RxBuf[5]=0;
EA=0;
PWMCKS=0x10;
T2L = 0xEB;
T2H = 0xFF;
PWM(960,960,960,960);
Delay(60000); //校準完畢滴一聲
PWM(1000,1000,1000,1000);
PWMCKS=0x00;
EA=1;
i=0;
}
if(RxBuf[6]==1&&i>128)
{
IAP_Angle();
RxBuf[6]=0;
EA=0;
PWMCKS=0x10;
T2L = 0xEB;
T2H = 0xFF;
PWM(960,960,960,960);
Delay(60000); //校準完畢滴一聲
PWM(1000,1000,1000,1000);
PWMCKS=0x00;
EA=1;
i=0;
}
//Send(ich1,ich2,ich3,ich4,ich5,ich6); //串口發(fā)送數(shù)據如需連接上位機，須取消注釋本句！��！注釋本句是為了減小遙控延時
}
}

復制代碼