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
Unified 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. 1
      Core/Protobuf/Proto/bsp_GV.proto
  5. 122
      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_Up :1;//竖直上换道
uint16_t LaneChange_Vertical_Down :1;//竖直下换道 uint16_t LaneChange_Vertical_Down :1;//竖直下换道
uint16_t Wireless_FirstFlag;
}DHRoughening; }DHRoughening;

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

@ -44,7 +44,9 @@ void read_DHRoughening_Data(void) {
DHRoughening_Controller_UART_Handler); 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) { void decode_DHRoughening(uint8_t *buffer, uint16_t length) {
memcpy(data5555,buffer,length); memcpy(data5555,buffer,length);
@ -52,10 +54,18 @@ void decode_DHRoughening(uint8_t *buffer, uint16_t length) {
/* CRC 校验正确 */ /* CRC 校验正确 */
if (crc_check == MB_CRC16(buffer, length - 2)) if (crc_check == MB_CRC16(buffer, length - 2))
{ {
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
"DHRoughening", 1); "DHRoughening", 1);
memcpy(&DHRougheningContrller,&buffer[3],12);//16个字节数字 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_LeftCompensation=(*Roughenging_LeftCompensation_Range)*DHRougheningContrller.Left_Compensation*100/65535;
// *Roughenging_RightCompensation=(*Roughenging_RightCompensation_Range)*DHRougheningContrller.Right_Compensation*100/65535; // *Roughenging_RightCompensation=(*Roughenging_RightCompensation_Range)*DHRougheningContrller.Right_Compensation*100/65535;
} else } else
@ -72,8 +82,6 @@ void send_to_DHRoughening()
(uint16_t) abs(GV.Robot_Angle.RF_Angle_Roll), (uint16_t) abs(GV.Robot_Angle.RF_Angle_Roll),
(uint16_t) abs(GV.Robot_ForceValue )); (uint16_t) abs(GV.Robot_ForceValue ));
} }
//左补偿 xxx.xx 度 0x0000—0xFFFF==0—655.35 度 //左补偿 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_ForceValue;
int32_t Robot_DynamometerValue; int32_t Robot_DynamometerValue;
double Robot_Speed_Mpm; double Robot_Speed_Mpm;
int32_t Robot_Move_Step;
int32_t Emergency; int32_t Emergency;
} GV_struct_define; } GV_struct_define;
@ -51,8 +52,8 @@ extern "C" {
#endif #endif
/* Initializer values for message structs */ /* 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_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} #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) */ /* Field tags (for use in manual encoding/decoding) */
#define GV_struct_define_PV_tag 1 #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_ForceValue_tag 19
#define GV_struct_define_Robot_DynamometerValue_tag 20 #define GV_struct_define_Robot_DynamometerValue_tag 20
#define GV_struct_define_Robot_Speed_Mpm_tag 21 #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 #define GV_struct_define_Emergency_tag 23
/* Struct field encoding specification for nanopb */ /* 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_ForceValue, 19) \
X(a, STATIC, SINGULAR, INT32, Robot_DynamometerValue, 20) \ X(a, STATIC, SINGULAR, INT32, Robot_DynamometerValue, 20) \
X(a, STATIC, SINGULAR, DOUBLE, Robot_Speed_Mpm, 21) \ 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) X(a, STATIC, SINGULAR, INT32, Emergency, 23)
#define GV_struct_define_CALLBACK NULL #define GV_struct_define_CALLBACK NULL
#define GV_struct_define_DEFAULT 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) */ /* Maximum encoded size of messages (where known) */
#define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size #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 #ifdef __cplusplus
} /* extern "C" */ } /* extern "C" */

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

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

122
Core/Src/FSM.c
View File

@ -138,6 +138,61 @@ double Speed_Judge()
return 0; return 0;
} }
void Robot_Halt(MoveSTATE_t STATE)
{
if(DHRougheningContrller.Horizontal_Task == 1
|| DHRougheningContrller.Vertical_Task == 1)
{
GV.AuTo_Flag = 99;
}
else
{
GV.AuTo_Flag = 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;
}
}
void Robot_Move() void Robot_Move()
{ {
// 急停版本一 // 急停版本一
@ -150,7 +205,7 @@ void Robot_Move()
if(Polish_Count == 100 && polish_Flag == 1) if(Polish_Count == 100 && polish_Flag == 1)
{ {
GF_BSP_GPIO_SetIO(Out_Polish,1); GF_BSP_GPIO_SetIO(Out_Polish,1);
polish_Flag = 0; polish_Flag = 99;
Polish_Count = 0; Polish_Count = 0;
} }
else if(polish_Flag == 1) else if(polish_Flag == 1)
@ -158,17 +213,19 @@ void Robot_Move()
GF_BSP_GPIO_SetIO(Out_Polish,0); GF_BSP_GPIO_SetIO(Out_Polish,0);
Polish_Count++; Polish_Count++;
} }
Robot_Halt(Move_Emergency);
CurrentMoveState = Move_Emergency;
return; return;
} }
else if(DHRougheningContrller.Emergency_Stop == 0) else if(DHRougheningContrller.Emergency_Stop == 0)
{ {
if(DHRougheningContrller.Roughening_Stop && polish_Flag == 99)
{
polish_Flag = 0;
}
} }
// //运动部分 Move Region //运动部分 Move Region
if((DHRougheningContrller.LaneChange_Horizontal_Left == 1 if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1 || DHRougheningContrller.LaneChange_Horizontal_Right == 1
|| DHRougheningContrller.LaneChange_Vertical_Down == 1 || DHRougheningContrller.LaneChange_Vertical_Down == 1
@ -267,6 +324,7 @@ void Robot_Move()
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 && DHRougheningContrller.LaneChange_Vertical_Up == 0
&& DHRougheningContrller.Vertical_Task == 0 && DHRougheningContrller.Vertical_Task == 0
&& DHRougheningContrller.Horizontal_Task == 0) && DHRougheningContrller.Horizontal_Task == 0)
&& (DHRougheningContrller.Forwards == 1 && (DHRougheningContrller.Forwards == 1
|| DHRougheningContrller.Backwards == 1 || DHRougheningContrller.Backwards == 1
|| DHRougheningContrller.Left_Turn == 1 || DHRougheningContrller.Left_Turn == 1
@ -311,42 +369,9 @@ void Robot_Move()
} }
else else
{ {
Robot_Halt(Move_HALT); //停车
if(DHRougheningContrller.Horizontal_Task == 1
|| DHRougheningContrller.Vertical_Task == 1)
{
GV.AuTo_Flag = 99;
}
else
{
GV.AuTo_Flag = 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 = Move_HALT; //停车
}
} }
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
Robot_Move();
/****************************************/ /****************************************/
//LU_Single 控制打磨电机 //LU_Single 控制打磨电机
if(DHRougheningContrller.Roughening_Start && polish_Flag == 0) if(DHRougheningContrller.Roughening_Start && polish_Flag == 0)
{ {
@ -381,10 +406,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
Polish_Count=0; Polish_Count=0;
} }
//LU_Three控制打磨升降 //LU_Three控制打磨升降
if(DHRougheningContrller.End_Up) if(DHRougheningContrller.End_Up)
{ {
@ -401,9 +422,20 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
GF_BSP_GPIO_SetIO(Out_Lift_1,1); GF_BSP_GPIO_SetIO(Out_Lift_1,1);
GF_BSP_GPIO_SetIO(Out_Lift_2,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; IV.Robot_CurrentState = CurrentMoveState;

Write Preview
Loading…
Cancel
Save