亚洲国产情侣偷自在线二页,少妇荡乳情欲办公室456视频,亚洲精品又粗又大又爽a片

#include "sys.h"
//期望角度（機(jī)械中值——>不一定為0）
float Mid_angle = 0;
//直立環(huán)（Kp、Kd）與速度環(huán)（Kp、Ki）的PID參數(shù)
float
// Vertical_Kp = 400, //直立環(huán)Kp
// Vertical_Kd = 1; //直立環(huán)Kd
Vertical_Kp = 240, //直立環(huán)Kp
Vertical_Kd = 0.6; //直立環(huán)Kd
float
Velocity_Kp = 0.38, //速度環(huán)Kp
Velocity_Ki = 0.0019; //速度環(huán)Ki
float
Turn_Kp = 1.1; //轉(zhuǎn)向環(huán)Kp
//PID控制輸出
int Velocity_out, Vertical_out, Turn_out;
//聲明函數(shù)
int Velocity_PI(int Encoder_left, int Encoder_right);
int Vertical_PD(float Mid_angle, float Real_angle, float gyro_y);
int Turn_P(int gyro_z);
void EXTI9_5_IRQHandler()
{
if(EXTI_GetITStatus(EXTI_Line5) != 0 )
{
int PWM_out;
if(PBin(5) == 0) //判斷是否為低電平（外部中斷下降沿觸發(fā)）
{
EXTI_ClearITPendingBit(EXTI_Line5);
//1.1、采集編碼器數(shù)據(jù)
Encoder_left = -Read_Speed(2); //讀取編碼器2數(shù)據(jù)
Encoder_right = Read_Speed(4); //讀取編碼器4數(shù)據(jù)
//1.2、采集MPU6050數(shù)據(jù)
mpu_dmp_get_data(&Pitch, &Roll, &Yaw);
MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);
MPU_Get_Accelerometer(&aacx, &aacy, &aacz);
//2、帶入?yún)?shù)進(jìn)行PID控制
Vertical_out = Vertical_PD(Mid_angle, Pitch, gyroy); //前后移動(dòng)由Pitch控制
Velocity_out = Velocity_PI(Encoder_left, Encoder_right);
Turn_out = Turn_P(gyroz);
PWM_out = Vertical_out - Vertical_Kp * Velocity_out; //串級PID推導(dǎo)公式（速度環(huán)與直立環(huán)）
//3、將PID輸出值賦給電機(jī)
Motor_1 = PWM_out - Turn_out;
Motor_2 = PWM_out + Turn_out;
Limit(&Motor_1, &Motor_2);
Load(Motor_1, Motor_2);
}
}
}
//速度環(huán) PI控制
/*
輸入：
Encoder_left 左編碼器速度
Encoder_right 右編碼器速度
輸出：
PWM值
*/
int Velocity_PI(int encoder_left, int encoder_right)
{
static int
PWM_out,
Encoder_err, //Encoder_err 為編碼器速度偏差
Encoder_S; //Encoder_S 為編碼器速度的積分值
static int
Encoder_err_low_out, //Encoder_err_low_out 為低通濾波器輸出
Encoder_err_low_out_last; //Encoder_err_low_out_last 為上一次低通濾波器輸出
float
a = 0.7; //a 為低通濾波器的系數(shù)
//1、計(jì)算速度偏差
Encoder_err = (encoder_left + encoder_right) - 0; //除以2會(huì)造成舍去誤差
// Encoder_err = (Encoder_left + Encoder_right) / 2 - 0; //0表示為期望速度
//2、對速度偏差進(jìn)行低通濾波
//（濾除高頻干擾，使得波形更加平滑，防止速度突變影響直立環(huán)的正常工作）
Encoder_err_low_out = (1 - a) * Encoder_err + a * Encoder_err_low_out_last;
Encoder_err_low_out_last = Encoder_err_low_out;
//3、對速度偏差進(jìn)行積分，得到位移
Encoder_S += Encoder_err_low_out;
//4、積分限幅
Encoder_S = Encoder_S > 10000 ? 10000 : (Encoder_S < (-10000) ? (-10000) : Encoder_S);
//5、速度環(huán)輸出
PWM_out = Velocity_Kp * Encoder_err_low_out + Velocity_Ki * Encoder_S;
return PWM_out;
}
//直立環(huán) PD控制
/*
輸入：
Mid_angle 期望角度（中值角度）
Real_angle 真實(shí)角度
gyro_y 真是角速度
輸出：
PWM值
*/
int Vertical_PD(float Mid_angle, float Real_angle, float gyro_y)
{
int PWM_out;
PWM_out = Vertical_Kp * (Real_angle - Mid_angle) + Vertical_Kd * (gyro_y - 0);
return PWM_out;
}
//轉(zhuǎn)向環(huán) P控制
int Turn_P(int gyro_z)
{
int PWM_out;
PWM_out = Turn_Kp * gyro_z;
return PWM_out;
}

復(fù)制代碼