/*
 * robot_state.c
 *
 *  Created on: Aug 2, 2024
 *      Author: akeguo
 */
#include "robot_state.h"
#include "fsm.h"

#include "BSP/bsp_GPIO.h"
#include "MSP/msp_PID.h"


int32_t *Robot_Platform_Speed[10]; //目标速度

int32_t *DHRoughening_LaneChangeDistance[10]; //目标速度

int32_t *Robot_LeftCompensation;
int32_t *Robot_RightCompensation;
int32_t *Roughenging_LeftCompensation_Range;
int32_t *Robot_RightCompensation_Range;
int32_t *Robot_Angle_Start_BASE;
int32_t *Robot_Angle;
int32_t *Robot_Deri_Speed;


double Angle_Error_LLL=0;
int CRLU_Flag=0;















//Manual_State_Entry, Manual_State_Do, Manual_State_Exit
void Calbrate_Robot_Positon(int position);
int32_t Horizontal_Target_Angle = 0;

void Move_Forwards_Do(int32_t Target_Angle);
void Move_Backwards_Do(int32_t Target_Angle);

void Move_PushRod_Halt_Func_Do(void)
{
	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
	GF_BSP_GPIO_SetIO(3, 1);
}
void Move_PushRod_Up_Func_Do(void)
{
	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
	GF_BSP_GPIO_SetIO(3, 1);

}
void Move_PushRod_Down_Func_Do(void)
{
	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
	GF_BSP_GPIO_SetIO(3, 0);
}

void Move_PushRod_Halt_Func_Do_1(void)
{
	GF_BSP_GPIO_SetIO(4, 1);
	GF_BSP_GPIO_SetIO(5, 1);
}

void Move_PushRod_Up_Func_Do_1(void)
{
	GF_BSP_GPIO_SetIO(4, 0);
	GF_BSP_GPIO_SetIO(5, 1);

}

void Move_PushRod_Down_Func_Do_1(void)
{
	GF_BSP_GPIO_SetIO(4, 1);
	GF_BSP_GPIO_SetIO(5, 0);
}



















///////////////////////////////////////////////////////////////////////////////////////////////
void Abnormal_State_Do(void)
{
	LOG("Abnormal_State_Do");

}



void HALT_State_Do(void)
{
	GV.LeftMotor.Target_Velcity = 0;
	GV.RightMotor.Target_Velcity = 0;
}


char is_within_the_target_angle_flag =0;
//水平方向前进，保持水平方向

//水平方向后退，保持水平方向



/* 终端上下移动，在洗舱里是抬头低头，这里是上下，直接沿用Tilt Up 和 TiltDown函数
 * */
void TiltUp_Do()
{
//	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
//	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);

	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 0);
}

void TiltDown_Do()
{

//	if (GV.ForceValue >= CV.PV.EndPushForceValue)
//	{
//		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
//		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
//	}
//	else
//	{
//		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
//		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 0);

		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
		GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
//	}
}

void TiltHalt_Do()
{
	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
	GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
}

/* 拉毛函数控制
 * */