/*
 * fsm.c
 *状态机：通过遥控器的指令，判断机器人的运动状态。
 *  Created on: Oct 18, 2024
 *      Author: akeguo
 */

#include "fsm.h"
#include "BHBF_ROBOT.h"
#include "bsp_include.h"
#include "robot_state.h"

int32_t *RobotSpeed;
//int32_t JontSwingSpeed;
//int32_t JontTiltSpeed;
int32_t JontSwingSpeed = 400;
int32_t JontTiltSpeed = 400;
void action_perfrom(transition_t transitions[], int length, int state);
void GF_Dispatch();
void action_perfrom();
MoveSTATE_t CurrentMoveState;
SwingSTATE_t CurrentSwingState;
TiltSTATE_t CurrentTiltState; //点头，抬头
SetSTATE_t CurrentSetState; //设置
int index_counter = 0;
int Polish_Flag = 0; //打磨标志
int Polish_Count = 0; //打磨启停上升沿计数
int Polish_lock = 0;        // 打磨锁定标志（非自锁）
int Spray_Flag = 0;        //喷漆标志
int Spray_Count = 0;
int Spray_lock = 0;        // 喷漆打磨锁定标志（非自锁）
//int Vacuum_Flag = 0;
//int Vacuum_Count = 0;
int Vacuum_Delay_Count = 0;  // Polish和Vacuum之间的延迟计数器
int emergency_lock_count = 0;  // 急停：锁定计数器
int Operation_lock = 0;  // 通用锁定标志

int process_state = 0;       // 后台处理状态（0:空闲 1:开启中 2:关闭中）
void IV_control();
//手动下 机器人运动
transition_t MoveTransitions[] =
{

{ Move_Forwards, Forwards_State_Do },
{ Move_Backwards, Backwards_State_Do },
{ Move_TurnLeft, TurnLeft_State_Do },
{ Move_TurnRight, TurnRight_State_Do },
{ Move_HALT, HALT_State_Do },
{ Move_AutoForward, Auto },
{ Move_AutoBackward, Auto },
{ Move_ChgLeft, ChgLeft },
{ Move_ChgRight, ChgRight },
{ Move_ChgUp, ChgUp },
{ Move_ChgDown, ChgDown },
{ Move_ChgFinish, ChgFinish },
{ Move_Reset, Reset }, };

transition_t SwingTransitions[] =
{

{ SwingHALT, SwingHALT_State_Do },
{ SwingLeft, SwingLeft_State_Do },
{ SwingRight, SwingRight_State_Do },
{ SwingHome, SwingHome_Do }, };

transition_t SetTransitions[] =
{

{ OtherMode, OtherMode_State_Do },
{ SetLaneChangeDistance, LaneChangeDistance_Setting_State_Do },
{ SetBackwardsDistance, BackWardsDistance_Setting_State_Do },

};

transition_t TiltTransitions[] =
{

{ TiltHALT, TiltHalt_Do },
{ TiltUP, TiltUp_Do },
{ TiltDown, TiltDown_Do },
{ TiltHome, TiltHome_Do },
{ TiltCurrentModeDown, TiltCurrentModeDown_Do }, };

void Fsm_Init()
{
	IV.RobotRestart = 1; //机器人上电初始化，需要通知机器人
	GF_BSP_Interrupt_Add_CallBack(
			DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
}
//状态迁移

//double Y_Value_1;
//double X_Value_1;
//double Rocker_angle;

int JJ_Flag;

/*初始版本：存在问题，1是换道时误触摇杆会暂停；2是换道完成后误触摇杆会二次触发*/
//void Robot_Move()
//{
//	CurrentMoveState = Move_HALT;
//	// 第一个大条件：当不是所有按钮都处于默认状态（-1000）且摇杆在中心区域
//	if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
//			&& P_U7->M6 == -1000 && P_U7->M5 == -1000
//			&& P_U7->RU_Three == 0)
//			&& (P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
//			&& (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300))
//	{
//		//仅RU_Three按下，自动巡航判断
//		if(P_U7->RU_Three != 0
//				&& P_U7->M2 == -1000 && P_U7->M1 == -1000
//				&& P_U7->M6 == -1000 && P_U7->M5 == -1000)	//自动作业与换道互斥
//		{
//		   if(P_U7->RU_Three == 1000)
//		   {
//				CurrentMoveState = Move_AutoForward;
//				if(GV.AuTo_Flag == 0)
//				{
//					GV.AuTo_Flag=1;
//				}
//		   }
//		   else if(P_U7->RU_Three == -1000)
//		   {
//				CurrentMoveState = Move_AutoBackward;
//				if(GV.AuTo_Flag == 0)
//				{
//					GV.AuTo_Flag=3;
//				}
//		   }
//		}
//		// 换道的M1、2、5、6有一个被按下，换道判断
//		else if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
//				&& P_U7->M6 == -1000 && P_U7->M5 == -1000)
//				&& P_U7->RU_Three == 0)
//		{
//			if(GV.Chg_Flag == 1 && (P_U7->M1 == 1000 || P_U7->M2 == 1000
//					|| P_U7->M5 == 1000 || P_U7->M6 == 1000))
//			{
//				CurrentMoveState = Move_ChgFinish;
//			}
//			else if(P_U7->M1 == 1000
//					&& P_U7->M2 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000)
//			{
//				CurrentMoveState = Move_ChgUp;
//			}
//			else if(P_U7->M2 == 1000
//					&& P_U7->M1 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000)
//			{
//				CurrentMoveState = Move_ChgDown;
//			}
//			else if(P_U7->M5 == 1000
//					&& P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M6 == -1000)
//			{
//				CurrentMoveState = Move_ChgLeft;
//			}
//			else if(P_U7->M6 == 1000
//					&& P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M5 == -1000)
//			{
//				CurrentMoveState = Move_ChgRight;
//			}
//		}
//	}
//	//第二个大条件：当摇杆不在中心区域且所有按钮都是默认值（-1000）且RU_Three为0时，根据摇杆的方向设置前进、后退、左转、右转状态
//	else if( !((P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
//			&&(P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300))
//			&& P_U7->M2 == -1000 && P_U7->M1 == -1000
//			&& P_U7->M6 == -1000 && P_U7->M5 == -1000
//			&& P_U7->RU_Three == 0)
//	{
//		if (P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)//前进周围300范围内才继续检测前进是否按下
//		{
//			if (P_U7->CH0_LYB_V > 300)//前进
//			{
//				CurrentMoveState = Move_Forwards;
//			}
//			else if (P_U7->CH0_LYB_V < -300)
//			{
//				CurrentMoveState = Move_Backwards;  //后退
//			}
//			else
//			{
//				CurrentMoveState = Move_HALT;//停车
//			}
//		}
//		else if (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300)
//		{
//			if (P_U7->CH1_LYB_H > 300)
//			{
//				CurrentMoveState = Move_TurnRight;  //右转
//			}
//			else if (P_U7->CH1_LYB_H < -300)
//			{
//				CurrentMoveState = Move_TurnLeft;  //左转
//			}
//			else
//			{
//				CurrentMoveState = Move_HALT;  //停车
//			}
//		}
//	}
//	else
//	{
//		CurrentMoveState = Move_HALT;  //停车
//	}
//}

int ManualMode_Move();
int ManualMode_Move()
{
	if (P_U7->RU_Three == 1000)
	{
		CurrentMoveState = Move_AutoForward;
		if (GV.AuTo_Flag == 0)
		{
			GV.AuTo_Flag = 1;
		}
		return 1; // 表示已处理自动模式
	} else if (P_U7->RU_Three == -1000)
	{
		CurrentMoveState = Move_AutoBackward;
		if (GV.AuTo_Flag == 0)
		{
			GV.AuTo_Flag = 3;
		}
		return 1; // 表示已处理自动模式
	}
	return 0;
}

int LaneChangeControl();
int LaneChangeControl()
{
	if (GV.Chg_Flag == 1
			&& (P_U7->M1 == 1000 || P_U7->M2 == 1000 || P_U7->M5 == 1000
					|| P_U7->M6 == 1000))
	{
		CurrentMoveState = Move_ChgFinish;
		return 1; // 表示已处理换道完成
	} else if (P_U7->M1 == 1000 && P_U7->M2 == -1000 && P_U7->M6 == -1000
			&& P_U7->M5 == -1000)
	{
		CurrentMoveState = Move_ChgUp;
		return 1;
	} else if (P_U7->M2 == 1000 && P_U7->M1 == -1000 && P_U7->M6 == -1000
			&& P_U7->M5 == -1000)
	{
		CurrentMoveState = Move_ChgDown;
		return 1;
	} else if (P_U7->M5 == 1000 && P_U7->M1 == -1000 && P_U7->M2 == -1000
			&& P_U7->M6 == -1000)
	{
		CurrentMoveState = Move_ChgLeft;
		return 1;
	} else if (P_U7->M6 == 1000 && P_U7->M1 == -1000 && P_U7->M2 == -1000
			&& P_U7->M5 == -1000)
	{
		CurrentMoveState = Move_ChgRight;
		return 1;
	}
	return 0;
}

void Robot_Move()
{
	CurrentMoveState = Move_HALT;

	//换道优先级优于普通的运行;
	if (LaneChangeControl() != 0)
	{
		*AuTo_Flag = 0;
		return;
	}

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

	if (P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300) //前进周围300范围内才继续检测前进是否按下
	{
		if (P_U7->CH0_LYB_V > 300) //前进
		{
			CurrentMoveState = Move_Forwards;
		} else if (P_U7->CH0_LYB_V < -300)
		{
			CurrentMoveState = Move_Backwards;  //后退
		} else
		{
			CurrentMoveState = Move_HALT;  //停车
		}
	} else if (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300)
	{
		if (P_U7->CH1_LYB_H > 300)
		{
			CurrentMoveState = Move_TurnRight;  //右转
		} else if (P_U7->CH1_LYB_H < -300)
		{
			CurrentMoveState = Move_TurnLeft;  //左转
		} else
		{
			CurrentMoveState = Move_HALT;  //停车
		}
	}

}

/*增加互斥版本3：使用标志位法。理论上没有问题，但未经测试*/
//int auto_mode_active = 0;  // 自动模式激活标志
//int chg_mode_active = 0;   // 换道模式激活标志
//
//void Robot_Move()
//{
//    CurrentMoveState = Move_HALT;
//
//    // 模式互斥保护：如果已有模式激活，只处理该模式的输入
//    if(auto_mode_active) {
//        //自动
//        if(P_U7->RU_Three == 1000) {
//            CurrentMoveState = Move_AutoForward;
//            if(GV.AuTo_Flag == 0) GV.AuTo_Flag=1;
//        }
//        else if(P_U7->RU_Three == -1000) {
//            CurrentMoveState = Move_AutoBackward;
//            if(GV.AuTo_Flag == 0) GV.AuTo_Flag=3;
//        }
//        else {
//            // 自动模式信号消失，退出自动模式
//            auto_mode_active = 0;
//            GV.AuTo_Flag = 0;
//        }
//        return; // 自动模式下忽略其他所有输入
//    }
//    else if(chg_mode_active) {
//        //换道
//        if(GV.Chg_Flag == 1 && (P_U7->M1 == 1000 || P_U7->M2 == 1000
//                || P_U7->M5 == 1000 || P_U7->M6 == 1000)) {
//            CurrentMoveState = Move_ChgFinish;
//            chg_mode_active = 0; //换道完成，退出换道模式
//        }
//        else if(P_U7->M1 == 1000
//                && P_U7->M2 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000) {
//            CurrentMoveState = Move_ChgUp;
//        }
//        else if(P_U7->M2 == 1000
//                && P_U7->M1 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000) {
//            CurrentMoveState = Move_ChgDown;
//        }
//        else if(P_U7->M5 == 1000
//                && P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M6 == -1000) {
//            CurrentMoveState = Move_ChgLeft;
//        }
//        else if(P_U7->M6 == 1000
//                && P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M5 == -1000) {
//            CurrentMoveState = Move_ChgRight;
//        }
//        else {
//            // 所有换道按钮释放，退出换道模式
//            chg_mode_active = 0;
//        }
//        return; // 换道模式下忽略其他所有输入
//    }
//
//    // 原代码逻辑，但添加模式激活设置
//    if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
//            && P_U7->M6 == -1000 && P_U7->M5 == -1000
//            && P_U7->RU_Three == 0)
//            && (P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
//            && (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300))
//    {
//        if(P_U7->RU_Three != 0
//                && P_U7->M2 == -1000 && P_U7->M1 == -1000
//                && P_U7->M6 == -1000 && P_U7->M5 == -1000)
//        {
//            // 设置自动模式激活标志
//            auto_mode_active = 1;
//
//            if(P_U7->RU_Three == 1000)
//            {
//                CurrentMoveState = Move_AutoForward;
//                if(GV.AuTo_Flag == 0)
//                {
//                    GV.AuTo_Flag=1;
//                }
//            }
//            else if(P_U7->RU_Three == -1000)
//            {
//                CurrentMoveState = Move_AutoBackward;
//                if(GV.AuTo_Flag == 0)
//                {
//                    GV.AuTo_Flag=3;
//                }
//            }
//        }
//        else if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
//                && P_U7->M6 == -1000 && P_U7->M5 == -1000)
//                && P_U7->RU_Three == 0)
//        {
//            // 设置换道模式激活标志
//            chg_mode_active = 1;
//
//            if(GV.Chg_Flag == 1 && (P_U7->M1 == 1000 || P_U7->M2 == 1000
//                    || P_U7->M5 == 1000 || P_U7->M6 == 1000))
//            {
//                CurrentMoveState = Move_ChgFinish;
//            }
//            else if(P_U7->M1 == 1000
//                    && P_U7->M2 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000)
//            {
//                CurrentMoveState = Move_ChgUp;
//            }
//            else if(P_U7->M2 == 1000
//                    && P_U7->M1 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000)
//            {
//                CurrentMoveState = Move_ChgDown;
//            }
//            else if(P_U7->M5 == 1000
//                    && P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M6 == -1000)
//            {
//                CurrentMoveState = Move_ChgLeft;
//            }
//            else if(P_U7->M6 == 1000
//                    && P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M5 == -1000)
//            {
//                CurrentMoveState = Move_ChgRight;
//            }
//        }
//    }
//    else if( !((P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
//            &&(P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300))
//            && P_U7->M2 == -1000 && P_U7->M1 == -1000
//            && P_U7->M6 == -1000 && P_U7->M5 == -1000
//            && P_U7->RU_Three == 0)
//    {
//        // 手动
//        if (P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
//        {
//            if (P_U7->CH0_LYB_V > 300)
//            {
//                CurrentMoveState = Move_Forwards;
//            }
//            else if (P_U7->CH0_LYB_V < -300)
//            {
//                CurrentMoveState = Move_Backwards;
//            }
//            else
//            {
//                CurrentMoveState = Move_HALT;
//            }
//        }
//        else if (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300)
//        {
//            if (P_U7->CH1_LYB_H > 300)
//            {
//                CurrentMoveState = Move_TurnRight;
//            }
//            else if (P_U7->CH1_LYB_H < -300)
//            {
//                CurrentMoveState = Move_TurnLeft;
//            }
//            else
//            {
//                CurrentMoveState = Move_HALT;
//            }
//        }
//    }
//    else
//    {
//        CurrentMoveState = Move_HALT;
//    }
//}
//

void Front_End()
{
	//	 版本二：LU_Single 控制打磨电机和吸尘器（非自锁版本）
	if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0)
	{
		Polish_lock = 1;  // 立即锁定，防止重复触发

		if (Polish_Flag == 0)
		{
			process_state = 1;  // 准备开俩设备
			Vacuum_Delay_Count = 0;            // 开吸尘器前，提前重置上一轮计数
		} else
		{
			process_state = 2;  // 准备关闭俩设备
			Polish_Count = 100; // 从100开始倒计时
		}
	} else if (P_U7->LU_Single == -1000)
	{
		Polish_lock = 0;
	}

	if (process_state == 1)
	{
		GF_BSP_GPIO_SetIO(Out_Vacuum, 0);
		if (Vacuum_Delay_Count < 200)
		{
			Vacuum_Delay_Count++;
		} else
		{
			GF_BSP_GPIO_SetIO(Out_Polish, 0);

			if (Polish_Count >= 100)
			{
				GF_BSP_GPIO_SetIO(Out_Polish, 1);
				Polish_Flag = 1;
				process_state = 0;  // 跑完耗时操作就退回初始状态
				Polish_Count = 0;   // 重置计数
			} else
			{
				Polish_Count++;
			}
		}
	}

	else if (process_state == 2)
	{
		GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
		GF_BSP_GPIO_SetIO(Out_Polish, 0);

		if (Polish_Count <= 0)
		{
			GF_BSP_GPIO_SetIO(Out_Polish, 1);
			Polish_Flag = 0;
			//		        GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
			process_state = 0;  // 跑完退回
		} else
		{
			Polish_Count--;
		}
	}

	//LU_Three控制打磨推杆升降
	if (P_U7->LU_Three == 1000)
	{
		GF_BSP_GPIO_SetIO(Out_Lift_2, 0);
		GF_BSP_GPIO_SetIO(Out_Lift_1, 1);
	} else if (P_U7->LU_Three == -1000
			&& GV.Robot_ForceValue < GV.PV.Robot_Force && P_U7->RU_Three == 0
			&& P_U7->CH0_LYB_V < 100 && P_U7->CH1_LYB_H < 100)
	{
		GF_BSP_GPIO_SetIO(Out_Lift_2, 1);
		GF_BSP_GPIO_SetIO(Out_Lift_1, 0);
	} else
	{
		GF_BSP_GPIO_SetIO(Out_Lift_2, 1);
		GF_BSP_GPIO_SetIO(Out_Lift_1, 1);
	}

	// 控制电磁阀喷枪（非自锁版本）
	// 功能：按一下开启，再按一下关闭，长按不会反复切换
	// Flag管当前喷枪状态、Lock管长短按问题
	if (P_U7->RU_Single == 1000)
	{
		if (Spray_lock == 0) // 未锁定时才处理
		{
			if (Spray_Flag == 0) // 当前关闭，切换为开启
			{
				GF_BSP_GPIO_SetIO(Out_Spray, 0);
				Spray_Flag = 1;
				Spray_lock = 1; // 上锁。一旦按下实现状态切换就锁定。
			} else                 // 当前开启，切换为关闭
			{
				GF_BSP_GPIO_SetIO(Out_Spray, 1);
				Spray_Flag = 0;
				Spray_lock = 1; // 上锁关闭后锁定，防止长按再次触发
			}
		}
	} else
	{
		// 松开时复位锁定，允许下次按键触发
		Spray_lock = 0;
	}

}
void Emergency()
{
	/*急停版本2：手动复位按键版
	 * 急停：同时按下S3、S4则触发急停；
	 * 退出急停：极短的一段时间后，按下S3、S4任意键则解除
	 *
	 * 急停一旦触发即锁定，并关闭全部设备；退出急停后，需手动复位所有被按下按键才能恢复控制
	 *
	 * 优先级：
	 * 急停且自动锁定、急停退出但且自动锁定、急停退出且解除自动锁定
	 * */

	if (P_U7->S3 == 1000 && P_U7->S4 == 1000)
	{
		GV.Emergency = 1;
		Operation_lock = 1;  // 急停时自动锁定
		Reset_Flag = 0;
	}

	// 2急停锁定计时（仅在急停状态下计数，解决松手不同时问题）
	if (GV.Emergency == 1)
	{
		if (emergency_lock_count < 500)
		{
			emergency_lock_count++;
		}
	} else
	{
		emergency_lock_count = 0;  // 非急停状态重置
	}

	if (GV.Emergency == 1 && emergency_lock_count >= 500)
	{
		if ((P_U7->S3 == 1000 && P_U7->S4 == -1000)
				|| (P_U7->S3 == -1000 && P_U7->S4 == 1000))
		{
			GV.Emergency = 0;
			emergency_lock_count = 0;
			// 退出后保持锁定（Operation_lock仍为1），需手动解锁
		}
	}

	// 解锁条件：按键全部复位：即，三键的推杆回中（0）且两键恢复默认状态（-1000）
	// 仅在已退出急停（GV.Emergency==0）但仍锁定时生效
	if (GV.Emergency == 0 && Operation_lock)
	{
//	    if (P_U7->LU_Three == 0 && P_U7->LU_Single == -1000 && P_U7->RU_Single == -1000)
		if (P_U7->LU_Three == 0)
		{
			Operation_lock = 0;  // 满足条件，解锁
			Reset_Flag = 0;
		}
		GF_BSP_GPIO_SetIO(0, 0);
	}

	// 急停
	if (GV.Emergency == 1 || (GV.Emergency == 0 && Operation_lock == 1))
	{
		Polish_Flag = 0;
		Spray_Flag = 0;
		GF_BSP_GPIO_SetIO(Out_Spray, 1);
		GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
		GF_BSP_GPIO_SetIO(Out_Lift_2, 1);
		GF_BSP_GPIO_SetIO(Out_Lift_1, 1);
		GF_BSP_GPIO_SetIO(0, 1);
		CurrentMoveState = Move_Emergency;
	}

}

void GF_Dispatch()  //2ms调用一次 给车体速度等赋值
{
	// 如果处于复位模式，直接停车，不再执行运动逻辑
	if (system_mode == SYSTEM_RESET)
	{
		action_perfrom(MoveTransitions,
				sizeof(MoveTransitions) / sizeof(transition_t),
				CurrentMoveState);  //结构体数组/大小/当前状态
		return;
	}
	//运动部分 Move Region
	//左补偿、右补偿
	Lcompensation_control();
	Rcompensation_control();
	Robot_Move();
	Front_End();
	Emergency();
	/****************************************/
	//Robot Speed 指向的是GV.movespeed
	GV.Robot_ManualSpeed = abs(
			(GV.PV.Robot_ManualSpeedBase * 186.9 * (P_U7->CH0_LYB_V)) / 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
	CV.TurnLeftRightSpeed = abs((2 * 186.9 * (P_U7->CH1_LYB_H)) / 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的

	GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase * 186.9;
	// IV_control();
	action_perfrom(MoveTransitions,
			sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态

}

void IV_control()
{
	IV.Robot_AngleRoll = GV.Robot_Angle.RF_Angle_Roll;
	IV.Robot_ForceValue = GV.Robot_ForceValue;
	IV.Robot_DynamometerValue = GV.Robot_DynamometerValue;
	IV.Robot_Current_Left = GV.LeftFrontMotor.Current;
	IV.Robot_Current_Right = GV.RightFrontMotor.Current;

	IV.Robot_Compensation_Left = GV.Left_Compensation;
	IV.Robot_Compensation_Right = GV.Right_Compensation;

	IV.Robot_Error_Left = GV.LeftFrontMotor.ERROR_Flag;
	IV.Robot_Error_Right = GV.RightFrontMotor.ERROR_Flag;
	IV.Robot_CurrentState = CurrentMoveState;

	IV.TimeStamp = SystemTimeMiliCount * 2;	//2ms一个计数，这里标记的是单片机运行时间

	if (decoded_PV.RobotRestartAccepted == 1)
	{
		IV.RobotRestart = 0;
	}
	IV.RobotAngle = GV.Robot_Angle.RF_Angle_Pitch;

}

void action_perfrom(transition_t transitions[], int length, int state)
{

	for (index_counter = 0; index_counter < length; index_counter++)
	{
		if (transitions[index_counter].State == state)
		{
			if (transitions[index_counter].robotRun != NULL)	//函数不为空
			{
				transitions[index_counter].robotRun();
			}

			break;
			//return;
		}
	}
}