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

更换压力传感器cmcu；水平换道补偿从60改成了10

master
LIN\54376 2 months ago
parent
cbefc6df64
commit
0bd734d0f5
9 changed files with 67 additions and 235 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. 2
      BASE/Inc/BSP/BHBF_ROBOT.h
  2. 10
      Core/Inc/FSM.h
  3. 14
      Core/Protobuf/PSource/bsp_IV.pb.h
  4. 30
      Core/Protobuf/PSource/msp_U7.pb.h
  5. 4
      Core/Protobuf/Proto/bsp_IV.proto
  6. 10
      Core/Protobuf/Proto/msp_U7.proto
  7. 222
      Core/Src/FSM.c
  8. 2
      Core/Src/main.c
  9. 8
      Core/Src/robot_state.c

2
BASE/Inc/BSP/BHBF_ROBOT.h
View File

@ -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"

10
Core/Inc/FSM.h
View File

@ -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

14
Core/Protobuf/PSource/bsp_IV.pb.h
View File

@ -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" */

30
Core/Protobuf/PSource/msp_U7.pb.h
View File

@ -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)

4
Core/Protobuf/Proto/bsp_IV.proto
View File

@ -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;
};

10
Core/Protobuf/Proto/msp_U7.proto
View File

@ -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;

222
Core/Src/FSM.c
View File

@ -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;

2
Core/Src/main.c
View File

@ -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;

8
Core/Src/robot_state.c
View File

@ -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;

Write Preview
Loading…
Cancel
Save