253 lines
11 KiB
C
253 lines
11 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 "UART.h"
|
|
|
|
#define Gimbal_YawMotor GM6020_1//Yaw轴电机
|
|
#define Gimbal_PitchMotor GM6020_2//Pitch轴电机
|
|
#define Gimbal_L_FrictionWheel M3508_1//左摩擦轮
|
|
#define Gimbal_R_FrictionWheel M3508_2//右摩擦轮
|
|
|
|
|
|
|
|
uint8_t Gimbal_FrictionWheelFlag;//云台小陀螺标志位,云台开摩擦轮标志位
|
|
|
|
PID_PositionInitTypedef Gimbal_YawAnglePositionPID,Gimbal_YawAngleSpeedPID;//Yaw轴GM6020电机PID
|
|
PID_PositionInitTypedef Gimbal_PitchAnglePositionPID,Gimbal_PitchAngleSpeedPID;//Pitch轴GM6020电机PID
|
|
PID_PositionInitTypedef Gimbal_L_FrictionWheelPID,Gimbal_R_FrictionWheelPID;//摩擦轮转速PID
|
|
PID_PositionInitTypedef Gimbal_RammerSpinSpeedPID;//拨弹盘旋转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,20);
|
|
PID_PositionSetEkRange(&Gimbal_YawAnglePositionPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_YawAnglePositionPID,-350,350);
|
|
PID_PositionStructureInit(&Gimbal_YawAngleSpeedPID,200);
|
|
PID_PositionSetParameter(&Gimbal_YawAngleSpeedPID,150,0,10);
|
|
PID_PositionSetEkRange(&Gimbal_YawAngleSpeedPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_YawAngleSpeedPID,-50000,50000);
|
|
|
|
PID_PositionStructureInit(&Gimbal_PitchAnglePositionPID,0);//Pitch轴陀螺仪闭环
|
|
PID_PositionSetParameter(&Gimbal_PitchAnglePositionPID,5,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_PitchAnglePositionPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_PitchAnglePositionPID,-150,150);
|
|
PID_PositionStructureInit(&Gimbal_PitchAngleSpeedPID,150);
|
|
PID_PositionSetParameter(&Gimbal_PitchAngleSpeedPID,250,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_PitchAngleSpeedPID,-1,1);
|
|
PID_PositionSetOUTRange(&Gimbal_PitchAngleSpeedPID,-30000,30000);
|
|
|
|
PID_PositionStructureInit(&Gimbal_L_FrictionWheelPID,0);//左摩擦轮
|
|
PID_PositionSetParameter(&Gimbal_L_FrictionWheelPID,16,0,30);
|
|
PID_PositionSetEkRange(&Gimbal_L_FrictionWheelPID,-5,5);
|
|
PID_PositionSetOUTRange(&Gimbal_L_FrictionWheelPID,-15000,15000);
|
|
PID_PositionStructureInit(&Gimbal_R_FrictionWheelPID,0);//右摩擦轮
|
|
PID_PositionSetParameter(&Gimbal_R_FrictionWheelPID,16,0,30);
|
|
PID_PositionSetEkRange(&Gimbal_R_FrictionWheelPID,-5,5);
|
|
PID_PositionSetOUTRange(&Gimbal_R_FrictionWheelPID,-15000,15000);
|
|
|
|
PID_PositionStructureInit(&Gimbal_RammerSpinSpeedPID,-Gimbal_RammerSpeed);//拨弹盘
|
|
PID_PositionSetParameter(&Gimbal_RammerSpinSpeedPID,16,0,0);
|
|
PID_PositionSetEkRange(&Gimbal_RammerSpinSpeedPID,-20,20);
|
|
PID_PositionSetOUTRange(&Gimbal_RammerSpinSpeedPID,-30000,30000);
|
|
|
|
|
|
|
|
|
|
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_L_FrictionWheelPID);
|
|
PID_PositionClean(&Gimbal_R_FrictionWheelPID);
|
|
PID_PositionClean(&Gimbal_RammerSpinSpeedPID);
|
|
}
|
|
|
|
/*
|
|
*函数简介:云台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_Mouse_KeyR==1 && Visual_ReceiveFlag==1)//自瞄,补偿角度
|
|
{
|
|
Visual_ReceiveFlag=0;
|
|
if(GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Position>Pitch_GM6020PositionUpperLinit && Visual_Pitch>0)Gimbal_PitchAnglePositionPID.Need_Value=AttitudeAlgorithms_DegRoll-Visual_Pitch;
|
|
if(GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Position<Pitch_GM6020PositionLowerLinit && Visual_Pitch<0)Gimbal_PitchAnglePositionPID.Need_Value=AttitudeAlgorithms_DegRoll-Visual_Pitch;
|
|
Gimbal_YawAnglePositionPID.Need_Value=AttitudeAlgorithms_DegYaw+Visual_Yaw;
|
|
Remote_RxData.Remote_Mouse_KeyPushR=0;
|
|
}
|
|
|
|
//串级PID闭环Pitch角
|
|
|
|
PID_PositionCalc(&Gimbal_PitchAnglePositionPID,AttitudeAlgorithms_DegRoll);
|
|
Gimbal_PitchAngleSpeedPID.Need_Value=Gimbal_PitchAnglePositionPID.OUT;
|
|
|
|
PID_PositionCalc(&Gimbal_PitchAngleSpeedPID,GM6020_MotorStatus[Gimbal_PitchMotor-0x205].Speed);
|
|
//UART2_Printf("%d\n",AttitudeAlgorithms_DegRoll);
|
|
//UART2_Printf("%d\n",AttitudeAlgorithms_DegYaw);
|
|
}
|
|
|
|
/*
|
|
*函数简介:云台PID清理
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:无
|
|
*/
|
|
void Debug(void)
|
|
{
|
|
//UART2_Printf("%f,%f,%f,%f\n",Gimbal_PitchAngleSpeedPID.Need_Value,Gimbal_PitchAngleSpeedPID.Now_Value,Gimbal_PitchAnglePositionPID.Now_Value,Gimbal_PitchAnglePositionPID.Need_Value);
|
|
UART2_Printf("%f\n",AttitudeAlgorithms_DegRoll);
|
|
|
|
}
|
|
|
|
|
|
/*
|
|
*函数简介:云台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_L_FrictionWheelPID.Need_Value=-Gimbal_FrictionWheelSpeed;
|
|
Gimbal_R_FrictionWheelPID.Need_Value=Gimbal_FrictionWheelSpeed;
|
|
Laser_ON();//开激光
|
|
Gimbal_FrictionWheelFlag=1;
|
|
}
|
|
else//摩擦轮关
|
|
{
|
|
Gimbal_L_FrictionWheelPID.Need_Value=Gimbal_R_FrictionWheelPID.Need_Value=0;
|
|
Laser_OFF();//关激光
|
|
Gimbal_FrictionWheelFlag=0;
|
|
}
|
|
|
|
PID_PositionCalc(&Gimbal_L_FrictionWheelPID,M3508_MotorStatus[Gimbal_L_FrictionWheel-0x201].RotorSpeed);
|
|
PID_PositionCalc(&Gimbal_R_FrictionWheelPID,M3508_MotorStatus[Gimbal_R_FrictionWheel-0x201].RotorSpeed);
|
|
M3508_CANSetLIDCurrent(Gimbal_L_FrictionWheelPID.OUT,Gimbal_R_FrictionWheelPID.OUT,0,0);
|
|
}
|
|
|
|
/*
|
|
*函数简介:拨弹盘控制
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:俯仰轴GM6020报文标识符和M2006高位ID一致,故均在拨弹盘控制函数中统一发送控制报文
|
|
*/
|
|
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;
|
|
else if((Remote_RxData.Remote_ThumbWheel>1050 && RefereeSystem_Status==0) || PC_Ejection==1)
|
|
Gimbal_RammerSpinSpeedPID.Need_Value=Gimbal_RammerSpeed;
|
|
else
|
|
Gimbal_RammerSpinSpeedPID.Need_Value=0;
|
|
}
|
|
else Gimbal_RammerSpinSpeedPID.Need_Value=0;
|
|
|
|
PID_PositionCalc(&Gimbal_RammerSpinSpeedPID,M2006_MotorStatus[6].RotorSpeed);
|
|
M2006_CANSetHIDCurrent(0,Gimbal_PitchAngleSpeedPID.OUT,Gimbal_RammerSpinSpeedPID.OUT,0);
|
|
}
|
|
|
|
/*
|
|
*函数简介:云台运动控制
|
|
*参数说明:无
|
|
*返回类型:无
|
|
*备注:无
|
|
*/
|
|
void Gimbal_MoveControl(void)
|
|
{
|
|
Gimbal_PitchControl();//云台Pitch轴控制
|
|
Gimbal_YawControl();//云台Yaw轴控制
|
|
|
|
Gimbal_FiringMechanismControl();//摩擦轮控制
|
|
Gimbal_Rammer();//拨弹盘控制
|
|
}
|