void PIT0_IRQHandler(void)
{ if(aa<1000) aa++;
delayms();
mpu6050_read();
angle_calculate();
angle_calculate1();
if (flag==2||flag==1)
set_gyro();
if (flag==3&&active==1)
{
set_gyro1_3();
set_gyro_3();
}
// oledplay();
if(flag==1)
{
motor_control();
SetMotorVoltage(0.05,0);
MotorSpeedOut();
}
if(qd==0)
{
motor_control();
SetMotorVoltage(0.05,0);
MotorSpeedOut();
}
if (flag==3&&active==1)
{
motor_control_3();
// SetMotorVoltage(0,0.5);
MotorSpeedOut_3();
}
if (flag==4)
{
motor_control_4();
SetMotorVoltage(0.05,0);
MotorSpeedOut();
}
if(flag==5)
{
set_gyro1();
motor_control1();
SetMotorVoltage(0,0.05);
MotorSpeedOut1();
angle_set1=16;
}
PIT_Flag_Clear(PIT0); //清中断标志位
}
/*-----------------------------------------------------------------------*/
void AngleControl(void)
{ float delta_angle;
float delta_anglespeed;
float temp_angle, temp_anglespeed;
float currentanglespeed, lastanglespeed=0;
float last_angle=0;
angle_calculate();
g_fCarAngle= AngleCalculate[0];
g_fGyroscopeAngleSpeed= -AngleCalculate[1];
// g_fGyroscopeTurnSpeed= AngleCalculateResult[2];
temp_angle=CarAngleInitial - g_fCarAngle;
temp_anglespeed= CarAnglespeedInitial - g_fGyroscopeAngleSpeed;
// if(temp_angle<-15)
// data_angle_pid.p=150; //150//100开环
// else if(temp_angle>=-15&temp_angle<=0)
// data_angle_pid.p=260; //230//200开环
// else if(temp_angle>0&temp_angle<=15)
// data_angle_pid.p=260; //230//200开环
// else
// data_angle_pid.p=150; //150//100开环
//
//
// if(temp_anglespeed>=50||temp_anglespeed<=-50)
// data_angle_pid.d=2;//0.3
// else
// data_angle_pid.d=0.5;//0.1
currentanglespeed=g_fCarAngle;
delta_anglespeed=currentanglespeed-lastanglespeed;
lastanglespeed=currentanglespeed;
delta_angle = data_angle_pid.p*(CarAngleInitial - g_fCarAngle);
delta_angle+=data_angle_pid.d*(CarAnglespeedInitial - g_fGyroscopeAngleSpeed);
// delta_angle+=data_angle_pid.d*0.4*delta_anglespeed;
//delta_angle = data_angle_pid.p*(CarAngleInitial - g_fCarAngle) /5000 +data_angle_pid.d*(CarAnglespeedInitial - g_fGyroscopeAngleSpeed) /15000; // 1000 与10000是否根据实际需要调整
//angle_pwm=delta_angle;
/* if(delta_angle>AngleControlOutMax)
delta_angle=AngleControlOutMax;
else if(delta_angle<AngleControlOutMin)
delta_angle=AngleControlOutMin;*/
PITCH_angle_pwm=delta_angle;
}
/*-----------------------------------------------------------------------*/
void BalanceControl(void)//暂时用data_speed_pid
{
/*
//用电位器调节转速，试验用
int potential=1;
int sss=1;
potential=(int)ADC.CDR[33].B.CDATA;//PB9
if(potential<0)
{
potential=0-potential;
}
if(potential>SPEED_PWM_MAX)
{
potential=SPEED_PWM_MAX;
}
if(potential<200)
sss=200;
else
sss=500;
set_motor_pwm(100);
*/
float delta_angle_balance;
float delta_anglespeed_balance;
float temp_angle_balance, temp_anglespeed_balance;
static float currentanglespeed_balance, lastanglespeed_balance=0;
float last_angle_balance=0;
float temp_p,temp_d;
angle_calculate();
g_fCarAngle_balance= AngleCalculate[2];
g_fGyroscopeAngleSpeed_balance=-AngleCalculate[3];
temp_angle_balance=CarAngleInitial_balance - g_fCarAngle_balance;
temp_anglespeed_balance= CarAnglespeedInitial_balance - g_fGyroscopeAngleSpeed_balance;
if(g_fCarAngle_balance>7||g_fCarAngle_balance<-7)
{
temp_d=0;
temp_p=90;
}
else
{
temp_p=data_ROLL_angle_pid.p;
temp_d=data_ROLL_angle_pid.d;
}
// currentanglespeed_balance=g_fCarAngle_balance;
// delta_anglespeed_balance=currentanglespeed_balance-lastanglespeed_balance;
// lastanglespeed_balance=currentanglespeed_balance;
delta_angle_balance = temp_p*(CarAngleInitial_balance - g_fCarAngle_balance);
delta_angle_balance+=temp_d*(CarAnglespeedInitial_balance - g_fGyroscopeAngleSpeed_balance);
// delta_angle_balance+=data_speed_pid.d*0.4*delta_anglespeed_balance;
//angle_pwm_balance=dta_angle;
ROLL_angle_pwm=delta_angle_balance;
LCD_PrintoutInt(0, 6, ROLL_angle_pwm);
}
