#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();//拨弹盘控制
}