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 mode3()
{
angle_set=20;
angle_set1=20;
if(active==0)
{
SetMotorVoltage(0.3,0.3);
if(angle_fuse>8)
{
active=1;
rollcount=1;
}
}
}
void MotorSpeedOut1(void)
{
float fLeftVal, fRightVal;
fLeftVal = white_motor;
fRightVal = red_motor;
if(fLeftVal > 0)
fLeftVal += MOTOR_OUT_DEAD_VAL;
else if(fLeftVal < 0)
fLeftVal -= MOTOR_OUT_DEAD_VAL;
if(fRightVal > 0)
fRightVal += MOTOR_OUT_DEAD_VAL;
else if(fRightVal < 0)
fRightVal -= MOTOR_OUT_DEAD_VAL;//死区处理
SetMotorVoltage(-fLeftVal,fRightVal);
}
//---------------------------------控制三环结束---------------------------------------------
void CarControlStop(void)
{
SetMotorVoltage(0, 0);
g_nCarControlFlag = 0;// CAR_CONTROL_CLEAR;
g_nAngleControlFlag = 0;// ANGLE_CONTROL_STOP;
g_nSpeedControlFlag = 0;// SPEED_CONTROL_STOP;
g_nDirectionControlFlag = 0;// DIRECTION_CONTROL_STOP;
// PTA9_O = 1;//蜂鸣器
// delayMs(15);
// PTA9_O = 0;
g_fLeftMotorOut = g_fRightMotorOut = 0;
g_fAngleControlOut = 0;
g_fSpeedControlOut = 0;
g_fDirectionControlOut = 0;
//g_nWaitCarStandCount = 0;
}
void MotorSpeedOut_3(void)
{
float fLeftVal, fRightVal;
fLeftVal = white_motor;
fRightVal = red_motor;
if(fLeftVal > 0)
fLeftVal += MOTOR_OUT_DEAD_VAL;
else if(fLeftVal < 0)
fLeftVal -= MOTOR_OUT_DEAD_VAL;
if(fRightVal > 0)
fRightVal += MOTOR_OUT_DEAD_VAL;
else if(fRightVal < 0)
fRightVal -= MOTOR_OUT_DEAD_VAL;//死区处理
//SetMotorVoltage((fRightVal/sin(aset/180*pi)+fLeftVal/sin(aset/180*pi))*0.05,(fRightVal/cos(aset/180*pi)+fLeftVal/cos(aset/180*pi))*0.05);
//display[4]=fRightVal/sin(aset/180*pi)+fLeftVal/sin(aset/180*pi),fRightVal/cos(aset/180*pi)+fLeftVal/cos(aset/180*pi);
SetMotorVoltage(fLeftVal/3,fRightVal/3);
}
/********************************舵机PD控制*************************************/
void PDkongzhi()
{
P = actuator_P, D = actuator_D;
LastError = Error; //上一次偏差
// if(left_diuxian1 > 10 && right_diuxian1 < 10 &&
// shuang_diuxian1 < 10 && slope.right > -0.01 &&
// slope.right < 0.01 && slope.right!=0)
// {
// Error = 15;
// led(LED1,LED_ON);
// }
// else if(right_diuxian1 > 10 && left_diuxian1 < 10 &&
// shuang_diuxian1 < 10 && slope.left > -0.01 &&
// slope.left < 0.01 && slope.left!=0)
// {
// Error = -15;
// led(LED1,LED_ON);
// }
if(left_diuxian1 > 10 && right_diuxian1 < 10 && wide_check())
{
Error = 0;
led(LED1,LED_ON);
}
else if(right_diuxian1 > 10 && left_diuxian1 < 10 && wide_check())
{
Error = 0;
led(LED1,LED_ON);
}
else
{
Error = black_centre + (int)(slope.slope * 160); //平均中点和理论中点0之间的偏差 + 斜率
led(LED1,LED_OFF);
}
dajiao = control_actuator_center - (int)(P * (float)Error) - (int)(D * (float)(Error - LastError));//舵机打角
if(dajiao > control_actuator_max) //左最大
dajiao = control_actuator_max;
else if(dajiao < control_actuator_min) //右最大
dajiao = control_actuator_min;
if(Error>35)
Error=35;
if(Error<-35)
Error=-35;
ftm_pwm_duty(S3010_FTM, S3010_CH, (uint32)dajiao);
//SetMotorVoltage(speedl , speedr );
if(stop_done == 1)
{
// #if ( CAR_NUMBER == 2 )
// lptmr_delay_ms(1000); //延时防撞
// #endif
SetMotorVoltage(0, 0);
}
else
{
SetMotorVoltage(speed + 0.006 * Error, speed - 0.006 * Error);
//SetMotorVoltage(speedl + 0.0015 * Error, speedr - 0.0015 * Error);
}
}