YuLin_Zhen
/
FW2.5
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

新增程序初始化后按键是否复位检测

FW2.5
LIN\54376 1 month ago
parent
e1ee32b603
commit
4d259f5c7b
5 changed files with 256 additions and 211 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      BASE/Inc/MSP/msp_DH_Roughening.h
  2. 14
      BASE/Src/MSP/msp_DH_Roughening.c
  3. 9
      Core/Protobuf/PSource/bsp_GV.pb.h
  4. 3
      Core/Protobuf/Proto/bsp_GV.proto
  5. 440
      Core/Src/FSM.c

1
BASE/Inc/MSP/msp_DH_Roughening.h
View File

@ -186,6 +186,7 @@ typedef struct _DHRoughening
uint16_t LaneChange_Vertical_Up :1;//竖直上换道
uint16_t LaneChange_Vertical_Down :1;//竖直下换道
uint16_t Wireless_FirstFlag;
}DHRoughening;

14
BASE/Src/MSP/msp_DH_Roughening.c
View File

@ -44,7 +44,9 @@ void read_DHRoughening_Data(void) {
DHRoughening_Controller_UART_Handler);
}
uint8_t data5555[20];
uint8_t data5555[2000];
uint8_t Wireless_LastState;
void decode_DHRoughening(uint8_t *buffer, uint16_t length) {
memcpy(data5555,buffer,length);
@ -52,10 +54,18 @@ void decode_DHRoughening(uint8_t *buffer, uint16_t length) {
/* CRC 校验正确 */
if (crc_check == MB_CRC16(buffer, length - 2))
{
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
"DHRoughening", 1);
memcpy(&DHRougheningContrller,&buffer[3],12);//16个字节数字
if(Wireless_LastState = 0
&& DHRougheningContrller.Wireless_State == 1)
{
DHRougheningContrller.Wireless_FirstFlag = 1;
}
Wireless_LastState = DHRougheningContrller.Wireless_State;
// *Roughenging_LeftCompensation=(*Roughenging_LeftCompensation_Range)*DHRougheningContrller.Left_Compensation*100/65535;
// *Roughenging_RightCompensation=(*Roughenging_RightCompensation_Range)*DHRougheningContrller.Right_Compensation*100/65535;
} else
@ -72,8 +82,6 @@ void send_to_DHRoughening()
(uint16_t) abs(GV.Robot_Angle.RF_Angle_Roll),
(uint16_t) abs(GV.Robot_ForceValue ));
}
//左补偿 xxx.xx 度 0x0000—0xFFFF==0—655.35 度

9
Core/Protobuf/PSource/bsp_GV.pb.h
View File

@ -42,6 +42,7 @@ typedef struct _GV_struct_define {
int32_t Robot_ForceValue;
int32_t Robot_DynamometerValue;
double Robot_Speed_Mpm;
int32_t Robot_Move_Step;
int32_t Emergency;
} GV_struct_define;
@ -51,8 +52,8 @@ extern "C" {
#endif
/* Initializer values for message structs */
#define GV_struct_define_init_default {false, PV_struct_define_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, SP_MSP_U7_Button_init_default, 0, false, ErrorData_init_default, false, MSP_TL720DParameters_init_default, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define GV_struct_define_init_zero {false, PV_struct_define_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, SP_MSP_U7_Button_init_zero, 0, false, ErrorData_init_zero, false, MSP_TL720DParameters_init_zero, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define GV_struct_define_init_default {false, PV_struct_define_init_default, false, MotorParameters_init_default, false, MotorParameters_init_default, false, SP_MSP_U7_Button_init_default, 0, false, ErrorData_init_default, false, MSP_TL720DParameters_init_default, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define GV_struct_define_init_zero {false, PV_struct_define_init_zero, false, MotorParameters_init_zero, false, MotorParameters_init_zero, false, SP_MSP_U7_Button_init_zero, 0, false, ErrorData_init_zero, false, MSP_TL720DParameters_init_zero, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
/* Field tags (for use in manual encoding/decoding) */
#define GV_struct_define_PV_tag 1
@ -72,6 +73,7 @@ extern "C" {
#define GV_struct_define_Robot_ForceValue_tag 19
#define GV_struct_define_Robot_DynamometerValue_tag 20
#define GV_struct_define_Robot_Speed_Mpm_tag 21
#define GV_struct_define_Robot_Move_Step_tag 22
#define GV_struct_define_Emergency_tag 23
/* Struct field encoding specification for nanopb */
@ -93,6 +95,7 @@ X(a, STATIC, SINGULAR, INT32, Robot_ChgLength, 18) \
X(a, STATIC, SINGULAR, INT32, Robot_ForceValue, 19) \
X(a, STATIC, SINGULAR, INT32, Robot_DynamometerValue, 20) \
X(a, STATIC, SINGULAR, DOUBLE, Robot_Speed_Mpm, 21) \
X(a, STATIC, SINGULAR, INT32, Robot_Move_Step, 22) \
X(a, STATIC, SINGULAR, INT32, Emergency, 23)
#define GV_struct_define_CALLBACK NULL
#define GV_struct_define_DEFAULT NULL
@ -110,7 +113,7 @@ extern const pb_msgdesc_t GV_struct_define_msg;
/* Maximum encoded size of messages (where known) */
#define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size
#define GV_struct_define_size 912
#define GV_struct_define_size 924
#ifdef __cplusplus
} /* extern "C" */

3
Core/Protobuf/Proto/bsp_GV.proto
View File

@ -32,7 +32,8 @@ message GV_struct_define
int32 Robot_ForceValue = 19;
int32 Robot_DynamometerValue = 20;
double Robot_Speed_Mpm= 21;
int32 Robot_Move_Step = 22;
int32 Emergency = 23;
};

440
Core/Src/FSM.c
View File

@ -138,272 +138,304 @@ double Speed_Judge()
return 0;
}
void Robot_Move()
void Robot_Halt(MoveSTATE_t STATE)
{
// 急停版本一
if(DHRougheningContrller.Emergency_Stop == 1)
{
GF_BSP_GPIO_SetIO(Out_Vacucm,1);
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
if(Polish_Count == 100 && polish_Flag == 1)
{
GF_BSP_GPIO_SetIO(Out_Polish,1);
polish_Flag = 0;
Polish_Count = 0;
}
else if(polish_Flag == 1)
{
GF_BSP_GPIO_SetIO(Out_Polish,0);
Polish_Count++;
}
if(DHRougheningContrller.Horizontal_Task == 1
|| DHRougheningContrller.Vertical_Task == 1)
{
GV.AuTo_Flag = 99;
}
else
{
GV.AuTo_Flag = 0;
}
CurrentMoveState = Move_Emergency;
return;
}
else if(DHRougheningContrller.Emergency_Stop == 0)
{
}
if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|| DHRougheningContrller.LaneChange_Vertical_Up == 1))
{
GV.Chg_Flag = 1;
Chg_Pa.change_road_finish_flag = 1;
}
else
{
GV.Chg_Flag = 0;
Chg_Pa.change_road_finish_flag = 0;
}
CurrentMoveState = STATE; //停车
}
void Robot_ResetJudge()
{
if(DHRougheningContrller.Wireless_State == 0)
{
return;
}
if((DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 0
&& DHRougheningContrller.Vertical_Task == 0
&& DHRougheningContrller.Horizontal_Task ==0)
&& DHRougheningContrller.Forwards == 0
&& DHRougheningContrller.Backwards == 0
&& DHRougheningContrller.Left_Turn == 0
&& DHRougheningContrller.Right_Turn == 0
&& DHRougheningContrller.Roughening_Start == 0
&& DHRougheningContrller.End_Down == 0
&& DHRougheningContrller.End_Up == 0)
{
GV.Robot_Move_Step = 1;
}
}
// //运动部分 Move Region
if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|| DHRougheningContrller.LaneChange_Vertical_Up == 1
|| DHRougheningContrller.Vertical_Task == 1
|| DHRougheningContrller.Horizontal_Task ==1)
&& DHRougheningContrller.Forwards == 0
&& DHRougheningContrller.Backwards == 0
&& DHRougheningContrller.Left_Turn == 0
&& DHRougheningContrller.Right_Turn == 0)
void Robot_Move()
{
// 急停版本一
if(DHRougheningContrller.Emergency_Stop == 1)
{
if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 0
&& (DHRougheningContrller.Vertical_Task == 1
|| DHRougheningContrller.Horizontal_Task == 1)) //自动作业与换道互斥
GF_BSP_GPIO_SetIO(Out_Vacucm,1);
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
if(Polish_Count == 100 && polish_Flag == 1)
{
if(DHRougheningContrller.Horizontal_Task == 1)
{
CurrentMoveState = Move_AutoForward;
if(GV.AuTo_Flag == 0)
{
GV.AuTo_Flag=1;
}
}
else if(DHRougheningContrller.Vertical_Task == 1)
{
CurrentMoveState = Move_AutoBackward;
if(GV.AuTo_Flag == 0)
{
GV.AuTo_Flag=3;
}
}
else
{
CurrentMoveState = Move_HALT;//停车
}
GF_BSP_GPIO_SetIO(Out_Polish,1);
polish_Flag = 99;
Polish_Count = 0;
}
else if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|| DHRougheningContrller.LaneChange_Vertical_Up == 1)
&& (DHRougheningContrller.Vertical_Task == 0
&& DHRougheningContrller.Horizontal_Task == 0))
else if(polish_Flag == 1)
{
if(GV.Chg_Flag == 1
&& (DHRougheningContrller.LaneChange_Horizontal_Left == 1
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|| DHRougheningContrller.LaneChange_Vertical_Up == 1))
{
CurrentMoveState = Move_ChgFinish;
}
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 1 )
{
CurrentMoveState = Move_ChgUp;
}
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 1
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
{
CurrentMoveState = Move_ChgDown;
}
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 1
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
{
CurrentMoveState = Move_ChgLeft;
}
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 1
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
{
CurrentMoveState = Move_ChgRight;
}
else
{
CurrentMoveState = Move_HALT;//停车
}
GF_BSP_GPIO_SetIO(Out_Polish,0);
Polish_Count++;
}
else
Robot_Halt(Move_Emergency);
return;
}
else if(DHRougheningContrller.Emergency_Stop == 0)
{
if(DHRougheningContrller.Roughening_Stop && polish_Flag == 99)
{
CurrentMoveState = Move_HALT;//停车
polish_Flag = 0;
}
}
else if((DHRougheningContrller.LaneChange_Horizontal_Left == 0
//运动部分 Move Region
if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|| DHRougheningContrller.LaneChange_Vertical_Up == 1
|| DHRougheningContrller.Vertical_Task == 1
|| DHRougheningContrller.Horizontal_Task ==1)
&& DHRougheningContrller.Forwards == 0
&& DHRougheningContrller.Backwards == 0
&& DHRougheningContrller.Left_Turn == 0
&& DHRougheningContrller.Right_Turn == 0)
{
if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 0
&& DHRougheningContrller.Vertical_Task == 0
&& DHRougheningContrller.Horizontal_Task == 0)
&& (DHRougheningContrller.Forwards == 1
|| DHRougheningContrller.Backwards == 1
|| DHRougheningContrller.Left_Turn == 1
|| DHRougheningContrller.Right_Turn == 1))
&& (DHRougheningContrller.Vertical_Task == 1
|| DHRougheningContrller.Horizontal_Task == 1)) //自动作业与换道互斥
{
if (DHRougheningContrller.Left_Turn == 0
&& DHRougheningContrller.Right_Turn == 0)//前进周围300范围内才继续检测前进是否按下
{
if (DHRougheningContrller.Forwards == 1)//前进
if(DHRougheningContrller.Horizontal_Task == 1)
{
CurrentMoveState = Move_AutoForward;
if(GV.AuTo_Flag == 0)
{
CurrentMoveState = Move_Forwards;
GV.AuTo_Flag=1;
}
else if (DHRougheningContrller.Backwards == 1)
}
else if(DHRougheningContrller.Vertical_Task == 1)
{
CurrentMoveState = Move_AutoBackward;
if(GV.AuTo_Flag == 0)
{
CurrentMoveState = Move_Backwards; //后退
GV.AuTo_Flag=3;
}
else
{
CurrentMoveState = Move_HALT;//停车
}
}
else if (DHRougheningContrller.Forwards == 0
&& DHRougheningContrller.Backwards == 0)
}
else
{
CurrentMoveState = Move_HALT;//停车
}
}
else if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|| DHRougheningContrller.LaneChange_Vertical_Up == 1)
&& (DHRougheningContrller.Vertical_Task == 0
&& DHRougheningContrller.Horizontal_Task == 0))
{
if(GV.Chg_Flag == 1
&& (DHRougheningContrller.LaneChange_Horizontal_Left == 1
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|| DHRougheningContrller.LaneChange_Vertical_Up == 1))
{
if (DHRougheningContrller.Right_Turn == 1)
{
CurrentMoveState = Move_TurnRight; //右转
}
else if (DHRougheningContrller.Left_Turn == 1)
{
CurrentMoveState = Move_TurnLeft; //左转
}
else
{
CurrentMoveState = Move_HALT; //停车
}
CurrentMoveState = Move_ChgFinish;
}
else
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 1 )
{
CurrentMoveState = Move_HALT; //停车
CurrentMoveState = Move_ChgUp;
}
}
else
{
if(DHRougheningContrller.Horizontal_Task == 1
|| DHRougheningContrller.Vertical_Task == 1)
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 1
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
{
GV.AuTo_Flag = 99;
CurrentMoveState = Move_ChgDown;
}
else
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 1
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
{
GV.AuTo_Flag = 0;
CurrentMoveState = Move_ChgLeft;
}
if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|| DHRougheningContrller.LaneChange_Vertical_Up == 1))
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 1
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
{
GV.Chg_Flag = 1;
Chg_Pa.change_road_finish_flag = 1;
CurrentMoveState = Move_ChgRight;
}
else
{
GV.Chg_Flag = 0;
Chg_Pa.change_road_finish_flag = 0;
CurrentMoveState = Move_HALT;//停车
}
CurrentMoveState = Move_HALT; //停车
}
}
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
Robot_Move();
/****************************************/
//LU_Single 控制打磨电机
if(DHRougheningContrller.Roughening_Start && polish_Flag == 0)
else
{
GF_BSP_GPIO_SetIO(Out_Vacucm,0);
GF_BSP_GPIO_SetIO(Out_Polish,0);
if(Polish_Count == 100)
CurrentMoveState = Move_HALT;//停车
}
}
else if((DHRougheningContrller.LaneChange_Horizontal_Left == 0
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
&& DHRougheningContrller.LaneChange_Vertical_Up == 0
&& DHRougheningContrller.Vertical_Task == 0
&& DHRougheningContrller.Horizontal_Task == 0)
&& (DHRougheningContrller.Forwards == 1
|| DHRougheningContrller.Backwards == 1
|| DHRougheningContrller.Left_Turn == 1
|| DHRougheningContrller.Right_Turn == 1))
{
if (DHRougheningContrller.Left_Turn == 0
&& DHRougheningContrller.Right_Turn == 0)//前进周围300范围内才继续检测前进是否按下
{
if (DHRougheningContrller.Forwards == 1)//前进
{
CurrentMoveState = Move_Forwards;
}
else if (DHRougheningContrller.Backwards == 1)
{
GF_BSP_GPIO_SetIO(Out_Polish,1);
polish_Flag = 1;
CurrentMoveState = Move_Backwards; //后退
}
else
{
Polish_Count++;
CurrentMoveState = Move_HALT;//停车
}
}
else if(DHRougheningContrller.Roughening_Stop && polish_Flag == 1)
else if (DHRougheningContrller.Forwards == 0
&& DHRougheningContrller.Backwards == 0)
{
GF_BSP_GPIO_SetIO(Out_Vacucm,1);
GF_BSP_GPIO_SetIO(Out_Polish,0);
if(Polish_Count == 100)
if (DHRougheningContrller.Right_Turn == 1)
{
CurrentMoveState = Move_TurnRight; //右转
}
else if (DHRougheningContrller.Left_Turn == 1)
{
GF_BSP_GPIO_SetIO(Out_Polish,1);
polish_Flag = 0;
CurrentMoveState = Move_TurnLeft; //左转
}
else
{
Polish_Count++;
CurrentMoveState = Move_HALT; //停车
}
}
else
{
Polish_Count=0;
CurrentMoveState = Move_HALT; //停车
}
//LU_Three控制打磨升降
if(DHRougheningContrller.End_Up)
}
else
{
Robot_Halt(Move_HALT); //停车
}
/****************************************/
//LU_Single 控制打磨电机
if(DHRougheningContrller.Roughening_Start && polish_Flag == 0)
{
GF_BSP_GPIO_SetIO(Out_Vacucm,0);
GF_BSP_GPIO_SetIO(Out_Polish,0);
if(Polish_Count == 100)
{
GF_BSP_GPIO_SetIO(Out_Lift_1,0);
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
GF_BSP_GPIO_SetIO(Out_Polish,1);
polish_Flag = 1;
}
else if(DHRougheningContrller.End_Down)
else
{
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
GF_BSP_GPIO_SetIO(Out_Lift_2,0);
Polish_Count++;
}
}
else if(DHRougheningContrller.Roughening_Stop && polish_Flag == 1)
{
GF_BSP_GPIO_SetIO(Out_Vacucm,1);
GF_BSP_GPIO_SetIO(Out_Polish,0);
if(Polish_Count == 100)
{
GF_BSP_GPIO_SetIO(Out_Polish,1);
polish_Flag = 0;
}
else
{
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
Polish_Count++;
}
}
else
{
Polish_Count=0;
}
//LU_Three控制打磨升降
if(DHRougheningContrller.End_Up)
{
GF_BSP_GPIO_SetIO(Out_Lift_1,0);
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
}
else if(DHRougheningContrller.End_Down)
{
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
GF_BSP_GPIO_SetIO(Out_Lift_2,0);
}
else
{
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
}
}
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
switch(GV.Robot_Move_Step)
{
case 0:
Robot_ResetJudge();
break;
case 1:
Robot_Move();
default:
break;
}
IV.Robot_CurrentState = CurrentMoveState;

Write Preview
Loading…
Cancel
Save