diff --git a/云台/云台/CarBody/Gimbal.c b/云台/云台/CarBody/Gimbal.c index 2d25f39..ed267d1 100644 --- a/云台/云台/CarBody/Gimbal.c +++ b/云台/云台/CarBody/Gimbal.c @@ -26,12 +26,12 @@ KFP kfp_PITCH={0.02,0,0,0,0.01,0.543};//卡尔曼滤波器结构体 uint8_t Gimbal_FrictionWheelFlag;//云台小陀螺标志位,云台开摩擦轮标志位 -uint8_t Fire_Flag=0;//开火指示位,用于单发限位 +bool Fire_Flag=0;//开火指示位,用于单发限位 PID_PositionInitTypedef Gimbal_YawAnglePositionPID,Gimbal_YawAngleSpeedPID;//Yaw轴GM6020电机PID PID_PositionInitTypedef Gimbal_PitchAnglePositionPID,Gimbal_PitchAngleSpeedPID;//Pitch轴GM6020电机PID PID_PositionInitTypedef Gimbal_L2_FrictionWheelPID,Gimbal_R2_FrictionWheelPID,Gimbal_L1_FrictionWheelPID,Gimbal_R1_FrictionWheelPID;//摩擦轮转速PID -PID_PositionInitTypedef Gimbal_RammerSpinSpeedPID;//拨弹盘旋转PID +PID_PositionInitTypedef Gimbal_RammerSpinPositionPID;//拨弹盘旋转PID /* *函数简介:云台初始化 @@ -97,10 +97,10 @@ void Gimbal_Init(void) PID_PositionSetEkRange(&Gimbal_R2_FrictionWheelPID,-1,1); PID_PositionSetOUTRange(&Gimbal_R2_FrictionWheelPID,-15000,15000); - PID_PositionStructureInit(&Gimbal_RammerSpinSpeedPID,Gimbal_RammerSpeed);//拨弹盘 - PID_PositionSetParameter(&Gimbal_RammerSpinSpeedPID,50,1,0); - PID_PositionSetEkRange(&Gimbal_RammerSpinSpeedPID,-1,1); - PID_PositionSetOUTRange(&Gimbal_RammerSpinSpeedPID,-2000,2000); + PID_PositionStructureInit(&Gimbal_RammerSpinPositionPID,Gimbal_RammerSpeed);//拨弹盘 + PID_PositionSetParameter(&Gimbal_RammerSpinPositionPID,50,50,0); + PID_PositionSetEkRange(&Gimbal_RammerSpinPositionPID,-20,20); + PID_PositionSetOUTRange(&Gimbal_RammerSpinPositionPID,-2000,2000); @@ -123,7 +123,7 @@ void Gimbal_CleanPID(void) PID_PositionClean(&Gimbal_R1_FrictionWheelPID); PID_PositionClean(&Gimbal_L2_FrictionWheelPID); PID_PositionClean(&Gimbal_R2_FrictionWheelPID); - PID_PositionClean(&Gimbal_RammerSpinSpeedPID); + PID_PositionClean(&Gimbal_RammerSpinPositionPID); } @@ -249,19 +249,19 @@ void Gimbal_Rammer(void) if(Gimbal_FrictionWheelFlag==1) { if((Remote_RxData.Remote_ThumbWheel<1000 && RefereeSystem_Status==0) || PC_Fire==1) - Gimbal_RammerSpinSpeedPID.Need_Value+=-Gimbal_RammerSpeed; + Gimbal_RammerSpinPositionPID.Need_Value=-Gimbal_RammerSpeed; else if((Remote_RxData.Remote_ThumbWheel>1050 && RefereeSystem_Status==0) || PC_Ejection==1) - Gimbal_RammerSpinSpeedPID.Need_Value+=Gimbal_RammerSpeed; + Gimbal_RammerSpinPositionPID.Need_Value=Gimbal_RammerSpeed; else - Gimbal_RammerSpinSpeedPID.Need_Value=0; + Gimbal_RammerSpinPositionPID.Need_Value=0; } - else Gimbal_RammerSpinSpeedPID.Need_Value=0; + else Gimbal_RammerSpinPositionPID.Need_Value=0; - PID_PositionCalc(&Gimbal_RammerSpinSpeedPID,M3508_MotorStatus[Gimbal_RammerSpinMotor-0x201].RotorSpeed); - M2006_CANSetHIDCurrent(0,Gimbal_PitchAngleSpeedPID.OUT,Gimbal_RammerSpinSpeedPID.OUT,0); + PID_PositionCalc(&Gimbal_RammerSpinPositionPID,M3508_MotorStatus[Gimbal_RammerSpinMotor-0x201].RotorSpeed); + //M2006_CANSetHIDCurrent(0,Gimbal_PitchAngleSpeedPID.OUT,Gimbal_RammerSpinPositionPID.OUT,0); //PID_PositionCalc(&Gimbal_PitchAngleSpeedPID,GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Speed); - //M3508_CANSetLIDCurrent(0,0,Gimbal_RammerSpinSpeedPID.OUT,0); - //M3508_CANSetHIDCurrent(0,0,Gimbal_RammerSpinSpeedPID.OUT,0); + //M3508_CANSetLIDCurrent(0,0,Gimbal_RammerSpinPositionPID.OUT,0); + //M3508_CANSetHIDCurrent(0,0,Gimbal_RammerSpinPositionPID.OUT,0); } /* @@ -274,31 +274,27 @@ void Gimbal_Rammer_Single_fire(void) { if(Gimbal_FrictionWheelFlag==1) { - if(((Remote_RxData.Remote_ThumbWheel<1000 && RefereeSystem_Status==0) || PC_Fire==1) && Fire_Flag == 0) + if(((Remote_RxData.Remote_ThumbWheel<1000 && RefereeSystem_Status==0) || PC_Fire==1) && Fire_Flag != 1;) { - Gimbal_RammerSpinSpeedPID.Need_Value+=-Gimbal_RammerSpeed;//3,2,1 Fire!!! + Gimbal_RammerSpinPositionPID.Need_Value=Gimbal_RammerSpinPositionPID.Need_Value-Gimbal_RammerSingleAmmo;//3,2,1 Fire!!! Fire_Flag=1;//单次开火标志置1 - } - else if(((Remote_RxData.Remote_ThumbWheel>1050 && RefereeSystem_Status==0) || PC_Ejection==1) && Fire_Flag == 1) + else if((Remote_RxData.Remote_ThumbWheel>1050 && RefereeSystem_Status==0) || PC_Ejection==1) { - Gimbal_RammerSpinSpeedPID.Need_Value+=Gimbal_RammerSpeed;//退弹 + Gimbal_RammerSpinPositionPID.Need_Value=Gimbal_RammerSpinPositionPID.Need_Value+Gimbal_RammerSpeed;//退弹 Fire_Flag=0;//退弹就不用一发一发退了 } - else if(((Remote_RxData.Remote_ThumbWheel>1000 && Remote_RxData.Remote_ThumbWheel<1050) ) && Fire_Flag == 1) - { - //Gimbal_RammerSpinSpeedPID.Need_Value=0;//停止开火! + else + Gimbal_RammerSpinPositionPID.Need_Value=0;//停止开火! Fire_Flag=0; - } } - //else - //Gimbal_RammerSpinSpeedPID.Need_Value=0;//防止意外走火? + else Gimbal_RammerSpinPositionPID.Need_Value=0;//防止意外走火? - PID_PositionCalc(&Gimbal_RammerSpinSpeedPID,M3508_MotorStatus[Gimbal_RammerSpinMotor-0x201].RotorPosition); + PID_PositionCalc(&Gimbal_RammerSpinPositionPID,M3508_MotorStatus[Gimbal_RammerSpinMotor-0x201].RotorSpeed); - //M2006_CANSetHIDCurrent(0,Gimbal_PitchAngleSpeedPID.OUT,Gimbal_RammerSpinSpeedPID.OUT,0); + //M2006_CANSetHIDCurrent(0,Gimbal_PitchAngleSpeedPID.OUT,Gimbal_RammerSpinPositionPID.OUT,0); - //M3508_CANSetLIDCurrent(0,0,Gimbal_RammerSpinSpeedPID.OUT,0); - //M3508_CANSetHIDCurrent(0,0,Gimbal_RammerSpinSpeedPID.OUT,0); + //M3508_CANSetLIDCurrent(0,0,Gimbal_RammerSpinPositionPID.OUT,0); + //M3508_CANSetHIDCurrent(0,0,Gimbal_RammerSpinPositionPID.OUT,0); }