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

//Manual_State_Entry, Manual_State_Do, Manual_State_Exit

void Manual_State_Do(void)
{

}

void LaneChangeDistance_Setting_State_Do(void)
{
	if (GV.DH_CAN.Increase == 1)
	{
		GV.LaneChangeDistance++;
	}
	if (GV.DH_CAN.Decrease == 1)
	{
		GV.LaneChangeDistance--;
	}
	if (GV.LaneChangeDistance <= 0)
	{
		GV.LaneChangeDistance = 0;
	}
	if (GV.LaneChangeDistance >= 4096)
	{
		GV.LaneChangeDistance = 4096;
	}

	CV.LaneChangeDistance = GV.LaneChangeDistance;
	GF_BSP_EEPROM_Set_CV(CV);
}

void BackWardsDistance_Setting_State_Do(void)
{
	if (GV.DH_CAN.Increase == 1)
	{
		GV.BackwardDistance++;
	}
	if (GV.DH_CAN.Decrease == 1)
	{
		GV.BackwardDistance--;
	}
	if (GV.BackwardDistance <= 0)
	{
		GV.BackwardDistance = 0;
	}
	if (GV.BackwardDistance >= 4096)
	{
		GV.BackwardDistance = 4096;
	}

	CV.BackWardsDistance = GV.BackwardDistance;
	GF_BSP_EEPROM_Set_CV(CV);
}

void Auto_State_Do(void)
{

}

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

}

void Forwards_State_Do(void)
{
	GV.LeftMotor.Target_Velcity = GV.Move_Speed;
	GV.RightMotor.Target_Velcity = GV.Move_Speed;
}

void Backwards_State_Do(void)
{
	GV.LeftMotor.Target_Velcity = -GV.Move_Speed;
	GV.RightMotor.Target_Velcity = -GV.Move_Speed;
}
void TurnLeft_State_Do(void)
{
	GV.LeftMotor.Target_Velcity = -GV.Move_Speed;
	GV.RightMotor.Target_Velcity = GV.Move_Speed;
}
void TurnRight_State_Do(void)
{
	GV.LeftMotor.Target_Velcity = GV.Move_Speed;
	GV.RightMotor.Target_Velcity = -GV.Move_Speed;
}
void HALT_State_Do(void)
{
	GV.LeftMotor.Target_Velcity = 0;
	GV.RightMotor.Target_Velcity = 0;
}
//Swing Motor

void SwingHALT_State_Do(void)
{
}
void SwingLeft_State_Do(void)
{
	GV.SwingMotor.Target_Position += 100;
}
void SwingRight_State_Do(void)
{
	GV.SwingMotor.Target_Position -= 100;
}

void OtherMode_State_Do(void)
{

}