From 0bd734d0f5080680b9b05c0ad85acc44f8b555f8 Mon Sep 17 00:00:00 2001 From: "LIN\\54376" <543769318@qq.com> Date: Fri, 19 Sep 2025 15:10:31 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=8D=A2=E5=8E=8B=E5=8A=9B=E4=BC=A0?= =?UTF-8?q?=E6=84=9F=E5=99=A8cmcu=EF=BC=9B=E6=B0=B4=E5=B9=B3=E6=8D=A2?= =?UTF-8?q?=E9=81=93=E8=A1=A5=E5=81=BF=E4=BB=8E60=E6=94=B9=E6=88=90?= =?UTF-8?q?=E4=BA=8610?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BASE/Inc/BSP/BHBF_ROBOT.h | 2 +- Core/Inc/FSM.h | 10 +- Core/Protobuf/PSource/bsp_IV.pb.h | 14 +- Core/Protobuf/PSource/msp_U7.pb.h | 30 ++-- Core/Protobuf/Proto/bsp_IV.proto | 4 + Core/Protobuf/Proto/msp_U7.proto | 10 +- Core/Src/FSM.c | 222 +++--------------------------- Core/Src/main.c | 2 +- Core/Src/robot_state.c | 8 +- 9 files changed, 67 insertions(+), 235 deletions(-) diff --git a/BASE/Inc/BSP/BHBF_ROBOT.h b/BASE/Inc/BSP/BHBF_ROBOT.h index 596cd53..c6d7c9c 100644 --- a/BASE/Inc/BSP/BHBF_ROBOT.h +++ b/BASE/Inc/BSP/BHBF_ROBOT.h @@ -27,7 +27,7 @@ #include "../../BASE/Inc/BSP/bsp_mqtt.h" #include "../../BASE/Inc/BSP/pb.h" -#include "../../BASE/Inc/MSP/msp_Force_Sensor.h" +#include "../../BASE/Inc/MSP/msp_CMCU.h" #include "../../BASE/Inc/MSP/msp_Dynamometer_Sensor.h" #include "../../BASE/Inc/BSP/pb_decode.h" #include "../../BASE/Inc/BSP/pb_encode.h" diff --git a/Core/Inc/FSM.h b/Core/Inc/FSM.h index c102c8b..94931ac 100644 --- a/Core/Inc/FSM.h +++ b/Core/Inc/FSM.h @@ -15,11 +15,11 @@ // //#define Out_Spray 3 //4 -#define Out_Polish 1 -#define Out_Vacuum 2 -#define Out_Lift_1 3 -#define Out_Lift_2 4 -#define Out_Spray 5 +#define Out_Polish 2 +#define Out_Vacuum 3 +#define Out_Lift_1 4 +#define Out_Lift_2 5 +#define Out_Spray 3 //typedef enum _STATE_t diff --git a/Core/Protobuf/PSource/bsp_IV.pb.h b/Core/Protobuf/PSource/bsp_IV.pb.h index 08ba246..213b6dc 100644 --- a/Core/Protobuf/PSource/bsp_IV.pb.h +++ b/Core/Protobuf/PSource/bsp_IV.pb.h @@ -22,6 +22,8 @@ typedef struct _IV_struct_define { int32_t Robot_CurrentState; int32_t Robot_Current_Left; int32_t Robot_Current_Right; + int32_t Robot_Error_Left; + int32_t Robot_Error_Right; } IV_struct_define; @@ -30,8 +32,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} -#define IV_struct_define_init_zero {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} +#define IV_struct_define_init_zero {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 @@ -44,6 +46,8 @@ extern "C" { #define IV_struct_define_Robot_CurrentState_tag 8 #define IV_struct_define_Robot_Current_Left_tag 9 #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 /* Struct field encoding specification for nanopb */ #define IV_struct_define_FIELDLIST(X, a) \ @@ -56,7 +60,9 @@ X(a, STATIC, SINGULAR, INT32, Robot_DynamometerValue, 6) \ X(a, STATIC, SINGULAR, INT32, Robot_ForceValue, 7) \ 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_Current_Right, 10) \ +X(a, STATIC, SINGULAR, INT32, Robot_Error_Left, 11) \ +X(a, STATIC, SINGULAR, INT32, Robot_Error_Right, 12) #define IV_struct_define_CALLBACK NULL #define IV_struct_define_DEFAULT NULL @@ -67,7 +73,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 110 +#define IV_struct_define_size 132 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/Protobuf/PSource/msp_U7.pb.h b/Core/Protobuf/PSource/msp_U7.pb.h index 34f6a72..187b42a 100644 --- a/Core/Protobuf/PSource/msp_U7.pb.h +++ b/Core/Protobuf/PSource/msp_U7.pb.h @@ -14,17 +14,17 @@ typedef struct _SP_MSP_U7_Button { int32_t RxIndex; int32_t CH0_LYB_V; int32_t CH1_LYB_H; - int32_t L1; /* int32 CH3_LYS_V= 4; */ - int32_t L2; /* int32 CH4_LYS_H= 5; */ - int32_t MODE; + int32_t M1; /* int32 CH3_LYS_V= 4; */ + int32_t M5; /* int32 CH4_LYS_H= 5; */ + int32_t M3; int32_t LU_pulley; int32_t RU_pulley; int32_t LU_Three; int32_t RU_Three; int32_t LU_Single; int32_t RU_Single; - int32_t R1; - int32_t R2; + int32_t M2; + int32_t M6; int32_t S3; int32_t IsOnline; int32_t S4; @@ -43,17 +43,17 @@ extern "C" { #define SP_MSP_U7_Button_RxIndex_tag 1 #define SP_MSP_U7_Button_CH0_LYB_V_tag 2 #define SP_MSP_U7_Button_CH1_LYB_H_tag 3 -#define SP_MSP_U7_Button_L1_tag 4 -#define SP_MSP_U7_Button_L2_tag 5 -#define SP_MSP_U7_Button_MODE_tag 6 +#define SP_MSP_U7_Button_M1_tag 4 +#define SP_MSP_U7_Button_M5_tag 5 +#define SP_MSP_U7_Button_M3_tag 6 #define SP_MSP_U7_Button_LU_pulley_tag 7 #define SP_MSP_U7_Button_RU_pulley_tag 8 #define SP_MSP_U7_Button_LU_Three_tag 9 #define SP_MSP_U7_Button_RU_Three_tag 10 #define SP_MSP_U7_Button_LU_Single_tag 11 #define SP_MSP_U7_Button_RU_Single_tag 12 -#define SP_MSP_U7_Button_R1_tag 13 -#define SP_MSP_U7_Button_R2_tag 14 +#define SP_MSP_U7_Button_M2_tag 13 +#define SP_MSP_U7_Button_M6_tag 14 #define SP_MSP_U7_Button_S3_tag 15 #define SP_MSP_U7_Button_IsOnline_tag 16 #define SP_MSP_U7_Button_S4_tag 18 @@ -63,17 +63,17 @@ extern "C" { X(a, STATIC, SINGULAR, INT32, RxIndex, 1) \ X(a, STATIC, SINGULAR, INT32, CH0_LYB_V, 2) \ X(a, STATIC, SINGULAR, INT32, CH1_LYB_H, 3) \ -X(a, STATIC, SINGULAR, INT32, L1, 4) \ -X(a, STATIC, SINGULAR, INT32, L2, 5) \ -X(a, STATIC, SINGULAR, INT32, MODE, 6) \ +X(a, STATIC, SINGULAR, INT32, M1, 4) \ +X(a, STATIC, SINGULAR, INT32, M5, 5) \ +X(a, STATIC, SINGULAR, INT32, M3, 6) \ X(a, STATIC, SINGULAR, INT32, LU_pulley, 7) \ X(a, STATIC, SINGULAR, INT32, RU_pulley, 8) \ X(a, STATIC, SINGULAR, INT32, LU_Three, 9) \ X(a, STATIC, SINGULAR, INT32, RU_Three, 10) \ X(a, STATIC, SINGULAR, INT32, LU_Single, 11) \ X(a, STATIC, SINGULAR, INT32, RU_Single, 12) \ -X(a, STATIC, SINGULAR, INT32, R1, 13) \ -X(a, STATIC, SINGULAR, INT32, R2, 14) \ +X(a, STATIC, SINGULAR, INT32, M2, 13) \ +X(a, STATIC, SINGULAR, INT32, M6, 14) \ X(a, STATIC, SINGULAR, INT32, S3, 15) \ X(a, STATIC, SINGULAR, INT32, IsOnline, 16) \ X(a, STATIC, SINGULAR, INT32, S4, 18) diff --git a/Core/Protobuf/Proto/bsp_IV.proto b/Core/Protobuf/Proto/bsp_IV.proto index b9fdc31..94a585c 100644 --- a/Core/Protobuf/Proto/bsp_IV.proto +++ b/Core/Protobuf/Proto/bsp_IV.proto @@ -13,4 +13,8 @@ message IV_struct_define{ int32 Robot_CurrentState= 8; int32 Robot_Current_Left= 9; int32 Robot_Current_Right= 10; + + int32 Robot_Error_Left= 11; + int32 Robot_Error_Right= 12; + }; diff --git a/Core/Protobuf/Proto/msp_U7.proto b/Core/Protobuf/Proto/msp_U7.proto index aa62737..54a81d6 100644 --- a/Core/Protobuf/Proto/msp_U7.proto +++ b/Core/Protobuf/Proto/msp_U7.proto @@ -4,17 +4,17 @@ message SP_MSP_U7_Button { int32 RxIndex= 1; int32 CH0_LYB_V= 2; int32 CH1_LYB_H= 3; - int32 L1= 4; //int32 CH3_LYS_V= 4; - int32 L2= 5; // int32 CH4_LYS_H= 5; - int32 MODE = 6; + int32 M1= 4; //int32 CH3_LYS_V= 4; + int32 M5= 5; // int32 CH4_LYS_H= 5; + int32 M3 = 6; int32 LU_pulley = 7; int32 RU_pulley = 8; int32 LU_Three = 9; int32 RU_Three = 10; int32 LU_Single = 11; int32 RU_Single = 12; - int32 R1 = 13; - int32 R2 = 14; + int32 M2 = 13; + int32 M6 = 14; int32 S3 = 15; int32 IsOnline= 16; int32 S4 = 18; diff --git a/Core/Src/FSM.c b/Core/Src/FSM.c index 01d3438..a9c7315 100644 --- a/Core/Src/FSM.c +++ b/Core/Src/FSM.c @@ -105,20 +105,18 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 if (Get_BIT(SystemErrorCode, u7_sbus) == 1) { - } - CurrentMoveState = Move_HALT; - if(!(P_U7->R1 == -1000 && P_U7->L1 == -1000 - && P_U7->R2 == -1000 && P_U7->L2 == -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)) { if(P_U7->RU_Three != 0 - && P_U7->R1 == -1000 && P_U7->L1 == -1000 - && P_U7->R2 == -1000 && P_U7->L2 == -1000) //自动作业与换道互斥 + && P_U7->M2 == -1000 && P_U7->M1 == -1000 + && P_U7->M6 == -1000 && P_U7->M5 == -1000) //自动作业与换道互斥 { if(P_U7->RU_Three == 1000) { @@ -127,13 +125,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 { GV.AuTo_Flag=1; } -// if(Spray_Flag == 0 -// && GV.LeftFrontMotor.Velcity >= GV.Robot_AutoSpeed - 100 -// && GV.RightFrontMotor.Velcity <= -GV.Robot_AutoSpeed + 100) -// { -// GF_BSP_GPIO_SetIO(Out_Spray,0); -// Spray_Flag = 1; -// } } else if(P_U7->RU_Three == -1000) @@ -143,45 +134,38 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 { GV.AuTo_Flag=3; } -// if(Spray_Flag == 0 -// && GV.RightFrontMotor.Velcity >= GV.Robot_AutoSpeed - 100 -// && GV.LeftFrontMotor.Velcity <= -GV.Robot_AutoSpeed + 100) -// { -// GF_BSP_GPIO_SetIO(Out_Spray,0); -// Spray_Flag = 1; -// } } else { CurrentMoveState = Move_HALT;//停车 } } - else if(!(P_U7->R1 == -1000 && P_U7->L1 == -1000 - && P_U7->R2 == -1000 && P_U7->L2 == -1000) + 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->L1 == 1000 || P_U7->R1 == 1000 - || P_U7->L2 == 1000 || P_U7->R2 == 1000)) + 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->L1 == 1000 - && P_U7->R1 == -1000 && P_U7->R2 == -1000 && P_U7->L2 == -1000) + 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->R1 == 1000 - && P_U7->L1 == -1000 && P_U7->R2 == -1000 && P_U7->L2 == -1000) + 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->L2 == 1000 - && P_U7->L1 == -1000 && P_U7->R1 == -1000 && P_U7->R2 == -1000) + 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->R2 == 1000 - && P_U7->L1 == -1000 && P_U7->R1 == -1000 && P_U7->L2 == -1000) + else if(P_U7->M6 == 1000 + && P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M5 == -1000) { CurrentMoveState = Move_ChgRight; } @@ -197,8 +181,8 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 } 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->R1 == -1000 && P_U7->L1 == -1000 - && P_U7->R2 == -1000 && P_U7->L2 == -1000 + && 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范围内才继续检测前进是否按下 @@ -213,25 +197,7 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 } else { -// if(CurrentMoveState == Move_AutoForward -// || CurrentMoveState == Move_AutoBackward) -// { -// if(Spray_Flag < 8) -// { -// GF_BSP_GPIO_SetIO(Out_Spray,1); -// Spray_Flag++; -// } -// else if(Spray_Flag >= 8) -// { -// Spray_Flag = 0; -// CurrentMoveState = Move_HALT;//停车 -// } -// } -// else -// { - CurrentMoveState = Move_HALT;//停车 -// } - + CurrentMoveState = Move_HALT;//停车 } } else if (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300) @@ -262,42 +228,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 /****************************************/ - //LU_Single 控制打磨电机:旧 -// if(P_U7->LU_Single==1000 && Polish_Flag == 0) -// { -// GF_BSP_GPIO_SetIO(Out_Polish,0); -// if(Polish_Count == 100) -// { -// GF_BSP_GPIO_SetIO(Out_Polish,1); -// Polish_Flag = 1; -// } -// else -// { -// Polish_Count++; -// } -// } -// else if(P_U7->LU_Single==-1000 && Polish_Flag == 1) -// { -// GF_BSP_GPIO_SetIO(Out_Polish,0); -// if(Polish_Count == 100) -// { -// GF_BSP_GPIO_SetIO(Out_Polish,1); -// Polish_Flag = 0; -// } -// else -// { -// Polish_Count++; -// } -// } -// else -// { -// Polish_Count=0; -// } - - - - - // 版本二：LU_Single 控制打磨电机和吸尘器（非自锁版本） if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0) { @@ -362,92 +292,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 } } - - -// /* 版本三：LU_Single状态机Switch版本（尚未验证） */ -// // 1. 按键触发逻辑（仅在空闲状态响应） -// if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0) -// { -// Polish_lock = 1; // 锁定触发，防止重复触发 -// -// if (Polish_Flag == 0) -// { -// process_state =21; // 切换到开启流程 -// Vacuum_Delay_Count = 0; // 重置吸尘器延迟计数 -// } -// else -// { -// process_state = 2; // 切换到关闭流程 -// Polish_Count = 100; // 初始化关闭计数 -// } -// } -// // 按键松开时解除锁定 -// else if (P_U7->LU_Single == -1000) -// { -// Polish_lock = 0; -// } -// -// // 2. 状态机主体（使用switch-case实现） -// switch (process_state) -// { -// case 0: -// // 空闲状态：无需执行操作，等待触发 -// break; -// -// case 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++; -// } -// } -// break; -// -// case 2: -// // 关闭流程：先处理打磨关闭，再关吸尘器 -// 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--; -// } -// break; -// -// default: -// // 异常状态处理：回到初始状态 -// process_state = 0; -// Polish_Count = 0; -// Vacuum_Delay_Count = 0; -// break; -// } - - - //LU_Three控制打磨推杆升降 if(P_U7->LU_Three==1000) { @@ -591,30 +435,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 - - - - -// if(P_U7->LU_pulley>=900 && JJ_Flag==0) -// { -// JJ_Flag=1; -// GV.PV.Robot_AutoSpeedBase = GV.PV.Robot_AutoSpeedBase + 1; -// } -// else if(P_U7->LU_pulley<= -900 && JJ_Flag==0) -// { -// JJ_Flag=1; -// GV.PV.Robot_AutoSpeedBase = GV.PV.Robot_AutoSpeedBase - 1; -// } -// else if(P_U7->LU_pulley<=100 && P_U7->LU_pulley>=-100 && JJ_Flag==1) -// { -// JJ_Flag=0; -// } -// if(GV.PV.Robot_AutoSpeedBase<=0) -// { -// GV.PV.Robot_AutoSpeedBase = 0; -// } - - //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的移动车体速度 这是整体车速 不是单个轮的 @@ -633,6 +453,7 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 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; @@ -646,7 +467,8 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值 IV.Robot_Error = 10000 + GV.LeftFrontMotor.ERROR_Flag * 100 + GV.RightFrontMotor.ERROR_Flag; } - + 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; diff --git a/Core/Src/main.c b/Core/Src/main.c index fa52063..2bc2f2a 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -264,7 +264,7 @@ void CV_GV_Init() P_U7 = &GV.U7_Key;//P_MK32 指向GV的遥控器参数 SP_MSP_RF_TL720D_Parameters_In = &GV.Robot_Angle ;// SP_MSP_RF_TL720D_Parameters_In 指向GV的�?�角 - ForceValue_in = &GV.Robot_ForceValue; + CMCUValue_in = &GV.Robot_ForceValue; DynamometerValue_in = &GV.Robot_DynamometerValue; diff --git a/Core/Src/robot_state.c b/Core/Src/robot_state.c index 50d5612..6c2f079 100644 --- a/Core/Src/robot_state.c +++ b/Core/Src/robot_state.c @@ -201,7 +201,7 @@ void Auto() //垂直 else if ((angle > -45 && angle < 45)) { - AM_Pa.ExpectationAngle = 0; + AM_Pa.ExpectationAngle = 0 - GV.PV.Robot_LeftCompensation + GV.PV.Robot_RightCompensation; } VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转 @@ -233,7 +233,7 @@ void Auto() //垂直 if ((angle > -45 && angle < 45)) { - AM_Pa.ExpectationAngle = 0; + AM_Pa.ExpectationAngle = 0 - GV.PV.Robot_LeftCompensation + GV.PV.Robot_RightCompensation; } VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转 @@ -512,7 +512,7 @@ void ChgLeft() { Chg_Pa.change_road_status_flag=1; GV.Chg_Flag = 0; - Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength-60; + Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength-10; WheelMotorSpeedInit(); } break; @@ -583,7 +583,7 @@ void ChgRight() { Chg_Pa.change_road_status_flag=1; GV.Chg_Flag = 0; - Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength-60; + Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength-10; WheelMotorSpeedInit(); } break;