From 815931fa417c616748e0e56f4b44ecc0f967c786 Mon Sep 17 00:00:00 2001
From: "HJB\\13752" <13752551070@163.com>
Date: Mon, 30 Mar 2026 14:00:23 +0800
Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9EIV=E3=80=81PV?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../.settings/language.settings.xml | 4 +-
.../BASE/Src/MSP/msp_zhr29_laser_sensor.c | 48 ++++++-------
.../Core/Protobuf/PSource/bsp_IV.pb.h | 25 ++++---
.../Core/Protobuf/PSource/bsp_PV.pb.h | 6 +-
.../Core/Protobuf/Proto/bsp_IV.proto | 9 ++-
.../Core/Protobuf/Proto/bsp_PV.proto | 2 +-
.../BHBF_Robot_Lifting_Lug/Core/Src/FSM.c | 8 ++-
.../Core/Src/robot_state.c | 72 -------------------
diaoerqiege/雷赛CAN指令.txt | 47 ++++++++++++
9 files changed, 99 insertions(+), 122 deletions(-)
create mode 100644 diaoerqiege/雷赛CAN指令.txt
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml b/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml
index 3f35568..e5e306c 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_zhr29_laser_sensor.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_zhr29_laser_sensor.c
index a95b7bc..9d8d430 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_zhr29_laser_sensor.c
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_zhr29_laser_sensor.c
@@ -89,41 +89,41 @@ void decode_laser_sensor(uint8_t *buffer, uint16_t length)
if((buffer[0] == 0x01) && (buffer[1] == 0x04) && (buffer[2] == 0x04))
{
*g_zhr29_200_laser_sensor_1 = ((buffer[3] << 24) | (buffer[4] << 16) | (buffer[5] << 8) | buffer[6]) / 1000;
- if(*g_zhr29_200_laser_sensor_1 >= 280)
- {
- *g_zhr29_200_laser_sensor_1 = 280;
- }
- if(*g_zhr29_200_laser_sensor_1 <= 120)
- {
- *g_zhr29_200_laser_sensor_1 = 120;
- }
+// if(*g_zhr29_200_laser_sensor_1 >= 280)
+// {
+// *g_zhr29_200_laser_sensor_1 = 280;
+// }
+// if(*g_zhr29_200_laser_sensor_1 <= 120)
+// {
+// *g_zhr29_200_laser_sensor_1 = 120;
+// }
}
else if((buffer[0] == 0x02) && (buffer[1] == 0x04) && (buffer[2] == 0x04))
{
*g_zhr29_200_laser_sensor_2 = ((buffer[3] << 24) | (buffer[4] << 16) | (buffer[5] << 8) | buffer[6]) / 1000;
- if(*g_zhr29_200_laser_sensor_2 >= 280)
- {
- *g_zhr29_200_laser_sensor_2 = 280;
- }
- if(*g_zhr29_200_laser_sensor_2 <= 120)
- {
- *g_zhr29_200_laser_sensor_2 = 120;
- }
+// if(*g_zhr29_200_laser_sensor_2 >= 280)
+// {
+// *g_zhr29_200_laser_sensor_2 = 280;
+// }
+// if(*g_zhr29_200_laser_sensor_2 <= 120)
+// {
+// *g_zhr29_200_laser_sensor_2 = 120;
+// }
}
else if((buffer[0] == 0x03) && (buffer[1] == 0x04) && (buffer[2] == 0x04))
{
*g_zhr29_200_laser_sensor_3 = ((buffer[3] << 24) | (buffer[4] << 16) | (buffer[5] << 8) | buffer[6]) / 1000;
- if(*g_zhr29_200_laser_sensor_3 >= 280)
- {
- *g_zhr29_200_laser_sensor_3 = 280;
- }
- if(*g_zhr29_200_laser_sensor_3 <= 120)
- {
- *g_zhr29_200_laser_sensor_3 = 120;
- }
+// if(*g_zhr29_200_laser_sensor_3 >= 280)
+// {
+// *g_zhr29_200_laser_sensor_3 = 280;
+// }
+// if(*g_zhr29_200_laser_sensor_3 <= 120)
+// {
+// *g_zhr29_200_laser_sensor_3 = 120;
+// }
}
else{
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_IV.pb.h b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_IV.pb.h
index f4c5f8a..32aab55 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_IV.pb.h
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_IV.pb.h
@@ -11,12 +11,11 @@
/* Struct definitions */
typedef struct _IV_struct_define {
- /* 洗舱项目 */
double Robot_Move_Speed;
- int32_t Robot_Swing_Speed;
- double Pit_time;
- double Rot_time;
int32_t RF_Angle_Roll;
+ int32_t laser_sensor_1_measure_distance;
+ int32_t laser_sensor_2_measure_distance;
+ int32_t laser_sensor_3_measure_distance;
} IV_struct_define;
@@ -30,18 +29,18 @@ extern "C" {
/* Field tags (for use in manual encoding/decoding) */
#define IV_struct_define_Robot_Move_Speed_tag 1
-#define IV_struct_define_Robot_Swing_Speed_tag 2
-#define IV_struct_define_Pit_time_tag 3
-#define IV_struct_define_Rot_time_tag 4
-#define IV_struct_define_RF_Angle_Roll_tag 5
+#define IV_struct_define_RF_Angle_Roll_tag 2
+#define IV_struct_define_laser_sensor_1_measure_distance_tag 3
+#define IV_struct_define_laser_sensor_2_measure_distance_tag 4
+#define IV_struct_define_laser_sensor_3_measure_distance_tag 5
/* Struct field encoding specification for nanopb */
#define IV_struct_define_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, DOUBLE, Robot_Move_Speed, 1) \
-X(a, STATIC, SINGULAR, INT32, Robot_Swing_Speed, 2) \
-X(a, STATIC, SINGULAR, DOUBLE, Pit_time, 3) \
-X(a, STATIC, SINGULAR, DOUBLE, Rot_time, 4) \
-X(a, STATIC, SINGULAR, INT32, RF_Angle_Roll, 5)
+X(a, STATIC, SINGULAR, INT32, RF_Angle_Roll, 2) \
+X(a, STATIC, SINGULAR, INT32, laser_sensor_1_measure_distance, 3) \
+X(a, STATIC, SINGULAR, INT32, laser_sensor_2_measure_distance, 4) \
+X(a, STATIC, SINGULAR, INT32, laser_sensor_3_measure_distance, 5)
#define IV_struct_define_CALLBACK NULL
#define IV_struct_define_DEFAULT NULL
@@ -52,7 +51,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 49
+#define IV_struct_define_size 53
#ifdef __cplusplus
} /* extern "C" */
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_PV.pb.h b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_PV.pb.h
index 3e94f5f..89630a4 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_PV.pb.h
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_PV.pb.h
@@ -11,7 +11,7 @@
/* Struct definitions */
typedef struct _PV_struct_define {
- int32_t Robot_Tilt_TargetCurrent;
+ int32_t knife_descending_height;
} PV_struct_define;
@@ -24,11 +24,11 @@ extern "C" {
#define PV_struct_define_init_zero {0}
/* Field tags (for use in manual encoding/decoding) */
-#define PV_struct_define_Robot_Tilt_TargetCurrent_tag 1
+#define PV_struct_define_knife_descending_height_tag 1
/* Struct field encoding specification for nanopb */
#define PV_struct_define_FIELDLIST(X, a) \
-X(a, STATIC, SINGULAR, INT32, Robot_Tilt_TargetCurrent, 1)
+X(a, STATIC, SINGULAR, INT32, knife_descending_height, 1)
#define PV_struct_define_CALLBACK NULL
#define PV_struct_define_DEFAULT NULL
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_IV.proto b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_IV.proto
index 9bcb93f..22872d5 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_IV.proto
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_IV.proto
@@ -5,11 +5,10 @@ syntax = "proto3";
message IV_struct_define{
- // 洗舱项目
double Robot_Move_Speed= 1;
- int32 Robot_Swing_Speed= 2;
- double Pit_time = 3;
- double Rot_time = 4;
- int32 RF_Angle_Roll = 5;
+ int32 RF_Angle_Roll = 2;
+ int32 laser_sensor_1_measure_distance = 3;
+ int32 laser_sensor_2_measure_distance = 4;
+ int32 laser_sensor_3_measure_distance = 5;
};
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_PV.proto b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_PV.proto
index 65a479e..98b9fdb 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_PV.proto
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_PV.proto
@@ -1,5 +1,5 @@
syntax = "proto3";
message PV_struct_define{
- int32 Robot_Tilt_TargetCurrent= 1;
+ int32 knife_descending_height = 1;
};
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
index d0ffd51..3aec5c4 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
@@ -32,9 +32,10 @@ static int index_counter = 0;
transition_t MoveTransitions[] =
{
+ { Move_HALT, HALT_State_Do },
{ Manual_State, Manual_State_Do },
{ low_Speed_Manual_State, low_Speed_Manual_State_Do},
- { Move_HALT, HALT_State_Do },
+
};
transition_t FrontendMoveTransitions[] =
@@ -92,7 +93,7 @@ void Strong_Grinding_Machine_Control()
IsAllowRotation = Knife_Detection();
if(IsAllowRotation == 1)
{
- if(GV.MK32_Key.CH7_SD == -1000)
+ if(GV.MK32_Key.CH6_SC == -1000)
{
StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_MOTION_STATE;
}
@@ -180,6 +181,9 @@ void IV_Control()
real_speed = (Act_Speed*360/100/101/6*3.1415926*0.325);
IV.Robot_Move_Speed = real_speed;
IV.RF_Angle_Roll = SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll;
+ IV.laser_sensor_1_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance;
+ IV.laser_sensor_2_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance;
+ IV.laser_sensor_3_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance;
}
void Robot_Control()
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
index f86c9dd..8611b01 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
@@ -271,78 +271,6 @@ void Strong_Grinding_Machine_Motion_State_Do(void)
}
-//void Manual_Up_Down_State_Do()
-//{
-// double Rocker_Range=2000; //摇杆范围
-// double X_Value_1=0;
-// double Y_Value_1=0;
-// double X_Value_2=0;
-// double Y_Value_2=0;
-//
-// // 前端移动速度，单位mm/s
-// int32_t SliderSpeed = 10;
-//
-// X_Value_1=GV.MK32_Key.CH0_RY_H+0.00001;
-// //CH1_RY_V [-1000,1000]
-// Y_Value_1=GV.MK32_Key.CH1_RY_V+0.00001;
-// //归一化
-// X_Value_2=X_Value_1/Rocker_Range;
-// Y_Value_2=Y_Value_1/Rocker_Range;
-//
-// //摇杆角度
-// double Rocker_angle = atan(Y_Value_2 / X_Value_2);
-//
-//
-// //int32_t SliderSpeed_PPS = SliderSpeed_mmps_2_pps(SliderSpeed);
-//
-//
-//// if(fabs(Rocker_angle) > PI*0.35)
-//// {
-//// if(GV.MK32_Key.CH1_RY_V > 0)
-//// {
-//// //前进
-//// SlideMotionSetPosition(-10000000,SliderSpeed_PPS);
-////
-//// }
-//// else if(GV.MK32_Key.CH1_RY_V < 0)
-//// {
-//// //后退
-//// SlideMotionSetPosition(10000000,SliderSpeed_PPS);
-//// }
-//// }
-//// else if(fabs(Rocker_angle) < PI*0.15)
-//// {
-//// if(GV.MK32_Key.CH0_RY_H > 0)
-//// {
-////
-//// //右转前行
-////
-//// }
-//// else if(GV.MK32_Key.CH0_RY_H <= 0)
-//// {
-//// //左转前行
-////
-//// }
-//// }
-//// else
-//// {
-//// SlideMotionSetPosition(10000000,0);
-//// }
-//
-//
-//}
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/diaoerqiege/雷赛CAN指令.txt b/diaoerqiege/雷赛CAN指令.txt
new file mode 100644
index 0000000..0e0f735
--- /dev/null
+++ b/diaoerqiege/雷赛CAN指令.txt
@@ -0,0 +1,47 @@
+CAN指令
+
+81 00 00 00 00 00 00 00 复位所有节点
+
+01 00 00 00 00 00 00 00 启动所有节点的远程控制
+
+
+步骤1（复位节点）和步骤2（启动节点）的帧ID是“0x0000”，其余的步骤的帧ID为SDO的地址0X0600+节点号
+
+
+2b 40 60 00 06 00 00 00 写控制字06H，状态机切换状态
+
+2b 40 60 00 07 00 00 00 写控制字07H，状态机切换状态
+
+2b 40 60 00 0f 00 00 00 写控制字0FH，状态机切换状态
+
+
+一、速度模式发以下指令
+
+2f 60 60 00 03 00 00 00 速度模式
+
+23 ff 60 00 A0 86 01 00 正转
+
+23 ff 60 00 60 79 FE FF 反转
+
+23 ff 60 00 00 00 00 00 停车
+
+
+
+
+二、位置模式发以下指令
+2f 60 60 00 01 00 00 00 位置模式
+
+23 81 60 00 A0 86 01 00 写速度
+
+23 7a 60 00 A0 86 01 00 正转10圈（5mm）
+
+23 7a 60 00 60 79 FE FF 反转10圈（5mm）
+
+23 7a 60 00 C0 D4 01 00 正转12圈（6mm）
+
+23 7a 60 00 40 2B FE FF 反转12圈（6mm）
+
+2b 40 60 00 4f 00 00 00 设置为相对运动模式
+
+2b 40 60 00 5f 00 00 00 电机开始运动
+