/*
 * robot_state.h
 *
 *  Created on: Aug 2, 2024
 *      Author: akeguo
 */

#ifndef INC_ROBOT_STATE_H_
#define INC_ROBOT_STATE_H_

#include "BHBF_ROBOT.h"
#include "bsp_include.h"



typedef struct ChgRoad_Parameters
{
	double KP11;
	double KP22;
	double KP33;
	double KP44;
	double KPCR1;
	double KDCR1;
	int32_t change_road_straight_count;
	int32_t change_road_distance;  //换道时的距离（执行换道前进程序的次数，可以理解为执行次数越多，机器人走的越远）
	int32_t change_road_status_flag;       //换道标志位
	int32_t change_road_finish_flag;

	double speed;
	/**转弯*/
	bool isTurn;

	int Ini_Postition_L;
	int Final_Postition_L;
	int Ini_Postition_R;
	int Final_Postition_R;
	double ChgWidth;
}ChgRoad_Parameters;


typedef struct
{
	double KP1;
	double KP2;
	double KP3;
	double KP4;
	double KPFB1;
	double KDFB1;
	int32_t deltaAngle1;
	int32_t deltaAngle2;
	int32_t deltaAngle3;
	double ExpectationAngle;
	int32_t change_road_finish_flag;

}AutoMove_Parameters;


extern ChgRoad_Parameters Chg_Pa;
extern int* AuTo_Flag;


void Manual_State_Entry(void);
void Manual_State_Do(void);
void LaneChangeDistance_Setting_State_Do(void);
void BackWardsDistance_Setting_State_Do(void);
void Auto_State_Do(void);
void Abnormal_State_Do(void);
//前进、后退、左转、右转
void Forwards_State_Do(void);
void Backwards_State_Do(void);
void TurnLeft_State_Do(void);
void TurnRight_State_Do(void);

void HALT_State_Do(void);//停止


void SwingHALT_State_Do(void);
void SwingLeft_State_Do(void);
void SwingRight_State_Do(void);


void SetLaneChangeDistance_State_Do(void);
void SetBackwardsDistance_State_Do(void);
void OtherMode_State_Do(void);



void TiltUp_Do();
void TiltDown_Do();
void TiltHalt_Do();

void TiltHome_Do();
void TiltCurrentModeDown_Do();

void SwingHome_Do();

void Auto();
void ChgLeft();
void ChgRight();
void ChgUp();
void ChgDown();
void ChgFinish();
#endif /* INC_ROBOT_STATE_H_ */