From 4305d7e8954884c32574a59b78fd2a86cfda631f Mon Sep 17 00:00:00 2001
From: Lizongdi <1210855344@qq.com>
Date: Mon, 15 Jun 2026 16:56:33 +0800
Subject: [PATCH] =?UTF-8?q?=E3=80=90=E7=BC=96=E8=AF=91=E9=80=9A=E8=BF=87?=
=?UTF-8?q?=E3=80=91=E5=88=80=E5=85=B7=E6=AD=A5=E8=BF=9B=E8=B7=9D=E7=A6=BB?=
=?UTF-8?q?=E6=94=B9=E6=88=900.1mm=EF=BC=8C=E8=BD=A6=E9=80=9F=E5=A2=9E?=
=?UTF-8?q?=E5=8A=A0=E4=BA=86=E5=90=91=E5=90=8E=E6=8B=A8SA=E4=B8=A4?=
=?UTF-8?q?=E5=80=8D=E9=80=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../.settings/language.settings.xml | 4 +-
.../BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h | 1 +
.../Core/Inc/robot_state.h | 1 +
.../BHBF_Robot_Lifting_Lug/Core/Src/FSM.c | 5 +
.../Core/Src/robot_state.c | 94 ++++++++++++++++++-
.../src/main/res/layout/activity_polish.xml | 2 +-
6 files changed, 103 insertions(+), 4 deletions(-)
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml b/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml
index dbe0709..f516fb3 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/Inc/FSM.h b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h
index c72102f..a302573 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h
@@ -15,6 +15,7 @@ typedef enum _MoveSTATE_t
Move_HALT=0,
Manual_State,
low_Speed_Manual_State,
+ high_Speed_Manual_State,
AUTO_FORWARD,
AUTO_BACKWARD
} MoveSTATE_t;
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/robot_state.h b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/robot_state.h
index 2c776ce..93fdd0e 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/robot_state.h
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/robot_state.h
@@ -17,6 +17,7 @@ extern double Act_Speed;
// ********机器人本体***********
extern void Manual_State_Do(void);
extern void low_Speed_Manual_State_Do(void);
+extern void high_Speed_Manual_State_Do(void);
extern void HALT_State_Do(void);
extern void auto_forward_state_do(void);
extern void auto_backward_state_do(void);
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
index ba52fad..3a682c9 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
@@ -35,6 +35,7 @@ transition_t MoveTransitions[] =
{ Move_HALT, HALT_State_Do },
{ Manual_State, Manual_State_Do },
{ low_Speed_Manual_State, low_Speed_Manual_State_Do},
+ { high_Speed_Manual_State, high_Speed_Manual_State_Do},
{ AUTO_FORWARD, auto_forward_state_do},
{ AUTO_BACKWARD, auto_backward_state_do}
};
@@ -583,6 +584,10 @@ void Robot_Control()
{
CurrentMoveState = low_Speed_Manual_State;
}
+ else
+ {
+ CurrentMoveState = high_Speed_Manual_State;
+ }
}
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 5e1f638..732b328 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
@@ -244,6 +244,98 @@ void low_Speed_Manual_State_Do(void)
GV.RightBackMotor.Target_Velcity = R2_Speed_Con * Ref_Speed;
}
+void high_Speed_Manual_State_Do(void)
+{
+ double Rocker_Range=2000; //摇杆范围
+ double X_Value_1=0;
+ double Y_Value_1=0;
+ double X_Value_2=0;
+ double Y_Value_2=0;
+
+ //右前电机
+ double R1_Speed_Con=0;
+ //左前电机
+ double L1_Speed_Con=0;
+ //右后电机
+ double R2_Speed_Con=0;
+ //左后电机
+ double L2_Speed_Con=0;
+
+ //CH0_RY_H [-1000,1000]
+ X_Value_1=GV.MK32_Key.CH3_LY_H+0.00001;
+ //CH1_RY_V [-1000,1000]
+ Y_Value_1=GV.MK32_Key.CH2_LY_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);
+
+ if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 1000))
+ {
+ speedAdj = -995.0f + (GV.MK32_Key.CH10_LD1 + 1000.0f) * 0.005f;
+ Speed_Ctrl = ((speedAdj + 1000) * 360 / 1000);
+ }
+
+ Ref_Speed = Speed_Ctrl;
+ Act_Speed = 2*Speed_Ctrl;
+ if(Ref_Speed >= 5000) Ref_Speed = 5000;
+
+ if(fabs(Rocker_angle) > PI*0.35)
+ {
+ //前后行驶
+ if(GV.MK32_Key.CH2_LY_V > 0)
+ {
+ //前进
+ R1_Speed_Con = -1;
+ L1_Speed_Con = 1;
+ R2_Speed_Con = -1;
+ L2_Speed_Con = 1;
+ }
+ else if(GV.MK32_Key.CH2_LY_V < 0)
+ {
+ //后退
+ R1_Speed_Con = 1;
+ L1_Speed_Con = -1;
+ R2_Speed_Con = 1;
+ L2_Speed_Con = -1;
+ }
+ }
+ else if(fabs(Rocker_angle) < PI*0.15)
+ {
+ if(GV.MK32_Key.CH3_LY_H > 0)
+ {
+
+ //右转前行
+ R1_Speed_Con = -0.35;
+ L1_Speed_Con = 1;
+ R2_Speed_Con = -0.35;
+ L2_Speed_Con = 1;
+ }
+ else if(GV.MK32_Key.CH3_LY_H <= 0)
+ {
+ //左转前行
+ R1_Speed_Con = -1;
+ L1_Speed_Con = 0.35;
+ R2_Speed_Con = -1;
+ L2_Speed_Con = 0.35;
+ }
+ }
+ else
+ {
+ R1_Speed_Con = 0;
+ L1_Speed_Con = 0;
+ R2_Speed_Con = 0;
+ L2_Speed_Con = 0;
+ }
+ GV.LeftFrontMotor.Target_Velcity = L1_Speed_Con * Ref_Speed;
+ GV.RightFrontMotor.Target_Velcity = R1_Speed_Con * Ref_Speed;
+ GV.LeftBackMotor.Target_Velcity = L2_Speed_Con * Ref_Speed;
+ GV.RightBackMotor.Target_Velcity = R2_Speed_Con * Ref_Speed;
+}
+
+
void HALT_State_Do(void)
{
GV.LeftFrontMotor.Target_Velcity = 0;
@@ -367,7 +459,7 @@ void Manual_low_speed_Down_State_Do(void)
LimitDownPosition();
}
-#define STEP_LEN_COUNT 20000 // 位置环每20000个count对应1mm
+#define STEP_LEN_COUNT 2000 // 位置环每2000个count对应0.1mm
void Manual_step_Up_State_Do(void)
{
diff --git a/diaoerqiege/diaoerqige_V1.0/app/src/main/res/layout/activity_polish.xml b/diaoerqiege/diaoerqige_V1.0/app/src/main/res/layout/activity_polish.xml
index 4292cfa..3055cd5 100644
--- a/diaoerqiege/diaoerqige_V1.0/app/src/main/res/layout/activity_polish.xml
+++ b/diaoerqiege/diaoerqige_V1.0/app/src/main/res/layout/activity_polish.xml
@@ -683,7 +683,7 @@
android:layout_height="match_parent"
android:layout_weight="1"
android:gravity="center"
- android:text="mm"
+ android:text="10⁻¹ mm"
android:textColor="@color/dark_ink_blue"
android:textSize="15dp" />