国产人无码A在线西瓜影音,在线成人一区二区

#include "stm32f10x.h"
#include "sys.h"
char x[10];//角度顯示緩沖數(shù)組
u8 Tim[7]= "Time : ";//時間顯示數(shù)組
extern float Roll; //讀取到的6050角度值
float Roll_1,Roll_2,Roll_3;//分別是最新時刻角度值，上一時刻角度值，以及積分角度值
float power;//小車最終輸出動力
float KP=8.0,KD=0.5,KI=0.02,PrevError_C =0;//PID參數(shù)
u8 pid_flag=0;//分時刻讀取角度值的標志位
extern u16 tim_flag;//計時標志位
extern int tim;//秒計時，一秒加一
double zz=0;//平衡計時
int main()//主函數(shù)
{
delay_init();//延時函數(shù)初始化
IIC_Init();//IIC初始化
MPU6050_initialize();//6050初始化
DMP_Init();//MPU6050內置DMP的初始化
OLED_Init();//OLED初始化
OLED_Clear();//OLED清屏
GPIO_INIT();//控制減速電機IO口初始化
HW_INIT();//紅外探頭使用IO口初始化
TIM3_INIT();//定時器初始化，用于放PWM波，控制車的速度
Qianjin();//使車向前跑（相對的）
TIM3->CCR1=80;//左輪動力80
TIM3->CCR2=80;//右輪動力80
OLED_ShowString(0,4, Tim);//OLED顯示不改變數(shù)組
Read_DMP();//讀取一次角度值
while(1)//無限循環(huán)
{
sprintf(x,"Angle: %0.2f",Roll);//此函數(shù)用于將數(shù)組加入可變參數(shù)
OLED_ShowString(0,0,(u8*)x);//顯示上列數(shù)組
if(tim>=99)tim=99;//計時上限99秒
OLED_ShowNum(48,4,tim,3,12);//顯示時間，單位/秒
OLED_ShowNum(0,6,zz,4,12);//顯示平衡時間
OLED_ShowNum(32,6,power,4,12);//顯示當前動力值
/***************************平衡PID算法********************************/
pid_flag++;//運行時間標志
if(pid_flag==1)//記錄1時刻角度值
{
Roll_1=Roll;
}
if(pid_flag==2)//記錄2時刻角度值
{
pid_flag=0;
Roll_2=Roll;
PrevError_C=Roll_2-Roll_1;//兩次角度的差值
}
Read_DMP();//讀取當前角度值
Roll_3+=Roll;//積分值累加
power=KP*abs(Roll)+KI*abs(Roll_3)+KD*PrevError_C;//PID計算當前合適的動力值
if(power>90) power=90;//動力值限幅
if(Roll>1.7)//判斷角度值
{
if(PrevError_C<-0.03)//預判是否需要后退
Qianjin();
else //不需要就繼續(xù)走
Houtui();
TIM3->CCR1=power+30;
TIM3->CCR2=power;
}
if(Roll<-1.7) //判斷角度值
{
if(PrevError_C>0.03)//預判是否需要后退
Houtui();
else //不需要就繼續(xù)走
Qianjin();
TIM3->CCR1=power+20;
TIM3->CCR2=power;
}
if((Roll<=1.7)&&(Roll>=-1.7)&&(PrevError_C<0.03&&PrevError_C>-0.03))//平衡條件限定
{
Tingzhi();//停止
TIM3->CCR1=0;//動力值為0
TIM3->CCR2=0;
Roll_3=0; //積分清零
zz++;//平衡計時
if(zz>888)//達到平衡時間觀察
{
while(1)
{
Qianjin();TIM3->CCR1=95;TIM3->CCR2=80;//勻速向前
if(HW5==0)//右后探頭檢測到黑線
{
Tingzhi();//停止
while(1)
{
Tingzhi();
TIM3->CCR1=0;//動力為0
TIM3->CCR2=0;
}
}
}
}
}
}
}

復制代碼