免费视频一二三区,免费黄看片,国产高清一区

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

復制代碼

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