无码国产一区二区免费,AV在线观看狼友永久网站,国产中老年妇女精品

#include "control.h"
#include "BSP.H"
#include "rc.h"
#include "imu.h"
#include "uart1.h"
PID PID_ROL,PID_PIT,PID_YAW;
u8 ARMED = 0;
extern vs16 QH,ZY,XZ;
float Get_MxMi(float num,float max,float min)
{
if(num>max)
return max;
else if(num<min)
return min;
else
return num;
}
void CONTROL(float rol_now, float pit_now, float yaw_now, float rol_tar, float pit_tar, float yaw_tar)
{ //當(dāng)前姿態(tài)角，，，目標(biāo)姿態(tài)角
u16 moto1=0,moto2=0,moto3=0,moto4=0;
vs16 throttle;
// u8 moto[8];
float rol = rol_tar + rol_now;
float pit = pit_tar + pit_now;
float yaw = yaw_tar + yaw_now;
throttle = Rc_Get.THROTTLE - 1000; //1000<遙控油門值<2000
if(throttle<0) throttle=0;
PID_ROL.IMAX = throttle/10; //積分限幅，積分值不超過當(dāng)前油門值的一半
PID_ROL.IMAX = Get_MxMi(PID_ROL.IMAX,1000,0); //限制積分結(jié)果為，0到1000
PID_PIT.IMAX = PID_ROL.IMAX;
PID_ROL.pout = PID_ROL.P * rol;
PID_PIT.pout = PID_PIT.P * pit;
//////////////////、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、
/////////////0.1 0.1 30 30
if(rol_tar*rol_tar<1 && pit_tar*pit_tar<1 && rol_now*rol_now<100 && pit_now*pit_now<100 && throttle>300)
{ //防止角度大了，積分超調(diào)//目標(biāo)姿態(tài)角水平，姿態(tài)角幾乎水平，油門值不太低
PID_ROL.iout += PID_ROL.I * rol;
PID_PIT.iout += PID_PIT.I * pit;
PID_ROL.iout = Get_MxMi(PID_ROL.iout,PID_ROL.IMAX,-PID_ROL.IMAX); //對輸出的積分限幅
PID_PIT.iout = Get_MxMi(PID_PIT.iout,PID_PIT.IMAX,-PID_PIT.IMAX);
}
else if(throttle<200)
{ //油門值較小時，積分項(xiàng)清零
PID_ROL.iout = 0;
PID_PIT.iout = 0;
}
//////////////////、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、
// rc_roll_d = rol_tar - getlast_roll;
// getlast_roll = rol_tar;
// PID_ROL.dout = PID_ROL.D * (MPU6050_GYRO_LAST.X+rc_roll_d*300);//角速度+控制誤差微分
//
// rc_pitch_d = pit_tar - getlast_pitch;
// getlast_pitch = pit_tar;
// PID_PIT.dout = PID_PIT.D * (MPU6050_GYRO_LAST.Y+rc_pitch_d*300);//角速度+控制誤差微分
PID_ROL.dout = PID_ROL.D * MPU6050_GYRO_LAST.X;
PID_PIT.dout = PID_PIT.D * MPU6050_GYRO_LAST.Y;
/////////////////////////////////////////////////////////////////////
PID_YAW.pout = PID_YAW.P * yaw;
//若 *yaw_now；鎖尾模式！�。。�
//若 *yaw；非鎖尾模式�。。�！
/////////////////////////////////////////////////////////////////////
vs16 yaw_d;
/////////////////////////////////////////////////////////////////////
if(1480>Rc_Get.YAW || Rc_Get.YAW>1520) //給遙控加死區(qū)（偏航角的死區(qū)）
{
yaw_d = MPU6050_GYRO_LAST.Z + (Rc_Get.YAW-1500)*10; //用Z軸角速度及目標(biāo)偏航角值進(jìn)行四軸運(yùn)動的預(yù)判
GYRO_I.Z = 0;
}
else
yaw_d = MPU6050_GYRO_LAST.Z;
PID_YAW.dout = PID_YAW.D * yaw_d;
/////////////////////////////////////////////////////////////////////
PID_ROL.OUT = PID_ROL.pout + PID_ROL.iout + PID_ROL.dout;
PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
if(throttle>200) //油門值大于200才起飛（遙控油門值大于1200）
{
// moto1 = throttle - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
// moto2 = throttle - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
// moto3 = throttle + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
// moto4 = throttle + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
// moto4 = throttle - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
// moto3 = throttle - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
// moto2 = throttle + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
// moto1 = throttle + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
// moto1 = throttle - 25 + QH + ZY - XZ - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
// moto2 = throttle - 25 + QH - ZY + XZ - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
// moto3 = throttle - 25 - QH + ZY + XZ + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
// moto4 = throttle + 75 - QH - ZY - XZ + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
moto4 = throttle + QH + ZY - XZ - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
moto3 = throttle + QH - ZY + XZ - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
moto2 = throttle - QH + ZY + XZ + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
moto1 = throttle - QH - ZY - XZ + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
// moto1 = throttle;
// moto2 = throttle;
// moto3 = throttle;
// moto4 = throttle;
}
else
{
moto1 = 0;
moto2 = 0;
moto3 = 0;
moto4 = 0;
}
// moto[1] = moto1 & 0xFF;
// moto[0] = (moto1>>8) & 0xFF;
// moto[3] = moto2 & 0xFF;
// moto[2] = (moto2>>8) & 0xFF;
// moto[5] = moto3 & 0xFF;
// moto[4] = (moto3>>8) & 0xFF;
// moto[7] = moto4 & 0xFF;
// moto[6] = (moto4>>8) & 0xFF;
//Uart1_Send_Buf(moto,8);
if(ARMED) Moto_PwmRflash(moto1,moto2,moto3,moto4); //未解鎖則空心杯不轉(zhuǎn) ARMED=1則解鎖
else Moto_PwmRflash(0,0,0,0);
}

復(fù)制代碼