/*
 * 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 middle_position=-944334;
int center_angle;
int offset_angle;
int present_angle;
int center_position;    //中间位置-944334
int limit_record=0;
int left_compare_value;
int right_compare_value;

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

void Robot_Swing_Operation_Function()
{
	if(P_MK32->CH0_RY_H>300)
	{
		GV.SwingMotor.Position_immediately1_Lag2=1;
		GV.SwingMotor.Tar_Position_count=-944334-90*TT_One_Deg_Count;
		GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond;

	}
	else if(P_MK32->CH0_RY_H<-300)
	{
		GV.SwingMotor.Position_immediately1_Lag2=1;
		GV.SwingMotor.Tar_Position_count=-944334+90*TT_One_Deg_Count;
		GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond;
	}
	else
	{
		Move_Swing_Halt_Func_Do();
	}
/*	*Swing_Speed = CV.PV.Swing_Speed;*/
//	action_perfrom(Set_Swing_States,sizeof(Set_Swing_States) / sizeof(transition_t), CurrentSwingState);
}



//摆臂电机左转延时生效
void Move_Swing_Left_Func_Do_lay(void)
{
	GV.SwingMotor.Position_immediately1_Lag2=2;
	GV.SwingMotor.Tar_Position_count=left_compare_value;
	GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond;
}

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

	GV.SwingMotor.Position_immediately1_Lag2=2;
	GV.SwingMotor.Tar_Position_count=right_compare_value;
	GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond;
}

//摆臂电机左转立即执行
void Move_Swing_Left_Func_Do_imm(void)
{
	if(GV.PV.Robot_Swing_Range_Angle!=center_angle)
	{
		center_angle=GV.PV.Robot_Swing_Range_Angle;
		center_position=GV.SwingMotor.Real_Position;
	}
	left_compare_value=center_position+(center_angle*TT_One_Deg_Count/2);
	right_compare_value=center_position-(center_angle*TT_One_Deg_Count/2);

	GV.SwingMotor.Position_immediately1_Lag2=1;
	GV.SwingMotor.Tar_Position_count=left_compare_value;
	GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond;


}

//摆臂电机右转立即执行
void Move_Swing_Right_Func_Do_imm(void)
{
	if(GV.PV.Robot_Swing_Range_Angle!=offset_angle)
	{
		offset_angle=GV.PV.Robot_Swing_Range_Angle;
		present_angle=-(GV.SwingMotor.Real_Position-middle_position)/TT_One_Deg_Count;
		center_angle=GV.SwingMotor.Real_Position;
	}
	left_compare_value=center_position+(center_angle*TT_One_Deg_Count/2);
	right_compare_value=center_position-(center_angle*TT_One_Deg_Count/2);

	GV.SwingMotor.Position_immediately1_Lag2=1;
	GV.SwingMotor.Tar_Position_count=right_compare_value;
	GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond;

}

void Move_Swing_Halt_Func_Do(void)
{
	GV.SwingMotor.Position_immediately1_Lag2=1;
	GV.SwingMotor.Tar_Position_count=0;
	GV.SwingMotor.Tar_Position_Velcity_RPM=0;

//	GV.SwingMotor.Target_Velcity = 0;
}