From 59c441387fd222fc2dea6b24ac616e41de4e647b Mon Sep 17 00:00:00 2001 From: "LIN\\54376" <543769318@qq.com> Date: Thu, 25 Sep 2025 11:39:47 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=B7=A6=E5=8F=B3=E6=BB=91?= =?UTF-8?q?=E8=BD=AE=E8=B0=83=E8=8A=82=E5=B7=A6=E5=8F=B3=E8=A1=A5=E5=81=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BASE/Inc/BSP/BHBF_ROBOT.h | 24 ++++++++++++++++++- BASE/Src/BSP/BHBF_ROBOT.c | 39 ++++++++++++++++++++++++++++--- Core/Protobuf/PSource/bsp_GV.pb.h | 2 +- Core/Protobuf/PSource/bsp_IV.pb.h | 14 +++++++---- Core/Protobuf/PSource/bsp_PV.pb.h | 20 ++++++---------- Core/Protobuf/Proto/bsp_IV.proto | 3 +++ Core/Protobuf/Proto/bsp_PV.proto | 6 ++--- Core/Src/FSM.c | 38 ++++++------------------------ Core/Src/main.c | 2 ++ Core/Src/robot_state.c | 16 ++++++------- 10 files changed, 99 insertions(+), 65 deletions(-) diff --git a/BASE/Inc/BSP/BHBF_ROBOT.h b/BASE/Inc/BSP/BHBF_ROBOT.h index c6d7c9c..06c3a5c 100644 --- a/BASE/Inc/BSP/BHBF_ROBOT.h +++ b/BASE/Inc/BSP/BHBF_ROBOT.h @@ -11,7 +11,7 @@ - +#include "left_right_compensation_control.h" #include "../../BASE/Inc/BSP/bsp_include.h" #include "bsp_PV.pb.h" @@ -67,6 +67,28 @@ #include "../../BASE/Inc/BSP/bsp_Error_Detect.h" #include "bsp_IV.pb.h" #include "bsp_PV.pb.h" + + + +typedef struct sys_timer_handler +{ + int start_timer; + int sys_current_timer_count; + int sys_timer_flag; + +}Sys_timer_handler; + +extern Sys_timer_handler timer_handler_1; +extern Sys_timer_handler timer_handler_2; +extern Sys_timer_handler timer_handler_3; +extern Sys_timer_handler timer_handler_4; + +//first start Timer, then WaitTimer +extern bool CompareTimer(int32_t DelayMiliSeconds,Sys_timer_handler * timer_handler); +extern void SystemTimer_Intialize(); +extern void GF_Timer_Count(); + + //#include "msp_pressure_sensor.h" //#include "robot_state.h" //FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 896K diff --git a/BASE/Src/BSP/BHBF_ROBOT.c b/BASE/Src/BSP/BHBF_ROBOT.c index 56f3dc1..fed0096 100644 --- a/BASE/Src/BSP/BHBF_ROBOT.c +++ b/BASE/Src/BSP/BHBF_ROBOT.c @@ -7,12 +7,15 @@ #include "fsm.h" #include "BHBF_ROBOT.h" #include - - #include "bsp_FDCAN.h" +int32_t SystemTimeMiliCount; //2ms加一 - +//初始化定时器 +Sys_timer_handler timer_handler_1; +Sys_timer_handler timer_handler_2; +Sys_timer_handler timer_handler_3; +Sys_timer_handler timer_handler_4; //7 motors MotorParameters *Motor[3]; @@ -65,3 +68,33 @@ void GF_WatchDog_Loop() } +//比较计数值 +bool CompareTimer(int32_t DelayMiliSeconds,Sys_timer_handler * timer_handler) +{ + if(timer_handler->start_timer==1) + { + timer_handler->sys_current_timer_count=DelayMiliSeconds/2+SystemTimeMiliCount; + timer_handler->start_timer=0; + } + if(timer_handler->sys_current_timer_count<=SystemTimeMiliCount) + { + return true; + }else + { + return false; + } + +} +//定时数数 +void GF_Timer_Count() +{ + SystemTimeMiliCount++; +} +//将数数写入定时器 +void SystemTimer_Intialize() +{ + SystemTimeMiliCount=0; + GF_BSP_Interrupt_Add_CallBack( + DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Timer_Count); +} + diff --git a/Core/Protobuf/PSource/bsp_GV.pb.h b/Core/Protobuf/PSource/bsp_GV.pb.h index 72363b9..3768f11 100644 --- a/Core/Protobuf/PSource/bsp_GV.pb.h +++ b/Core/Protobuf/PSource/bsp_GV.pb.h @@ -107,7 +107,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 902 +#define GV_struct_define_size 884 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/PSource/bsp_IV.pb.h b/Core/Protobuf/PSource/bsp_IV.pb.h index 213b6dc..06c9987 100644 --- a/Core/Protobuf/PSource/bsp_IV.pb.h +++ b/Core/Protobuf/PSource/bsp_IV.pb.h @@ -24,6 +24,8 @@ typedef struct _IV_struct_define { int32_t Robot_Current_Right; int32_t Robot_Error_Left; int32_t Robot_Error_Right; + int32_t Robot_Compensation_Left; + int32_t Robot_Compensation_Right; } IV_struct_define; @@ -32,8 +34,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define IV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#define IV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define IV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define IV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define IV_struct_define_Robot_Move_AutoSpeed_tag 1 @@ -48,6 +50,8 @@ extern "C" { #define IV_struct_define_Robot_Current_Right_tag 10 #define IV_struct_define_Robot_Error_Left_tag 11 #define IV_struct_define_Robot_Error_Right_tag 12 +#define IV_struct_define_Robot_Compensation_Left_tag 13 +#define IV_struct_define_Robot_Compensation_Right_tag 14 /* Struct field encoding specification for nanopb */ #define IV_struct_define_FIELDLIST(X, a) \ @@ -62,7 +66,9 @@ X(a, STATIC, SINGULAR, INT32, Robot_CurrentState, 8) \ X(a, STATIC, SINGULAR, INT32, Robot_Current_Left, 9) \ X(a, STATIC, SINGULAR, INT32, Robot_Current_Right, 10) \ X(a, STATIC, SINGULAR, INT32, Robot_Error_Left, 11) \ -X(a, STATIC, SINGULAR, INT32, Robot_Error_Right, 12) +X(a, STATIC, SINGULAR, INT32, Robot_Error_Right, 12) \ +X(a, STATIC, SINGULAR, INT32, Robot_Compensation_Left, 13) \ +X(a, STATIC, SINGULAR, INT32, Robot_Compensation_Right, 14) #define IV_struct_define_CALLBACK NULL #define IV_struct_define_DEFAULT NULL @@ -73,7 +79,7 @@ extern const pb_msgdesc_t IV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #define BSP_IV_PB_H_MAX_SIZE IV_struct_define_size -#define IV_struct_define_size 132 +#define IV_struct_define_size 154 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/PSource/bsp_PV.pb.h b/Core/Protobuf/PSource/bsp_PV.pb.h index 103c5b4..ea3c3fc 100644 --- a/Core/Protobuf/PSource/bsp_PV.pb.h +++ b/Core/Protobuf/PSource/bsp_PV.pb.h @@ -12,8 +12,6 @@ /* Struct definitions */ typedef struct _PV_struct_define { int32_t Robot_ChgLength; - double Robot_LeftCompensation; - double Robot_RightCompensation; double Robot_AutoSpeedBase; double Robot_ManualSpeedBase; } PV_struct_define; @@ -24,23 +22,19 @@ extern "C" { #endif /* Initializer values for message structs */ -#define PV_struct_define_init_default {0, 0, 0, 0, 0} -#define PV_struct_define_init_zero {0, 0, 0, 0, 0} +#define PV_struct_define_init_default {0, 0, 0} +#define PV_struct_define_init_zero {0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define PV_struct_define_Robot_ChgLength_tag 1 -#define PV_struct_define_Robot_LeftCompensation_tag 2 -#define PV_struct_define_Robot_RightCompensation_tag 3 -#define PV_struct_define_Robot_AutoSpeedBase_tag 4 -#define PV_struct_define_Robot_ManualSpeedBase_tag 5 +#define PV_struct_define_Robot_AutoSpeedBase_tag 2 +#define PV_struct_define_Robot_ManualSpeedBase_tag 3 /* Struct field encoding specification for nanopb */ #define PV_struct_define_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, INT32, Robot_ChgLength, 1) \ -X(a, STATIC, SINGULAR, DOUBLE, Robot_LeftCompensation, 2) \ -X(a, STATIC, SINGULAR, DOUBLE, Robot_RightCompensation, 3) \ -X(a, STATIC, SINGULAR, DOUBLE, Robot_AutoSpeedBase, 4) \ -X(a, STATIC, SINGULAR, DOUBLE, Robot_ManualSpeedBase, 5) +X(a, STATIC, SINGULAR, DOUBLE, Robot_AutoSpeedBase, 2) \ +X(a, STATIC, SINGULAR, DOUBLE, Robot_ManualSpeedBase, 3) #define PV_struct_define_CALLBACK NULL #define PV_struct_define_DEFAULT NULL @@ -51,7 +45,7 @@ extern const pb_msgdesc_t PV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #define BSP_PV_PB_H_MAX_SIZE PV_struct_define_size -#define PV_struct_define_size 47 +#define PV_struct_define_size 29 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/Proto/bsp_IV.proto b/Core/Protobuf/Proto/bsp_IV.proto index 94a585c..34b8185 100644 --- a/Core/Protobuf/Proto/bsp_IV.proto +++ b/Core/Protobuf/Proto/bsp_IV.proto @@ -17,4 +17,7 @@ message IV_struct_define{ int32 Robot_Error_Left= 11; int32 Robot_Error_Right= 12; + int32 Robot_Compensation_Left= 13; + int32 Robot_Compensation_Right= 14; + }; diff --git a/Core/Protobuf/Proto/bsp_PV.proto b/Core/Protobuf/Proto/bsp_PV.proto index ab0df54..4a50aef 100644 --- a/Core/Protobuf/Proto/bsp_PV.proto +++ b/Core/Protobuf/Proto/bsp_PV.proto @@ -2,8 +2,6 @@ syntax = "proto3"; message PV_struct_define{ int32 Robot_ChgLength= 1; - double Robot_LeftCompensation=2; - double Robot_RightCompensation=3; - double Robot_AutoSpeedBase=4; - double Robot_ManualSpeedBase=5; + double Robot_AutoSpeedBase=2; + double Robot_ManualSpeedBase=3; }; diff --git a/Core/Src/FSM.c b/Core/Src/FSM.c index a9c7315..d0a559a 100644 --- a/Core/Src/FSM.c +++ b/Core/Src/FSM.c @@ -100,6 +100,8 @@ int JJ_Flag; void GF_Dispatch()//2ms调用一次 给车体速度等赋值 { // //运动部分 Move Region + Lcompensation_control(); + Rcompensation_control(); int b=Get_BIT(SystemErrorCode, u7_sbus) ; if (Get_BIT(SystemErrorCode, u7_sbus) == 1) @@ -310,16 +312,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 } - //控制电磁阀喷枪：旧 -// if(P_U7->RU_Single==1000) // && CurrentMoveState != Move_AutoForward&& CurrentMoveState != Move_AutoBackward) -// { -// GF_BSP_GPIO_SetIO(Out_Spray,0); -// } -// else if(P_U7->RU_Single==-1000) // && CurrentMoveState != Move_AutoForward && CurrentMoveState != Move_AutoBackward) -// { -// GF_BSP_GPIO_SetIO(Out_Spray,1); -// } - // 控制电磁阀喷枪（非自锁版本） // 功能：按一下开启，再按一下关闭，长按不会反复切换 @@ -441,39 +433,23 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase* 186.9; // -// IV.Robot_Move_AutoSpeed = GV.PV.Robot_AutoSpeedBase;//好像是往安卓界面传数据的 -// IV.Robot_Move_ManualSpeed = GV.PV.Robot_ManualSpeedBase;//好像是往安卓界面传数据的 - // IV.Robot_Swing_Speed=GV.SwingMotor.Velcity; - // IV.Robot_Tilt_Speed=GV.TiltMotor.Velcity; + + 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_Error = 0; - if(GV.LeftFrontMotor.ERROR_Flag != 0) - { - IV.Robot_Error = 100+GV.LeftFrontMotor.ERROR_Flag; - } - else if(GV.RightFrontMotor.ERROR_Flag != 0) - { - IV.Robot_Error = 200+GV.RightFrontMotor.ERROR_Flag; - } - else if(GV.LeftFrontMotor.ERROR_Flag != 0 && GV.RightFrontMotor.ERROR_Flag != 0) - { - IV.Robot_Error = 10000 + GV.LeftFrontMotor.ERROR_Flag * 100 + GV.RightFrontMotor.ERROR_Flag; - } + 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; -// GV.Robot_ChgLength = CV.PV.Robot_ChgLength - 10; -// GV.Left_Compensation = CV.PV.Robot_LeftCompensation; -// GV.Right_Compensation = CV.PV.Robot_RightCompensation; + action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态 // action_perfrom(SwingTransitions,sizeof(SwingTransitions) / sizeof(transition_t), CurrentSwingState); diff --git a/Core/Src/main.c b/Core/Src/main.c index 2bc2f2a..1bede13 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -147,6 +147,8 @@ int main(void) MX_LPUART1_UART_Init(); MX_ADC2_Init(); /* USER CODE BEGIN 2 */ + SystemTimer_Intialize(); //加数数定时器 + GF_BSP_GPIO_SetIO(0,0);//0�??????????? 1�??????????? DLT_LOG_ENABLE_LEVEL=0;//7 send all information 0 send nothing Error_Detect_Intialzie(500);//every 1 seconds diff --git a/Core/Src/robot_state.c b/Core/Src/robot_state.c index 6c2f079..534577f 100644 --- a/Core/Src/robot_state.c +++ b/Core/Src/robot_state.c @@ -191,17 +191,17 @@ void Auto() //水平 if ((angle >= 45 && angle <= 135)) { - AM_Pa.ExpectationAngle = 90 - GV.PV.Robot_RightCompensation; + AM_Pa.ExpectationAngle = 90 - GV.Right_Compensation/100.0; } else if ((angle >= -135 && angle <= -45)) { - AM_Pa.ExpectationAngle = -90 + GV.PV.Robot_LeftCompensation; + AM_Pa.ExpectationAngle = -90 + GV.Left_Compensation/100.0; } //垂直 else if ((angle > -45 && angle < 45)) { - AM_Pa.ExpectationAngle = 0 - GV.PV.Robot_LeftCompensation + GV.PV.Robot_RightCompensation; + AM_Pa.ExpectationAngle = 0 - GV.Left_Compensation/100.0 + GV.Right_Compensation/100.0; } VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转 @@ -216,24 +216,24 @@ void Auto() break; case 2: VehicleAutoForward(angle, - GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动前进 + GV.Left_Compensation/100.0, GV.Right_Compensation/100.0);//自动前进 break; case 3: //水平 if ((angle >= 45 && angle <= 135)) { - AM_Pa.ExpectationAngle = 90 + GV.PV.Robot_LeftCompensation; + AM_Pa.ExpectationAngle = 90 + GV.Left_Compensation/100.0; } if ((angle >= -135 && angle <= -45)) { - AM_Pa.ExpectationAngle = - 90 - GV.PV.Robot_RightCompensation; + AM_Pa.ExpectationAngle = - 90 - GV.Right_Compensation/100.0; } //垂直 if ((angle > -45 && angle < 45)) { - AM_Pa.ExpectationAngle = 0 - GV.PV.Robot_LeftCompensation + GV.PV.Robot_RightCompensation; + AM_Pa.ExpectationAngle = 0 - GV.Left_Compensation/100.0 + GV.Right_Compensation/100.0; } VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转 @@ -248,7 +248,7 @@ void Auto() break; case 4: VehicleAutoBackward(angle, - GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动后退 + GV.Left_Compensation/100.0, GV.Right_Compensation/100.0);//自动后退 break;