日韩精品无码AV成人观看,樱桃视频在线观看,男JI大巴进入女人的直播

#include "sys.h"
int Position_Pwm,Moto_qb1,Moto_qb2,Count_Moto;
double Count_qb;
int myabs(int a)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
int Position_PID1 (float Angle,float Target)
{
Last_Bias1=Bias1; //保存上一次偏差
Bias1=Target-Angle; //計(jì)算偏差
Integral_bias1+=Bias1; //求出偏差的積分
Pwm1=-KP1*Bias1+KI1*Integral_bias1-KD1*(Bias1-Last_Bias1)*10; //位置式PID控制器
return Pwm1; //增量輸出
}
int Position_PID2 (float Angle,float Target)
{
Last_Bias2=Bias2; //保存上一次偏差
Bias2=Target-Angle; //計(jì)算偏差
Integral_bias2+=Bias2; //求出偏差的積分
Pwm2=-KP2*Bias2+KI2*Integral_bias2-KD2*(Bias2-Last_Bias2)*10; //位置式PID控制器
return Pwm2; //增量輸出
}
void Set_Pwm1(int moto)
{
if(moto<0) AIN2=1, AIN1=0;
else AIN2=0, AIN1=1;
TIM_SetCompare4(TIM3,myabs(moto));
}
void Set_Pwm2(int moto)
{
if(moto<0) BIN2=1, BIN1=0;
else BIN2=0, BIN1=1;
TIM_SetCompare2(TIM2,myabs(moto));
}
void balance(void)
{
float balance1,balance2;
balance1=Position_PID1(500,500);
balance2=Position_PID2(500,500);
Set_Pwm1(balance1);Set_Pwm2(balance2);
}
void control(void)
{
static float Count_Big_Angle=0.019635;
Count_qb+=Count_Big_Angle; //自變量
Target_Position1=A*sin(Count_qb+X)+495-m; //運(yùn)動(dòng)公式
Target_Position2=B*sin(Count_qb+Y)+500-n; //運(yùn)動(dòng)公式
Moto_qb1=Position_PID1((Roll-42.4)*10,Target_Position1); //位置閉環(huán)控制
Moto_qb2=Position_PID2((Pitch+50)*10,Target_Position2); //位置閉環(huán)控制
Set_Pwm1(Moto_qb1);Set_Pwm2(Moto_qb2); //賦值給PWM寄存器
}

復(fù)制代碼

#include "sys.h"
float Pitch,Roll,Yaw;
float KP1=0,KD1=0,KI1=0,KP2=0,KD2=0,KI2=0;
float Menu=1,A=0,B=0,X=0,Y=0,m=0,n=0;
int EN=0;
float Target_Position1=500,Target_Position2=500,Last_Bias1,Bias1,Pwm1,Integral_bias1,Last_Bias2,Bias2,Pwm2,Integral_bias2;
int main(void)
{
delay_init();
IIC_Init();
JTAG_Set(SWD_ENABLE);
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級(jí)，2位響應(yīng)優(yōu)先級(jí)
uart_init(9600); //串口初始化為9600
DMP_Init();
KEY_Init();
TIM4_Int_Init(49,7199);//10Khz的計(jì)數(shù)頻率，計(jì)數(shù)到30為3ms
TIM3_PWM_Init(1499,0); //不分頻。PWM頻率=72000000/900=80Khz
TIM2_PWM_Init(1499,0); //不分頻。PWM頻率=72000000/900=80Khz
OLED_Init();
LED_Init();
EXTIX_Init();
while(1)
{
DataScope();
oled_show();
}
}

復(fù)制代碼