/*
 * fsm_state_control.c
 *
 *  Created on: 2025年12月29日
 *      Author: xsq
 */

#include <bsp_tempature.h>
#include <math.h>

#include "BHBF_ROBOT.h"
#include "bsp_include.h"
#include "msp_PID.h"
#include "msp_MK32_1.h"
#include "motors.h"
#include "fsm_state.h"
#include "motors_power_action.h"
#include "fsm_state_control.h"
#include "roughening_end_actions.h"
#include "tilt_action.h"
#include "robot_move_actions.h"

#define Move_Horizontal_AngleThreshold_D 85
#define M_PI       3.14159265358979323846
double speed_selection;
char is_upper_computer_take_over_control = 0;
int32_t isAdjustCompensation = 1; //第一次将保存的补偿值赋给车体
int32_t isMannualAdjustPress = 1; //手动模式下 第一次自动调节压力
int index_counter = 0;

uint8_t MotorErrorDetect();
void robot_backwards();
void robot_forwards();
void joysticker_manual_control();
int LaneChangeControl();

void DHRougheningControl(); //拉毛前端控制
void Mannual_TiltControl();
//void Mannual_TiltControl1();
void Mannual_TiltControl2();

int Auto_TiltControl();


void IV_control();

void MoveControl();
void Pressure_Safety_Monitor(void);


int LaneChangeControl();

void Horizontal_Lane_Change_Turn_To_Left_Control();
void Horizontal_Lane_Change_Turn_To_Right_Control();

void Vertical_Lane_Change_Right_Control();
void Vertical_Lane_Change_Left_Control();

void RegionAuto_Horizontal_Distance_Err();
void RegionAuto_Vertical_Distance_Err();

void RegionAuto_Horizontal_RightToLeft();
void RegionAuto_Horizontal_LeftToRight();

void RegionAuto_Vertical_RightToLeft();
void RegionAuto_Vertical_LeftToRight();

void Manual_Mode_Move_Forwards();
void Manual_Mode_Move_Backwards();

int RegionAutoMoveControl();


void Manual_Mode_Joystick_Control();
void Manual_Mode_Joystick_Control1();
void Manual_Mode_Joystick_Control2();
void PV_Data_Reading();



int AbnormalDetect(); //异常检测



int EmegencyStop_Step = 10;
int EmegencyStop_StepNum = 10;
int RegionAuto_MoveTask_Test_Result = 0;







void Fsm_Init()
{

		IV_control();
		//机器人运动状态初始化
		fsm_state_init(&current_robot_move_state, &robot_halt_state);
		//拉毛前端初始化
		fsm_state_init(&current_roughening_state, &roughening_halt_state);
		//tilt初始化
		fsm_state_init(&current_tilt_state, &tilt_halt_state);
		//机器人电机供电状态初始化
		fsm_state_init(&current_motor_power_state, &motor_power_off_state);



		GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
		IV.RobotRestart = 1; //机器人上电初始化，需要通知机器人



}
void GF_Dispatch()
{

	fsm_state_run(&current_robot_move_state);  /*机器人移动*/
	fsm_state_run(&current_motor_power_state); /*供电*/
	fsm_state_run(&current_roughening_state); /*拉毛盘*/
	fsm_state_run(&current_tilt_state);   /*推杆*/

	PV_Data_Reading();
	IV_control();
	/*包含上电按钮检测 急停 SBUS 串口 等*/
	if (AbnormalDetect() == 1)
	{
		return;
	}

	/* 移动到Fsm_Init()里，实现软急停断48V后，只能重新上电恢复。
	 * 但移上去出现有点问题：，电机、推杆首次无法正常启动，需排查初次上电完整启动流程后才能考虑插入
	 *  */
//	/***上电检测通过 电机上电**/
	fsm_state_set(&current_motor_power_state, &motor_power_on_state); /* 上电关 此时设为开 */

	/*按下遥控 app无法选择*/
		if ((fabs(P_MK32->CH2_LY_V) <= 200) && (fabs(P_MK32->CH3_LY_H) <= 200)
		        && (fabs(P_MK32->CH0_RY_H) <= 200)
		        && (fabs(P_MK32->CH1_RY_V) <= 200) && (P_MK32->CH4_SA == 0)
		        && (P_MK32->CH5_SB == 0) && (P_MK32->CH6_SC != -1000)&& (P_MK32->CH7_SD != -1000))
		{
			IV.IsWorking = 0;
		}
		else
		{
			IV.IsWorking = 1;
		}

	MoveControl();


}





/*
 * 三级压力报警与震荡检测模块 V2.2
 * 功能：适配低频传感器（10/40Hz），通过降采样实现高频控制下的震荡检测
 */

// 全局状态变量
volatile uint8_t g_IsPressureLocked = 0;

// 压力保护参数配置
//volatile int g_Pressure_Lock_Threshold = 5000;   // 锁定阈值
volatile int g_Pressure_Lock_Threshold = 3500;
volatile int g_Pressure_Warn_Threshold = 0;      // 预警阈值
volatile int g_Pressure_Unlock_Threshold = 0;    // 解锁阈值

// 【新增】震荡检测配置参数
// 假设本函数每2ms调用一次：
// 设为 1 -> 每2ms检测一次 (适合高频传感器)
// 设为 5 -> 每10ms检测一次 (适合100Hz传感器)
// 设为 20 -> 每40ms检测一次 (适合25Hz传感器)
//volatile uint8_t g_Pressure_Oscillation_Step = 5;
volatile int g_Pressure_Oscillation_Step = 5;


volatile int OSCILLATION_THRESHOLD = 1000;     // 震荡判定阈值：相邻两次采样差值超过此值视为异常 (根据实际传感器量程调整)
volatile int OSCILLATION_CONFIRM_COUNT = 5;    // 震荡确认次数：连续5次(10ms)检测到剧烈跳变即触发
volatile int pressure_over_count = 0;
volatile int last_sampled_pressure = 0;       // 上一次“有效采样点”的压力
volatile int oscillation_count = 0;           // 震荡计数器
//volatile uint8_t sample_timer = 0;            // 降采样计时器
volatile int sample_timer = 0;

volatile int diff = 120;//测试是否真的能做出震荡判断


void Pressure_Safety_Monitor(void)
{
    // ============================================================
    // 【配置参数】
    // ============================================================
    const int COUNT_LIMIT = 250;                // 0.5s 持续超压滤波
//    const int OSCILLATION_THRESHOLD = 1000;     // 震荡判定阈值：差值绝对值
//    const int OSCILLATION_CONFIRM_COUNT = 5;    // 震荡确认次数

    // 动态计算滞回区间：即三级压力预警的剩余两个值
    g_Pressure_Warn_Threshold = g_Pressure_Lock_Threshold - 20;
    g_Pressure_Unlock_Threshold = g_Pressure_Lock_Threshold - 50;



    int current_press = IV.Press;

    // ============================================================
    // 1. 降采样震荡检测逻辑
    // ============================================================
    sample_timer++;

    // 只有当计时器达到设定步长时，才进行一次“有效对比”
    if (sample_timer >= g_Pressure_Oscillation_Step)
    {
//        int diff = current_press - last_sampled_pressure;
    	diff = current_press - last_sampled_pressure;
        if (diff < 0) diff = -diff; // 取绝对值

        // 判断是否剧烈跳变
        if (diff >= OSCILLATION_THRESHOLD)
        {
            oscillation_count++;
        }
        else
        {
            // 波动恢复正常，震荡计数清零
            oscillation_count = 0;
        }

        last_sampled_pressure = current_press;
        sample_timer = 0;
    }

    // ============================================================
    // 2. 状态机逻辑 (修复解锁漏洞)
    // ============================================================

    // --- 情况 A：如果当前已经锁定 ---
    if (g_IsPressureLocked == 1)
    {
        // 判断是否可以解锁
        // 逻辑：必须同时满足两个条件才算安全
        // 1. 压力值已经降下来 (解除超压风险)
        // 2. 震荡已经消失 (解除乱跳风险)

        bool is_pressure_low = (current_press < g_Pressure_Unlock_Threshold);
        bool is_oscillation_free = (oscillation_count == 0);

        if (is_pressure_low && is_oscillation_free)
        {
            // 只有既不乱跳，压力又低，才允许解锁
            g_IsPressureLocked = 0;
            pressure_over_count = 0;
        }
        // 否则保持锁定状态，直接返回，不再执行下面的超压计数逻辑
        return;
    }

    // --- 情况 B：当前未锁定，检查是否需要触发锁定 ---

    // 1. 优先检查震荡 (震荡优先级高于超压)
    if (oscillation_count >= OSCILLATION_CONFIRM_COUNT)
    {
        g_IsPressureLocked = 1;
        return; // 锁定后直接返回
    }

    // 2. 检查持续超压
    if (current_press >= g_Pressure_Lock_Threshold)
    {
        pressure_over_count++;
        if (pressure_over_count >= COUNT_LIMIT)
        {
            g_IsPressureLocked = 1;
            if (pressure_over_count > COUNT_LIMIT + 50) pressure_over_count = COUNT_LIMIT + 50;
        }
    }
    else
    {
        pressure_over_count = 0;
    }
}



// ============================================================
// 主控制函数
// ============================================================
void MoveControl()
{


    // 3. 补偿与速度计算
    Lcompensation_control();
    Rcompensation_control();
    speed_selection = 2.0 * (P_MK32->CH11_RD1 + 1000) / 200; // 旋钮设置移动速度；2是20/3是30
    GV.Robot_Move_Speed = speed_M_min_toE01_M_min(GetVehicleSpeed(speed_selection));
    IV.RobotMoveSpeed = GetVehicleSpeed(speed_selection);

    if(IV.Press>3500)
    {
    	 Mannual_TiltControl2();
    }
    else{
    	Mannual_TiltControl();
    }


    // 6. 运动逻辑
    DHRougheningControl();

    if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move)
    {
        Rough_RegionAutoMoveControl();
        return;
    }

    if (LaneChangeControl_Rough() != 0)
    {
        return;
    }

    if (P_MK32->CH5_SB == -1000)
    {
        robot_forwards();
        return;
    }
    if (P_MK32->CH5_SB == 1000)
    {
        robot_backwards();
        return;
    }



    joysticker_manual_control();
}








/* @brief: 电机出现报警返回1，电机正常返回0 */
uint8_t MotorErrorDetect()
{
	if (GV.LeftMotor.TT_Motor_Fault != 0 || GV.RightMotor.TT_Motor_Fault != 0)
	{
		return 1;
	}
	return 0;
}






/**
 * @brief       手动模式,自动巡航,前进，
 * @function    前进
 * @param       无
 * @retval      无
 */
void robot_forwards()
{

	/*   竖直向左 向右  */
	if (GV.PV.RunMode == Move_Vertical_Move_To_Left
	        || GV.PV.RunMode == Move_Vertical_Move_To_Right)
	{
		fsm_state_set(&current_robot_move_state,
		        &robot_move_vertical_task_forwards_state);/* 前进纠偏 */
		return;
	}
	if (GV.PV.RunMode == Move_Horizontal_Move)
	{
		if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
		        < Move_Horizontal_AngleThreshold_D * 100)
		{
			fsm_state_set(&current_robot_move_state,
			        &robot_move_horizontal_task_forwards_left_state);/* 水平朝左前进纠偏 */

		}
		else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
		        < Move_Horizontal_AngleThreshold_D * 100)
		{
			fsm_state_set(&current_robot_move_state,
			        &robot_move_horizontal_task_forwards_right_state);/* 水平朝右前进纠偏 */

		}
		else
		{
			fsm_state_set(&current_robot_move_state, &robot_halt_state);/* 角度小于5° 不动 */
		}
		return;
	}
	fsm_state_set(&current_robot_move_state, &robot_forwards_state);/* app 无 前进纠偏 */
	return;
}

/**
 * @brief       手动模式下自动巡航后退，
 * @function    自动后退： 竖直模式下
 * @param       无
 * @retval      无
 */
void robot_backwards()
{
	/*竖直*/
	if (GV.PV.RunMode == Move_Vertical_Move_To_Left
	        || GV.PV.RunMode == Move_Vertical_Move_To_Right)
	{

		fsm_state_set(&current_robot_move_state,
		        &robot_move_vertical_task_backwards_state);/* 后退纠偏 */
		return;
	}
	if (GV.PV.RunMode == Move_Horizontal_Move)
	{
		if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
		        < Move_Horizontal_AngleThreshold_D * 100)
		{
			fsm_state_set(&current_robot_move_state,
			        &robot_move_horizontal_task_backwards_left_state);/* 水平头朝左后退纠偏 */

		}
		else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
		        < Move_Horizontal_AngleThreshold_D * 100)
		{
			fsm_state_set(&current_robot_move_state,
			        &robot_move_horizontal_task_backwards_right_state);/* 水平朝右后退纠偏 */

		}
		else
		{
			fsm_state_set(&current_robot_move_state, &robot_halt_state);/* 角度大 不动 */
		}
		return;
	}

	fsm_state_set(&current_robot_move_state, &robot_backwards_state);/* 后退 */
	return;

}

void joysticker_manual_control()
{

	/*停止*/
	if (abs(P_MK32->CH2_LY_V) <= CV.Joy_Sticker_Value_Allowance
	        && abs(P_MK32->CH3_LY_H) <= CV.Joy_Sticker_Value_Allowance)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		return;
	}
	int angle = atan2(P_MK32->CH2_LY_V, P_MK32->CH3_LY_H) * 180 / M_PI;

	if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)
	{
		/*前进*/
		robot_forwards();  //根据app模式选择纠偏与不纠偏
		return;
	}
	/*后退*/
	if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance)
	{
		robot_backwards();
		return;
	}
	/*右转*/
	if (abs(angle - 0) <= CV.Joy_Sticker_Angle_Allowance)
	{
		fsm_state_set(&current_robot_move_state, &robot_turn_right_state); /*右转*/
		IV.CurrentSpeed = CV.LeftTurnSpeed / 10;
		return;
	}
	/*左转*/
	if (abs(angle - 180) <= CV.Joy_Sticker_Angle_Allowance
	        || abs(angle - (-180)) <= CV.Joy_Sticker_Angle_Allowance)
	{
		IV.CurrentSpeed = CV.LeftTurnSpeed / 10;
		fsm_state_set(&current_robot_move_state, &robot_turn_left_state); /*左转*/
		return;
	}

}





void Mannual_TiltControl()
{

	if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return;

	if (abs(P_MK32->CH1_RY_V) <= CV.Joy_Sticker_Value_Allowance
	        && abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance)/*停止*//*600*/
	{
		fsm_state_set(&current_tilt_state, &tilt_halt_state); /*停止推杆*/
		return;
	}
	int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI;

	if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance)
	{
		fsm_state_set(&current_tilt_state, &tilt_up_state);/*上升*/
		return;
	}
	// 原下压逻辑：无限位
	if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)/*45° 下降*/
	{
		if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet)
		{
			fsm_state_set(&current_tilt_state, &tilt_down_state);
			return;
		}
		fsm_state_set(&current_tilt_state, &tilt_halt_state);
	}

}


//void Mannual_TiltControl()
//{
//    if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return;
//
//    // 摇杆回中
//    if (abs(P_MK32->CH1_RY_V) <= CV.Joy_Sticker_Value_Allowance
//            && abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance)
//    {
//        fsm_state_set(&current_tilt_state, &tilt_halt_state); /*停止推杆*/
//        return;
//    }
//
//    int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI;
//
//    // 上升
//    if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance)
//    {
//        fsm_state_set(&current_tilt_state, &tilt_up_state);/*上升*/
//        return;
//    }
//
//    // 下降
//    if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)
//    {
//
//      fsm_state_set(&current_tilt_state, &tilt_down_state);
//
//
//    }
//}


volatile int g_AUTO_LIFT_RELEASE_PRESSURE = 1500; // 自动抬升的停止阈值，后续改成界面上压力设定值
// 压力过载下的推杆控制：自动模式下，一旦超出软限位值，
void Mannual_TiltControl2()
{
    // 基础模式检查
    if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return;

//    const int g_AUTO_LIFT_RELEASE_PRESSURE = 1500; // 自动抬升的停止阈值

    // 1. 自动抬升逻辑
    // 只要压力还大于停止阈值，就一直保持抬升状态
    if (IV.Press > GV.PV.PressSet)
    {
        fsm_state_set(&current_tilt_state, &tilt_up_state); /*强制上升*/
        return;
    }

    // 2. 停止逻辑
    // 当压力降到停止阈值以下，停止推杆
    fsm_state_set(&current_tilt_state, &tilt_halt_state); /*停止推杆*/
}


int Auto_TiltControl()
{
	if (0 == isAutoAdjustPress)
	{
		return 0;
	}
	TiltWorking_Mode = Tilt_Auto_Mode;
	/*存在一个问题，当显示大于设定值时需要上升，但是由于机械硬件无法上升，压力无法改变，会卡在推杆上升中，自动程序无法执行*/
	if (GV.Strain_Gauge.Pressure >= GV.PV.PressSet * 0.8 )
	{
		fsm_state_set(&current_tilt_state, &tilt_halt_state); /*关闭推杆*/
		/*正转开启拉毛盘*/
		if(GV.PV.ToolRotationDirection==1){fsm_state_set(&current_roughening_state, &roughening_swing_clockwise_state);}
		//反转
		if(GV.PV.ToolRotationDirection==2){fsm_state_set(&current_roughening_state, &roughening_swing_anticlockwise_state);}

		TiltWorking_Mode = Tilt_Manual_Mode;
		isAutoAdjustPress = 0;
		return 0;
	}
//	else if (GV.Strain_Gauge.Pressure >= GV.PV.PressSet * 0.8 + 50)
//	{
//		fsm_state_set(&current_tilt_state, &tilt_up_state); /*上升推杆*/
//		return 1;
//
//	}
	else if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet * 0.8)
	{
		fsm_state_set(&current_tilt_state, &tilt_down_state); /*下降推杆*/
		return 1;
	}

}

/*
 * 拉毛前端控制

 */
void DHRougheningControl()
{
	if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) /*app 自动水平模式 无法控制*/
	{
		return;
	}
	if (P_MK32->CH6_SC == -1000)	//SC上
	{
		if (isMannualAdjustPress == 1)
		{
			if (GV.Strain_Gauge.Pressure < GV.PV.PressSet)/*开启气动马达时先自动下降至设定压力*/
			{
				fsm_state_set(&current_tilt_state, &tilt_down_state);/*推杆下降*/
				return;
			}
		}
		/*isMannualAdjustPress为0*/
		if(GV.PV.ToolRotationDirection==1)
		{
			fsm_state_set(&current_roughening_state,
								&roughening_swing_clockwise_state); /*正转开启拉毛盘*/
			isMannualAdjustPress = 0;
			return;
		}
		if(GV.PV.ToolRotationDirection==2)
		{
			fsm_state_set(&current_roughening_state,
								&roughening_swing_anticlockwise_state); /*反转开启拉毛盘*/
			isMannualAdjustPress = 0;
			return;
		}

	}
	else //SC回中下
	{
		isMannualAdjustPress = 1;
		fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
	}



}






void IV_control()
{

	/*除10是因为天太电机读出来的转速是×10的*/
	IV.CurrentSpeed = GV.LeftMotor.Real_Velcity/10.0/CV.wheel_Reduction_Ratio*M_PI*CV.wheel_Diameter_m;

	IV.LeftCompensation = GV.Left_Compensation;
	IV.RightCompensation = GV.Right_Compensation;
	IV.CurrentAngle = GV.Robot_Angle;
	IV.Press = GV.Strain_Gauge.Pressure;
	IV.SystemError = GV.SystemErrorData.Com_Error_Code;
	IV.Left_Motor_Err = GV.LeftMotor.TT_Motor_Fault;
	IV.Right_Motor_Err = GV.RightMotor.TT_Motor_Fault;

	IV.Left_Motor_Temp = GV.LeftMotor.Tempature;
	IV.Right_Motor_Temp = GV.RightMotor.Tempature;
	IV.TimeStamp = SystemTimeMiliCount * 2;	//2ms一个计数，这里标记的是单片机运行时间
	IV.SBUS_State=GV.P_MK32.IsOnline;
	if (decoded_PV.RobotRestartAccepted == 1)
	{
		IV.RobotRestart = 0;
	}
}

/*
 * 暂存App传输的PV数据，用于在业务执行过程中进行数据隔离
 */
void PV_Data_Reading()
{
	if (P_MK32->CH4_SA == 0 && P_MK32->CH5_SB == 0 && P_MK32->CH7_SD == 0
	        && P_MK32->CH14_LT == 0 && P_MK32->CH15_RT == 0)
	{
		GV.PV = decoded_PV;
		if (isAdjustCompensation == 1)
		{
			GV.Left_Compensation = GV.PV.LeftCompensation;
			GV.Right_Compensation = GV.PV.RightCompensation;
			isAdjustCompensation = 0;
		}

	}




}
int AbnormalDetect()
{

	//上位机此时，其他设备全部处于不可操作状态，喷漆停止、摆臂上抬，只保持轮子的基本操作（为处理遥控器无法使用的情况）
	if (is_upper_computer_take_over_control == Taken_Over)
	{
		GV.PV.RunMode = Move_Manual;
		fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
		fsm_state_set(&current_motor_power_state, &motor_power_on_state); //电机上电，上电后，激活电机，
		return 1;
	}

  //SBUS出错
	if (Get_BIT(SystemErrorCode, ComError_MK32_SBus) == DISCONNECTED && Get_BIT(SystemErrorCode, ComError_Android_485) == DISCONNECTED)
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
		fsm_state_set(&current_motor_power_state, &motor_power_off_state); /*关闭电机电磁阀*/
		return 1;
	}

	/*急停*/
	if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000)
	{
		fsm_state_set(&current_motor_power_state, &motor_power_off_state); /*关闭电机电磁阀*/
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
		Is_All_Button_Has_Reset = 0; //急停之后必须复位才能动
		IV.CurrentSpeed = 0;
		return 1;
	}

	/* 按钮未复位 第一次上电  SystemErrorCode不为0 计算值为1 未复位 */
	if (Get_BIT(SystemErrorCode, ComError_Remote_Button_Reset_State) //按钮未复位  SystemErrorCode 右移ComError_Remote_Button_Reset_State位
	!= Has_Reset) //Has_Reset 0 复位 1未复位
	{
		fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
		fsm_state_set(&current_motor_power_state, &motor_power_off_state); /*关闭电机电磁阀*/
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		return 1;
	}


	//陀螺仪无信号
	if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED)
	{
//		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
		return 1;
	}
	// 预留：初次上电时，检测压力传感器值是否在零位附近如【-50，+50】
//	if (IV.Press >= -50 && IV.Press <= 50)
//	{
////		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
//		fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
//		return 1;
//	}



	if (P_MK32->IsOnline == 0)	//等于0时 subus有数，但是遥控器关机了，或者失联
	{
		fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
		fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
		Is_All_Button_Has_Reset = 0;	//遥控关机
		return 1;
	}
	return 0;
}