317 lines
14 KiB
C
317 lines
14 KiB
C
#include "stm32f4xx.h" // Device header
|
|
#include "stm32f4xx_conf.h"
|
|
#include "Parameter.h"
|
|
#include "PID.h"
|
|
#include "Remote.h"
|
|
#include "AttitudeAlgorithms.h"
|
|
#include "M3508.h"
|
|
#include "M2006.h"
|
|
#include "GM6020.h"
|
|
#include "Laser.h"
|
|
#include "RefereeSystem.h"
|
|
#include "Visual.h"
|
|
#include "WaveFiltering_Kalman_Filtering.h"
|
|
|
|
KFP kfp_PITCH={0.02,0,0,0,0.01,0.543};//卡尔曼滤波器结构体
|
|
|
|
|
|
|
|
#define Gimbal_YawMotor GM6020_1//Yaw轴电机
|
|
#define Gimbal_PitchMotor GM6020_2//Pitch轴电机
|
|
#define Gimbal_L1_FrictionWheel M3508_1//左摩擦轮
|
|
#define Gimbal_R1_FrictionWheel M3508_2//右摩擦轮
|
|
#define Gimbal_L2_FrictionWheel M3508_3//左摩擦轮
|
|
#define Gimbal_R2_FrictionWheel M3508_4//右摩擦轮
|
|
#define Gimbal_RammerSpinMotor M3508_7//拨弹盘电机
|
|
|
|
|
|
uint8_t Gimbal_FrictionWheelFlag;//云台小陀螺标志位,云台开摩擦轮标志位
|
|
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_RammerSpinPositionPID;//拨弹盘旋转PID
|
|
|
|
/*
|
|
*函数简介:云台初始化
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:无
|
|
*/
|
|
void Gimbal_Init(void)
|
|
{/*
|
|
PID_PositionStructureInit(&Gimbal_YawAnglePositionPID,0);//Yaw轴陀螺仪闭环
|
|
PID_PositionSetParameter(&Gimbal_YawAnglePositionPID,140,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_YawAnglePositionPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_YawAnglePositionPID,-200,200);
|
|
PID_PositionStructureInit(&Gimbal_YawAngleSpeedPID,200);
|
|
PID_PositionSetParameter(&Gimbal_YawAngleSpeedPID,1000,0,1);
|
|
PID_PositionSetEkRange(&Gimbal_YawAngleSpeedPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_YawAngleSpeedPID,-30000,30000);
|
|
|
|
PID_PositionStructureInit(&Gimbal_PitchAnglePositionPID,0);//Pitch轴陀螺仪闭环
|
|
PID_PositionSetParameter(&Gimbal_PitchAnglePositionPID,6,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_PitchAnglePositionPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_PitchAnglePositionPID,-150,150);
|
|
PID_PositionStructureInit(&Gimbal_PitchAngleSpeedPID,150);
|
|
PID_PositionSetParameter(&Gimbal_PitchAngleSpeedPID,200,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_PitchAngleSpeedPID,-5,5);
|
|
PID_PositionSetOUTRange(&Gimbal_PitchAngleSpeedPID,-30000,30000);
|
|
*/
|
|
PID_PositionStructureInit(&Gimbal_YawAnglePositionPID,0);//Yaw轴陀螺仪闭环
|
|
PID_PositionSetParameter(&Gimbal_YawAnglePositionPID,10,0,650);
|
|
PID_PositionSetEkRange(&Gimbal_YawAnglePositionPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_YawAnglePositionPID,-200,200);
|
|
PID_PositionStructureInit(&Gimbal_YawAngleSpeedPID,200);
|
|
PID_PositionSetParameter(&Gimbal_YawAngleSpeedPID,150,0,20);
|
|
PID_PositionSetEkRange(&Gimbal_YawAngleSpeedPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_YawAngleSpeedPID,-50000,50000);
|
|
|
|
PID_PositionStructureInit(&Gimbal_PitchAnglePositionPID,0);//Pitch轴陀螺仪闭环
|
|
PID_PositionSetParameter(&Gimbal_PitchAnglePositionPID,40,0,700);
|
|
PID_PositionSetEkRange(&Gimbal_PitchAnglePositionPID,-0.1,0.1);
|
|
PID_PositionSetOUTRange(&Gimbal_PitchAnglePositionPID,-200,200);
|
|
PID_PositionStructureInit(&Gimbal_PitchAngleSpeedPID,150);
|
|
PID_PositionSetParameter(&Gimbal_PitchAngleSpeedPID,150,0.001,7);
|
|
PID_PositionSetEkRange(&Gimbal_PitchAngleSpeedPID,-0.1,0.1);
|
|
PID_PositionSetOUTRange(&Gimbal_PitchAngleSpeedPID,-50000,50000);
|
|
|
|
PID_PositionStructureInit(&Gimbal_L1_FrictionWheelPID,0);//左摩擦轮
|
|
PID_PositionSetParameter(&Gimbal_L1_FrictionWheelPID,15,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_L1_FrictionWheelPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_L1_FrictionWheelPID,-15000,15000);
|
|
|
|
PID_PositionStructureInit(&Gimbal_R1_FrictionWheelPID,0);//右摩擦轮
|
|
PID_PositionSetParameter(&Gimbal_R1_FrictionWheelPID,15,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_R1_FrictionWheelPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_R1_FrictionWheelPID,-15000,15000);
|
|
|
|
PID_PositionStructureInit(&Gimbal_L2_FrictionWheelPID,0);//左摩擦轮
|
|
PID_PositionSetParameter(&Gimbal_L2_FrictionWheelPID,15,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_L2_FrictionWheelPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_L2_FrictionWheelPID,-15000,15000);
|
|
|
|
PID_PositionStructureInit(&Gimbal_R2_FrictionWheelPID,0);//右摩擦轮
|
|
PID_PositionSetParameter(&Gimbal_R2_FrictionWheelPID,15,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_R2_FrictionWheelPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_R2_FrictionWheelPID,-15000,15000);
|
|
|
|
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);
|
|
|
|
|
|
|
|
Laser_Init();
|
|
}
|
|
|
|
/*
|
|
*函数简介:云台PID清理
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:无
|
|
*/
|
|
void Gimbal_CleanPID(void)
|
|
{
|
|
PID_PositionClean(&Gimbal_YawAnglePositionPID);
|
|
PID_PositionClean(&Gimbal_YawAngleSpeedPID);
|
|
PID_PositionClean(&Gimbal_PitchAnglePositionPID);
|
|
PID_PositionClean(&Gimbal_PitchAngleSpeedPID);
|
|
PID_PositionClean(&Gimbal_L1_FrictionWheelPID);
|
|
PID_PositionClean(&Gimbal_R1_FrictionWheelPID);
|
|
PID_PositionClean(&Gimbal_L2_FrictionWheelPID);
|
|
PID_PositionClean(&Gimbal_R2_FrictionWheelPID);
|
|
PID_PositionClean(&Gimbal_RammerSpinPositionPID);
|
|
}
|
|
|
|
|
|
/*
|
|
*函数简介:云台Pitch轴控制
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:根据拨杆或鼠标获得俯仰角度,映射比例在上方宏定义Gimbal_LeverSpeedMapRate更改
|
|
*备注:俯仰限幅由结构决定,参数由Parameter.h文件中的Pitch_GM6020PositionLowerLinit和Pitch_GM6020PositionUpperLinit决定
|
|
*备注:俯仰轴GM6020报文标识符和M2006高位ID一致,故均在拨弹盘控制函数中统一发送控制报文
|
|
*备注:在此函数中进行了视觉自瞄处理,由于视觉组摆烂,并没有开发出自瞄,也没用进行过联调,故自瞄部分没有拆出去独立函数
|
|
*/
|
|
void Gimbal_PitchControl(void)
|
|
{
|
|
if(Remote_StartFlag==2)Gimbal_PitchAnglePositionPID.Need_Value=0;//遥控器刚建立连接时,复位Pitch轴角度
|
|
if(((Remote_RxData.Remote_L_UD>1050 && RefereeSystem_Status==0) || (1024+Remote_RxData.Remote_Mouse_DU*3)<1000) && AttitudeAlgorithms_DegRoll>Pitch_GM6020AngleUpperLinit)
|
|
Gimbal_PitchAnglePositionPID.Need_Value-=Gimbal_LeverSpeedMapRate/8192.0f*360.0f;//通过遥控器或者鼠标获取俯仰情况
|
|
else if(((Remote_RxData.Remote_L_UD<1000 && RefereeSystem_Status==0) || (1024+Remote_RxData.Remote_Mouse_DU*3)>1050) && AttitudeAlgorithms_DegRoll<Pitch_GM6020AngleLowerLinit)
|
|
Gimbal_PitchAnglePositionPID.Need_Value+=Gimbal_LeverSpeedMapRate/8192.0f*360.0f;
|
|
|
|
if(Remote_RxData.Remote_LS==2 && Visual_ReceiveFlag==1 || Remote_RxData.Remote_Mouse_KeyR==1 && Visual_ReceiveFlag==1 )//自瞄,补偿角度
|
|
{
|
|
Visual_ReceiveFlag=0;
|
|
|
|
if((GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Position-1000)>Pitch_GM6020PositionUpperLinit && Visual_Pitch>0)
|
|
Gimbal_PitchAnglePositionPID.Need_Value=AttitudeAlgorithms_DegRoll-(float)Visual_Pitch;
|
|
if((GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Position-1000)<Pitch_GM6020PositionLowerLinit && Visual_Pitch<0)
|
|
Gimbal_PitchAnglePositionPID.Need_Value=AttitudeAlgorithms_DegRoll-(float)Visual_Pitch;
|
|
Gimbal_YawAnglePositionPID.Need_Value=AttitudeAlgorithms_DegYaw+(float)Visual_Yaw;
|
|
Remote_RxData.Remote_Mouse_KeyPushR=0;
|
|
Remote_RxData.Remote_LS=0;
|
|
}
|
|
|
|
//串级PID闭环Pitch角
|
|
PID_PositionCalc(&Gimbal_PitchAnglePositionPID,AttitudeAlgorithms_DegRoll);
|
|
Gimbal_PitchAnglePositionPID.OUT=kalmanFilter(&kfp_PITCH,Gimbal_PitchAnglePositionPID.OUT);
|
|
Gimbal_PitchAngleSpeedPID.Need_Value=-Gimbal_PitchAnglePositionPID.OUT;
|
|
PID_PositionCalc(&Gimbal_PitchAngleSpeedPID,GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Speed);
|
|
|
|
|
|
}
|
|
void Debug(void)
|
|
{
|
|
//UART2_Printf("%f,%f\n",Visual_Send1,Visual_Send2);
|
|
//UART2_Printf("%d\n",Gimbal_PitchAnglePositionPID.Need_Value);
|
|
//UART2_Printf("%d\n",Gimbal_PitchAngleSpeedPID.Need_Value);
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
*函数简介:云台Yaw轴控制
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:根据拨杆或鼠标获得偏航角度,映射比例在上方宏定义Gimbal_LeverSpeedMapRate和Gimbal_YawPitchSpeedRate更改
|
|
*备注:由于云台一直根据陀螺仪角度闭环,不需要考虑小陀螺问题
|
|
*/
|
|
void Gimbal_YawControl(void)
|
|
{
|
|
if(Remote_StartFlag==2)
|
|
Gimbal_YawAnglePositionPID.Need_Value=AttitudeAlgorithms_DegYaw;//遥控器刚建立连接时,复位Yaw轴角度
|
|
if((Remote_RxData.Remote_L_RL>1050 && RefereeSystem_Status==0) || 1024+PC_Spin*PC_Mouse_RLSensitivity>1050)//根据摇杆改变偏航
|
|
{
|
|
if(PC_Spin==0)
|
|
Gimbal_YawAnglePositionPID.Need_Value-=Gimbal_LeverSpeedMapRate*Gimbal_YawPitchSpeedRate*Gimbal_YawPitchSpeedRate*0.0439453125f*((Remote_RxData.Remote_L_RL-1024)/660.0f);
|
|
else
|
|
Gimbal_YawAnglePositionPID.Need_Value-=Gimbal_LeverSpeedMapRate*Gimbal_YawPitchSpeedRate*Gimbal_YawPitchSpeedRate*0.0439453125f*(PC_Spin*PC_Mouse_RLSensitivity/660.0f*2);
|
|
}
|
|
else if((Remote_RxData.Remote_L_RL<1000 && RefereeSystem_Status==0) || 1024+PC_Spin*PC_Mouse_RLSensitivity<1000)
|
|
{
|
|
if(PC_Spin==0)
|
|
Gimbal_YawAnglePositionPID.Need_Value+=Gimbal_LeverSpeedMapRate*Gimbal_YawPitchSpeedRate*Gimbal_YawPitchSpeedRate*0.0439453125f*((1024-Remote_RxData.Remote_L_RL)/660.0f);
|
|
else
|
|
Gimbal_YawAnglePositionPID.Need_Value-=Gimbal_LeverSpeedMapRate*Gimbal_YawPitchSpeedRate*Gimbal_YawPitchSpeedRate*0.0439453125f*(PC_Spin*PC_Mouse_RLSensitivity/660.0f*2);
|
|
}
|
|
|
|
//串级PID闭环Yaw角
|
|
PID_PositionCalc(&Gimbal_YawAnglePositionPID,AttitudeAlgorithms_DegYaw);
|
|
Gimbal_YawAngleSpeedPID.Need_Value=Gimbal_YawAnglePositionPID.OUT;
|
|
PID_PositionCalc(&Gimbal_YawAngleSpeedPID,GM6020_MotorStatus[0].Speed);
|
|
GM6020_CAN2SetLIDVoltage(Gimbal_YawAngleSpeedPID.OUT,0,0,0);
|
|
|
|
}
|
|
|
|
/*
|
|
*函数简介:摩擦轮控制
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:遥控左拨动开关向上拨(Remote_LS=1)开摩擦轮,摩擦轮打开的同时会打开激光
|
|
*/
|
|
void Gimbal_FiringMechanismControl(void)
|
|
{
|
|
if(((Remote_RxData.Remote_LS==1 && RefereeSystem_Status==0) || PC_FrictionWheel==1) && RefereeSystem_ShooterStatus==1)//摩擦轮开
|
|
{
|
|
Gimbal_L1_FrictionWheelPID.Need_Value=-Gimbal_FrictionWheelSpeed;Gimbal_R1_FrictionWheelPID.Need_Value=Gimbal_FrictionWheelSpeed;
|
|
Gimbal_L2_FrictionWheelPID.Need_Value=Gimbal_FrictionWheelSpeed;Gimbal_R2_FrictionWheelPID.Need_Value=- Gimbal_FrictionWheelSpeed;
|
|
Laser_ON();//开激光
|
|
Gimbal_FrictionWheelFlag=1;
|
|
}
|
|
else//摩擦轮关
|
|
{
|
|
Gimbal_L1_FrictionWheelPID.Need_Value=Gimbal_R1_FrictionWheelPID.Need_Value=Gimbal_L2_FrictionWheelPID.Need_Value=Gimbal_R2_FrictionWheelPID.Need_Value=0;
|
|
Laser_OFF();//关激光
|
|
Gimbal_FrictionWheelFlag=0;
|
|
}
|
|
|
|
PID_PositionCalc(&Gimbal_L1_FrictionWheelPID,M3508_MotorStatus[Gimbal_L1_FrictionWheel-0x201].RotorSpeed);
|
|
PID_PositionCalc(&Gimbal_R1_FrictionWheelPID,M3508_MotorStatus[Gimbal_R1_FrictionWheel-0x201].RotorSpeed);
|
|
PID_PositionCalc(&Gimbal_L2_FrictionWheelPID,M3508_MotorStatus[Gimbal_L2_FrictionWheel-0x201].RotorSpeed);
|
|
PID_PositionCalc(&Gimbal_R2_FrictionWheelPID,M3508_MotorStatus[Gimbal_R2_FrictionWheel-0x201].RotorSpeed);
|
|
M3508_CANSetLIDCurrent(Gimbal_L1_FrictionWheelPID.OUT,Gimbal_R1_FrictionWheelPID.OUT,Gimbal_L2_FrictionWheelPID.OUT,Gimbal_R2_FrictionWheelPID.OUT);
|
|
|
|
}
|
|
|
|
/*
|
|
*函数简介:拨弹盘控制
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:俯仰轴GM6020报文标识符和M2006高位ID一致,故均在拨弹盘控制函数中统一发送控制报文
|
|
*/
|
|
void Gimbal_Rammer(void)
|
|
{
|
|
if(Gimbal_FrictionWheelFlag==1)
|
|
{
|
|
if((Remote_RxData.Remote_ThumbWheel<1000 && RefereeSystem_Status==0) || PC_Fire==1)
|
|
Gimbal_RammerSpinPositionPID.Need_Value=-Gimbal_RammerSpeed;
|
|
else if((Remote_RxData.Remote_ThumbWheel>1050 && RefereeSystem_Status==0) || PC_Ejection==1)
|
|
Gimbal_RammerSpinPositionPID.Need_Value=Gimbal_RammerSpeed;
|
|
else
|
|
Gimbal_RammerSpinPositionPID.Need_Value=0;
|
|
}
|
|
else Gimbal_RammerSpinPositionPID.Need_Value=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_RammerSpinPositionPID.OUT,0);
|
|
//M3508_CANSetHIDCurrent(0,0,Gimbal_RammerSpinPositionPID.OUT,0);
|
|
}
|
|
|
|
/*
|
|
*函数简介:拨弹盘单发
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:俯仰轴GM6020报文标识符和M2006高位ID一致,故均在拨弹盘控制函数中统一发送控制报文
|
|
*/
|
|
void Gimbal_Rammer_Single_fire(void)
|
|
{
|
|
if(Gimbal_FrictionWheelFlag==1)
|
|
{
|
|
if(((Remote_RxData.Remote_ThumbWheel<1000 && RefereeSystem_Status==0) || PC_Fire==1) && Fire_Flag != 1;)
|
|
{
|
|
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)
|
|
{
|
|
Gimbal_RammerSpinPositionPID.Need_Value=Gimbal_RammerSpinPositionPID.Need_Value+Gimbal_RammerSpeed;//退弹
|
|
Fire_Flag=0;//退弹就不用一发一发退了
|
|
}
|
|
else
|
|
Gimbal_RammerSpinPositionPID.Need_Value=0;//停止开火！
|
|
Fire_Flag=0;
|
|
}
|
|
else Gimbal_RammerSpinPositionPID.Need_Value=0;//防止意外走火？
|
|
|
|
PID_PositionCalc(&Gimbal_RammerSpinPositionPID,M3508_MotorStatus[Gimbal_RammerSpinMotor-0x201].RotorSpeed);
|
|
|
|
//M2006_CANSetHIDCurrent(0,Gimbal_PitchAngleSpeedPID.OUT,Gimbal_RammerSpinPositionPID.OUT,0);
|
|
|
|
//M3508_CANSetLIDCurrent(0,0,Gimbal_RammerSpinPositionPID.OUT,0);
|
|
//M3508_CANSetHIDCurrent(0,0,Gimbal_RammerSpinPositionPID.OUT,0);
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
*函数简介:云台运动控制
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:无
|
|
*/
|
|
void Gimbal_MoveControl(void)
|
|
{
|
|
Gimbal_PitchControl();//云台Pitch轴控制
|
|
Gimbal_YawControl();//云台Yaw轴控制
|
|
|
|
Gimbal_FiringMechanismControl();//摩擦轮控制
|
|
Gimbal_Rammer();//拨弹盘控制
|
|
//UART2_Printf("%f,%f,%f,%f\n",Gimbal_PitchAngleSpeedPID.Need_Value,Gimbal_PitchAngleSpeedPID.Now_Value,Gimbal_PitchAnglePositionPID.Now_Value,Gimbal_PitchAnglePositionPID.Need_Value);
|
|
}
|