/*
 * swing_action.c
 *
 *  Created on: Mar 4, 2026
 *      Author: 42961
 */
#include "swing_action.h"
#include "Handset_Status_Setting.h"

#include "fsm_state.h"
#include "BHBF_ROBOT.h"


#define TT_One_Deg_Count 	11014///32768*121/360(减速比121)=11014
#define LEFT_LIMIT 1
#define RIGHT_LIMIT 2
float Swing_Speed_Deg_Sencond=201.7;//HJ32-121
//int center_angle;
//int offset_angle;
//int center_position;    //中间位置-944334     -1354574
int limit_record=0;
float left_compare_value=-30;
float right_compare_value=30;
float left_compare_updata;
float right_compare_updata;

int lef_positon=-569580;
int Right_positon=-1230420;

int swing_delay_count=0;
int com_delay=0;
int pos_flag=0;   //控制延时停摆臂只读一次角度
int pos_direction=0; //获取延时停摆臂的方向
//手动摆臂函数
void Robot_Swing_Operation_Function()
{
	com_delay=(P_MK32->CH10_LD1+1000)/2;
	if(P_MK32->CH0_RY_H>500)
	{
		swing_delay_count++;
		if(swing_delay_count>com_delay)
		{
			pos_flag=0;
			pos_direction=1;
			swing_delay_count=0;
			GV.SwingMotor.Position_immediately1_Lag2=1;
			GV.Tar_Position_angle=90;
			GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
		}


	}
	else if(P_MK32->CH0_RY_H<-500)
	{
		swing_delay_count++;
		if(swing_delay_count>com_delay)
		{
			pos_flag=0;
			pos_direction=2;
			swing_delay_count=0;
			GV.SwingMotor.Position_immediately1_Lag2=1;
			GV.Tar_Position_angle=-90;
			GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
		}
	}
	else
	{
		Move_Swing_Halt_Func_Do();
		swing_delay_count=0;
		pos_direction=0;
	}
}



//摆臂电机左转延时生效
void Move_Swing_Left_Func_Do_lay(void)
{
	GV.SwingMotor.Position_immediately1_Lag2=2;
	GV.Tar_Position_angle=left_compare_value;
	GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
}

//摆臂电机右转延时生效
void Move_Swing_Right_Func_Do_lay(void)
{

	GV.SwingMotor.Position_immediately1_Lag2=2;
	GV.Tar_Position_angle=right_compare_value;
	GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
}

//摆臂电机左转立即执行
void Move_Swing_Left_Func_Do_imm(void)
{

	//pos_flag=0;
	GV.SwingMotor.Position_immediately1_Lag2=1;

	GV.Tar_Position_angle=left_compare_value;
	GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;


}

//摆臂电机右转立即执行
void Move_Swing_Right_Func_Do_imm(void)
{

	//pos_flag=0;
	GV.SwingMotor.Position_immediately1_Lag2=1;
	GV.Tar_Position_angle=right_compare_value;
	GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;

}

int32_t Position_angle;

void Move_Swing_Halt_Func_Do(void)
{
	GV.SwingMotor.Position_immediately1_Lag2=1;
	if(pos_flag==0)
	{
		pos_flag=1;
		if(pos_direction==1)     //顺时针
		{
			//延几度再停
			Position_angle= -(float)GV.SwingMotor.Real_Position/11014
					+GV.Robot_Swing_Speed*0.5;
			if(Position_angle>90)     //别超右极限
			{
				Position_angle=90;
			}
			GV.Tar_Position_angle=Position_angle;
			GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
		}
		else if(pos_direction==2)   //逆时针
		{
			//延几度再停
			Position_angle= -(float)GV.SwingMotor.Real_Position/11014
					-GV.Robot_Swing_Speed*0.5;
			if(Position_angle<-90)   //别超左极限
			{
				Position_angle=-90;
			}
			GV.Tar_Position_angle=Position_angle;
			GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
		}
		else
		{
			GV.Tar_Position_Velcity_Degree_S=0;
			GV.SwingMotor.Tar_Position_count=0;

		}

    }
	else
	{
		//GV.Tar_Position_Velcity_Degree_S=0;
		//GV.SwingMotor.Tar_Position_count=0;
		//GV.Tar_Position_Velcity_Degree_S=0;
	}



	//GV.SwingMotor.Tar_Position_count=0;
    //GV.Tar_Position_Velcity_Degree_S=0;

}