You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
573 lines
15 KiB
573 lines
15 KiB
/*
|
|
* 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 AutoSpray_Stop_Count = 0; //自动喷漆结束计数
|
|
int AutoSpray_Flag=0;//喷枪标志
|
|
|
|
int USValue_Flag1 = 0; //超声波探边计数
|
|
int USValue_Flag2 = 0;
|
|
//手动下 机器人运动
|
|
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_Emergency, HALT_State_Do},
|
|
};
|
|
|
|
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()
|
|
{
|
|
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;
|
|
int Speed_Judge(T_F4_Value* T_F4)
|
|
{
|
|
T_F4_Value* m = T_F4;
|
|
if (m->Speed_Level_1 == 1)
|
|
return 1;
|
|
else if (m->Speed_Level_2 == 1)
|
|
return 3;
|
|
else if (m->Speed_Level_3 == 1)
|
|
return 5;
|
|
else if (m->Speed_Level_4 == 1)
|
|
return 8;
|
|
else if (m->Speed_Level_5 == 1)
|
|
return 12;
|
|
else if (m->Speed_Level_6 == 1)
|
|
return 14;
|
|
else if (m->Speed_Level_7 == 1)
|
|
return 16;
|
|
else if (m->Speed_Level_8 == 1)
|
|
return 18;
|
|
else if (m->Speed_Level_9 == 1)
|
|
return 20;
|
|
else if (m->Speed_Level_10 == 1)
|
|
return 22;
|
|
}
|
|
//--喷漆自动开启
|
|
void AutoForSpray_On()
|
|
{
|
|
CurrentMoveState = Move_AutoForward;
|
|
if(GV.AuTo_Flag == 0)
|
|
{
|
|
GV.AuTo_Flag=1;
|
|
}
|
|
|
|
if(AutoSpray_Flag == 0
|
|
&& Current_T_F4_Value.Spray_On
|
|
&& GV.LeftFrontMotor.Velcity >= GV.Robot_AutoSpeed - 100
|
|
&& GV.RightFrontMotor.Velcity <= -GV.Robot_AutoSpeed + 100)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Spray,0);
|
|
AutoSpray_Flag = 1;
|
|
}
|
|
}
|
|
void AutoBackSpray_On()
|
|
{
|
|
CurrentMoveState = Move_AutoBackward;
|
|
if(GV.AuTo_Flag == 0)
|
|
{
|
|
GV.AuTo_Flag=3;
|
|
}
|
|
|
|
if(AutoSpray_Flag == 0
|
|
&& Current_T_F4_Value.Spray_On
|
|
&& GV.RightFrontMotor.Velcity >= GV.Robot_AutoSpeed - 100
|
|
&& GV.LeftFrontMotor.Velcity <= -GV.Robot_AutoSpeed + 100)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Spray,0);
|
|
AutoSpray_Flag = 1;
|
|
}
|
|
}
|
|
//--
|
|
//--车体停
|
|
void Off_AutoSpray()
|
|
{
|
|
//喷漆自动关闭
|
|
if(AutoSpray_Flag == 1)
|
|
{
|
|
|
|
GF_BSP_GPIO_SetIO(Out_Spray,1);
|
|
AutoSpray_Stop_Count++;
|
|
}
|
|
//喷漆程序结束
|
|
|
|
//若自动喷漆启动，计数AutoSpray_Stop_Count*2ms后再停车
|
|
if(AutoSpray_Flag == 1)
|
|
{
|
|
if(AutoSpray_Stop_Count <= 300)
|
|
{
|
|
AutoSpray_Flag = 0;
|
|
AutoSpray_Stop_Count = 0;
|
|
CurrentMoveState = Move_HALT; //停车
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT; //停车
|
|
}
|
|
|
|
//停车，禁止其它置位
|
|
if(Current_T_F4_Value.AutoForWard == 1
|
|
|| Current_T_F4_Value.AutoBackWard == 1)
|
|
{
|
|
GV.AuTo_Flag = 99;
|
|
}
|
|
else
|
|
{
|
|
GV.AuTo_Flag = 0;
|
|
}
|
|
|
|
if(Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
|
|
|| Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1)
|
|
{
|
|
GV.Chg_Flag = 1;
|
|
Chg_Pa.change_road_finish_flag = 1;
|
|
|
|
}
|
|
else
|
|
{
|
|
GV.Chg_Flag = 0;
|
|
Chg_Pa.change_road_finish_flag = 0;
|
|
}
|
|
|
|
}
|
|
//--
|
|
//--探边检测
|
|
void Dangerous_Test()
|
|
{
|
|
//计数，置位标志
|
|
//前进探边计数
|
|
if(GV.Robot_Ultrasonic.USValue_1 >= 5000 || GV.Robot_Ultrasonic.USValue_1 == 0)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_1 ++;
|
|
if(GV.Robot_Ultrasonic.USValue_Count_1 >= 1)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_1 = 1;
|
|
GV.Robot_Ultrasonic.USValue_Flag_1 = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_1 = 0;
|
|
}
|
|
//后退探边计数
|
|
if(GV.Robot_Ultrasonic.USValue_2 >= 3000 || GV.Robot_Ultrasonic.USValue_2 == 0)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_2 ++;
|
|
if(GV.Robot_Ultrasonic.USValue_Count_2 >= 1)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_2 = 1;
|
|
GV.Robot_Ultrasonic.USValue_Flag_2 = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_2 = 0;
|
|
}
|
|
|
|
//前进避障计数
|
|
if(GV.Robot_Ultrasonic.USValue_4 <= 5000
|
|
&& GV.Robot_Ultrasonic.USValue_4 != 0)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_4 ++;
|
|
if(GV.Robot_Ultrasonic.USValue_Count_4 >= 1)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_4 = 1;
|
|
GV.Robot_Ultrasonic.USValue_Flag_4 = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_4 = 0;
|
|
}
|
|
|
|
//后退避障计数
|
|
if(GV.Robot_Ultrasonic.USValue_3 <= 4000
|
|
&& GV.Robot_Ultrasonic.USValue_3 != 0)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_3 ++;
|
|
if(GV.Robot_Ultrasonic.USValue_Count_3 >= 1)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_3 = 1;
|
|
GV.Robot_Ultrasonic.USValue_Flag_3 = 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Count_3 = 0;
|
|
}
|
|
|
|
//根据置位标志，置零遥控器
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_1 >= 1
|
|
|| GV.Robot_Ultrasonic.USValue_Flag_4 >= 1)
|
|
{
|
|
Current_T_F4_Value.AutoForWard = 0;
|
|
}
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_2 >= 1
|
|
|| GV.Robot_Ultrasonic.USValue_Flag_3 >= 1)
|
|
{
|
|
Current_T_F4_Value.AutoBackWard = 0;
|
|
}
|
|
|
|
}
|
|
//--
|
|
//--气压检测
|
|
void Dym_Test()
|
|
{
|
|
if(Current_T_F4_Value.Test_On == 1)
|
|
{
|
|
//计数，置位标志
|
|
//前进探边计数
|
|
if(GV.Robot_Ultrasonic.Dym_Value <= 10300 || GV.Robot_Ultrasonic.Dym_Value >= 11000)
|
|
{
|
|
GV.Robot_Ultrasonic.Dym_Value_Count ++;
|
|
if(GV.Robot_Ultrasonic.Dym_Value_Count >= 3)
|
|
{
|
|
GV.Robot_Ultrasonic.Dym_Value_Count = 3;
|
|
GV.Robot_Ultrasonic.Dym_Value_Flag = 1;
|
|
GF_BSP_GPIO_SetIO(Light, 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
GV.Robot_Ultrasonic.Dym_Value_Count = 0;
|
|
GF_BSP_GPIO_SetIO(Light, 1);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
GV.Robot_Ultrasonic.Dym_Value_Count = 0;
|
|
GF_BSP_GPIO_SetIO(Light, 1);
|
|
}
|
|
|
|
//报警后，等自动程序都复位才能继续
|
|
if(GV.Robot_Ultrasonic.Dym_Value_Count < 3
|
|
&& Current_T_F4_Value.AutoForWard == 0
|
|
&& Current_T_F4_Value.AutoBackWard == 0
|
|
&& Current_T_F4_Value.ChgDown_Ver == 0
|
|
&& Current_T_F4_Value.ChgUp_Ver == 0
|
|
&& Current_T_F4_Value.ChgLeft_Hor == 0
|
|
&& Current_T_F4_Value.ChgRight_Hor == 0)
|
|
{
|
|
GV.Robot_Ultrasonic.Dym_Value_Flag = 0;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
//--探边置位消除
|
|
void Dangerous_Flag_Delete()
|
|
{
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_1 >= 1
|
|
&& (CurrentMoveState == Move_Backwards
|
|
|| CurrentMoveState == Move_AutoBackward))
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Flag_1 ++;
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_1 >= 5)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Flag_1 = 0;
|
|
}
|
|
}
|
|
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_2 >= 1
|
|
&& (CurrentMoveState == Move_Forwards
|
|
|| CurrentMoveState == Move_AutoForward))
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Flag_2 ++;
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_2 >= 5)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Flag_2 = 0;
|
|
}
|
|
}
|
|
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_3 >= 1
|
|
&& (CurrentMoveState == Move_Forwards
|
|
|| CurrentMoveState == Move_AutoForward))
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Flag_3 ++;
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_3 >= 5)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Flag_3 = 0;
|
|
}
|
|
}
|
|
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_4 >= 1
|
|
&& (CurrentMoveState == Move_Backwards
|
|
|| CurrentMoveState == Move_AutoBackward))
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Flag_4 ++;
|
|
if(GV.Robot_Ultrasonic.USValue_Flag_4 >= 5)
|
|
{
|
|
GV.Robot_Ultrasonic.USValue_Flag_4 = 0;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
//--遥控器急停判断
|
|
void RC_Emergency()
|
|
{
|
|
if(Current_T_F4_Value.emergency_stop == 1)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Spray,1);
|
|
GF_BSP_GPIO_SetIO(0,1);
|
|
CurrentMoveState = Move_Emergency;
|
|
}
|
|
else
|
|
{
|
|
GF_BSP_GPIO_SetIO(0,0);
|
|
}
|
|
}
|
|
|
|
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
|
|
{
|
|
//--遥控器急停判断
|
|
RC_Emergency();
|
|
//--自动作业模式下的探边检测判断
|
|
if(Current_T_F4_Value.AutoForWard == 1
|
|
|| Current_T_F4_Value.AutoBackWard == 1)
|
|
{
|
|
Dangerous_Test();
|
|
}
|
|
//--
|
|
//--气压判断
|
|
Dym_Test();
|
|
//--
|
|
//运动部分 Move Region
|
|
if((Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
|
|
|| Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1
|
|
|| Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
|
|
&& (Current_T_F4_Value.robot_forward_backward_speed >= 0X7A
|
|
&& Current_T_F4_Value.robot_forward_backward_speed <= 0X84)
|
|
&& (Current_T_F4_Value.robot_left_right_turn_speed >= 0X7A
|
|
&& Current_T_F4_Value.robot_left_right_turn_speed <= 0X84)
|
|
&& GV.Robot_Ultrasonic.Dym_Value_Flag != 1)
|
|
{
|
|
if((Current_T_F4_Value.AutoForWard == 1 || Current_T_F4_Value.AutoBackWard == 1)
|
|
&&(Current_T_F4_Value.ChgDown_Ver != 1 && Current_T_F4_Value.ChgUp_Ver != 1
|
|
&& Current_T_F4_Value.ChgLeft_Hor != 1 && Current_T_F4_Value.ChgRight_Hor != 1))//自动作业与换道互斥
|
|
{
|
|
if(Current_T_F4_Value.AutoForWard == 1)
|
|
{
|
|
//如启停喷枪按下后判断前进速度到达后，喷漆自动开启
|
|
AutoForSpray_On();
|
|
//喷漆程序结束
|
|
}
|
|
else if(Current_T_F4_Value.AutoBackWard == 1)
|
|
{
|
|
//如启停喷枪按下后判断前进速度到达后，喷漆自动开启
|
|
AutoBackSpray_On();
|
|
//喷漆程序结束
|
|
}
|
|
}
|
|
else if((Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
|
|
&&(Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
|
|
|| Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1))
|
|
{
|
|
if(GV.Chg_Flag == 1 && (Current_T_F4_Value.ChgDown_Ver == 1 || Current_T_F4_Value.ChgUp_Ver == 1
|
|
&& Current_T_F4_Value.ChgLeft_Hor == 1 || Current_T_F4_Value.ChgRight_Hor == 1))
|
|
{
|
|
CurrentMoveState = Move_ChgFinish;
|
|
}
|
|
else if(Current_T_F4_Value.ChgUp_Ver == 1
|
|
&& !(Current_T_F4_Value.ChgDown_Ver == 1
|
|
|| Current_T_F4_Value.ChgLeft_Hor == 1
|
|
|| Current_T_F4_Value.ChgRight_Hor == 1))
|
|
{
|
|
CurrentMoveState = Move_ChgUp;
|
|
}
|
|
else if(Current_T_F4_Value.ChgDown_Ver == 1
|
|
&& !(Current_T_F4_Value.ChgUp_Ver == 1
|
|
|| Current_T_F4_Value.ChgLeft_Hor == 1
|
|
|| Current_T_F4_Value.ChgRight_Hor == 1))
|
|
{
|
|
CurrentMoveState = Move_ChgDown;
|
|
}
|
|
else if(Current_T_F4_Value.ChgLeft_Hor == 1
|
|
&& !(Current_T_F4_Value.ChgUp_Ver == 1
|
|
|| Current_T_F4_Value.ChgDown_Ver == 1
|
|
|| Current_T_F4_Value.ChgRight_Hor == 1))
|
|
{
|
|
CurrentMoveState = Move_ChgLeft;
|
|
}
|
|
else if(Current_T_F4_Value.ChgRight_Hor == 1
|
|
&& !(Current_T_F4_Value.ChgUp_Ver == 1
|
|
|| Current_T_F4_Value.ChgDown_Ver == 1
|
|
|| Current_T_F4_Value.ChgLeft_Hor == 1))
|
|
{
|
|
CurrentMoveState = Move_ChgRight;
|
|
}
|
|
}
|
|
}
|
|
else if((Current_T_F4_Value.ChgDown_Ver != 1 && Current_T_F4_Value.ChgUp_Ver != 1
|
|
&& Current_T_F4_Value.ChgLeft_Hor!= 1 && Current_T_F4_Value.ChgRight_Hor != 1
|
|
&& Current_T_F4_Value.AutoForWard != 1 && Current_T_F4_Value.AutoBackWard != 1)
|
|
&& (Current_T_F4_Value.robot_forward_backward_speed < 0X7A //7A 84
|
|
|| Current_T_F4_Value.robot_forward_backward_speed > 0X84
|
|
|| Current_T_F4_Value.robot_left_right_turn_speed < 0X7A
|
|
|| Current_T_F4_Value.robot_left_right_turn_speed > 0X84))
|
|
{
|
|
if (Current_T_F4_Value.robot_forward_backward_speed < 0X7A)//前进
|
|
{
|
|
CurrentMoveState = Move_Forwards;
|
|
}
|
|
else if (Current_T_F4_Value.robot_forward_backward_speed > 0X84)
|
|
{
|
|
CurrentMoveState = Move_Backwards; //后退
|
|
}
|
|
|
|
else if (Current_T_F4_Value.robot_left_right_turn_speed < 0X7A)
|
|
{
|
|
CurrentMoveState = Move_TurnLeft; //右转
|
|
}
|
|
else if (Current_T_F4_Value.robot_left_right_turn_speed > 0X84)
|
|
{
|
|
CurrentMoveState = Move_TurnRight; //左转
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Off_AutoSpray();
|
|
}
|
|
|
|
//--探边置位消除
|
|
Dangerous_Flag_Delete();
|
|
//--
|
|
|
|
/****************************************/
|
|
//LU_Three控制升降
|
|
if(Current_T_F4_Value.Lift_Up == 1)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Lift_2,0);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
|
|
}
|
|
else if(Current_T_F4_Value.Lift_Down == 1)
|
|
{
|
|
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);
|
|
}
|
|
|
|
//控制电磁阀喷枪
|
|
if(Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward&& CurrentMoveState != Move_AutoBackward)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Spray,0);
|
|
}
|
|
else if(!Current_T_F4_Value.FA && AutoSpray_Flag == 0) // && CurrentMoveState != Move_AutoForward && CurrentMoveState != Move_AutoBackward)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Spray,1);
|
|
}
|
|
|
|
|
|
|
|
|
|
//计算速度档位m/min
|
|
GV.Robot_Speed_mpm = Speed_Judge(&Current_T_F4_Value);
|
|
//代入速度m/min * 186.9 最大下发4725 即最大速度25m/min
|
|
GV.Robot_ManualSpeed = abs(GV.Robot_Speed_mpm * 186.9 * ((Current_T_F4_Value.robot_forward_backward_speed - 127)/ 127.0));//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
|
|
CV.TurnLeftRightSpeed = abs(2 * 186.9 * (Current_T_F4_Value.robot_left_right_turn_speed - 127 )/ 127.0);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
|
|
|
|
GV.Robot_AutoSpeed = (GV.Robot_Speed_mpm)* 186.9;
|
|
|
|
GV.Left_Compensation = Current_T_F4_Value.robot_Compensation_Left / 51.0;
|
|
GV.Right_Compensation = Current_T_F4_Value.robot_Compensation_Right / 51.0;
|
|
|
|
action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态
|
|
// action_perfrom(SwingTransitions,sizeof(SwingTransitions) / sizeof(transition_t), CurrentSwingState);
|
|
// action_perfrom(TiltTransitions, sizeof(TiltTransitions) / sizeof(transition_t), CurrentTiltState);
|
|
|
|
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
|