MaRuihan
/
BHBF_SWING
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

20260312

master
L1NG\42961 8 hours ago
parent
3d9a99b742
commit
c7e009de0d
12 changed files with 161 additions and 86 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      .settings/language.settings.xml
  2. 2
      Core/BASE/Protobuf/PSource/bsp_GV.pb.h
  3. 14
      Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h
  4. 2
      Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto
  5. 2
      Core/FSM/Inc/robot_move_actions.h
  6. 4
      Core/FSM/Inc/swing_action.h
  7. 1
      Core/FSM/Src/Handset_Status_Setting.c
  8. 2
      Core/FSM/Src/fsm_state_control.c
  9. 5
      Core/FSM/Src/motor.c
  10. 150
      Core/FSM/Src/robot_move_actions.c
  11. 55
      Core/FSM/Src/swing_action.c
  12. 6
      疑问记录.txt

4
.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1562461444099266630" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true"> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1333973149273858338" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>
@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/> <provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1562461444099266630" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true"> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1333973149273858338" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/> <language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>

2
Core/BASE/Protobuf/PSource/bsp_GV.pb.h
View File

@ -116,7 +116,7 @@ extern const pb_msgdesc_t GV_struct_define_msg;
/* Maximum encoded size of messages (where known) */ /* Maximum encoded size of messages (where known) */
#define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size #define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size
#define GV_struct_define_size 1714 #define GV_struct_define_size 1768
#ifdef __cplusplus #ifdef __cplusplus
} /* extern "C" */ } /* extern "C" */

14
Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h
View File

@ -39,6 +39,8 @@ typedef struct _TT_MotorParameters {
int32_t Position_immediately1_Lag2; /* 位置环模式，立即生效1，延后生效2 */ int32_t Position_immediately1_Lag2; /* 位置环模式，立即生效1，延后生效2 */
int32_t Tar_Position_count; /* 位置环模式，期望位置 */ int32_t Tar_Position_count; /* 位置环模式，期望位置 */
int32_t Tar_Position_Velcity_RPM; /* 位置环模式，速度 */ int32_t Tar_Position_Velcity_RPM; /* 位置环模式，速度 */
float Tar_Position_angle; /* 位置环模式，期望角度 */
int32_t Tar_Position_Velcity_Degree_S; /* 位置环模式，速度(m/min) */
} TT_MotorParameters; } TT_MotorParameters;
@ -47,8 +49,8 @@ extern "C" {
#endif #endif
/* Initializer values for message structs */ /* Initializer values for message structs */
#define TT_MotorParameters_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #define TT_MotorParameters_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define TT_MotorParameters_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #define TT_MotorParameters_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
/* Field tags (for use in manual encoding/decoding) */ /* Field tags (for use in manual encoding/decoding) */
#define TT_MotorParameters_MotorID_tag 1 #define TT_MotorParameters_MotorID_tag 1
@ -79,6 +81,8 @@ extern "C" {
#define TT_MotorParameters_Position_immediately1_Lag2_tag 46 #define TT_MotorParameters_Position_immediately1_Lag2_tag 46
#define TT_MotorParameters_Tar_Position_count_tag 47 #define TT_MotorParameters_Tar_Position_count_tag 47
#define TT_MotorParameters_Tar_Position_Velcity_RPM_tag 49 #define TT_MotorParameters_Tar_Position_Velcity_RPM_tag 49
#define TT_MotorParameters_Tar_Position_angle_tag 50
#define TT_MotorParameters_Tar_Position_Velcity_Degree_S_tag 51
/* Struct field encoding specification for nanopb */ /* Struct field encoding specification for nanopb */
#define TT_MotorParameters_FIELDLIST(X, a) \ #define TT_MotorParameters_FIELDLIST(X, a) \
@ -109,7 +113,9 @@ X(a, STATIC, SINGULAR, DOUBLE, Real_Disatnce, 44) \
X(a, STATIC, SINGULAR, INT32, Start_Measuring, 45) \ X(a, STATIC, SINGULAR, INT32, Start_Measuring, 45) \
X(a, STATIC, SINGULAR, INT32, Position_immediately1_Lag2, 46) \ X(a, STATIC, SINGULAR, INT32, Position_immediately1_Lag2, 46) \
X(a, STATIC, SINGULAR, INT32, Tar_Position_count, 47) \ X(a, STATIC, SINGULAR, INT32, Tar_Position_count, 47) \
X(a, STATIC, SINGULAR, INT32, Tar_Position_Velcity_RPM, 49) X(a, STATIC, SINGULAR, INT32, Tar_Position_Velcity_RPM, 49) \
X(a, STATIC, SINGULAR, FLOAT, Tar_Position_angle, 50) \
X(a, STATIC, SINGULAR, INT32, Tar_Position_Velcity_Degree_S, 51)
#define TT_MotorParameters_CALLBACK NULL #define TT_MotorParameters_CALLBACK NULL
#define TT_MotorParameters_DEFAULT NULL #define TT_MotorParameters_DEFAULT NULL
@ -120,7 +126,7 @@ extern const pb_msgdesc_t TT_MotorParameters_msg;
/* Maximum encoded size of messages (where known) */ /* Maximum encoded size of messages (where known) */
#define MSP_ZQ_MOTORPARAMETERS_PB_H_MAX_SIZE TT_MotorParameters_size #define MSP_ZQ_MOTORPARAMETERS_PB_H_MAX_SIZE TT_MotorParameters_size
#define TT_MotorParameters_size 328 #define TT_MotorParameters_size 346
#ifdef __cplusplus #ifdef __cplusplus
} /* extern "C" */ } /* extern "C" */

2
Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto
View File

@ -31,6 +31,8 @@ message TT_MotorParameters {
int32 Tar_Position_Velcity_RPM=49; //位置环模式，速度 int32 Tar_Position_Velcity_RPM=49; //位置环模式，速度
double Real_Disatnce=44; double Real_Disatnce=44;
int32 Start_Measuring=45;//开始测距 int32 Start_Measuring=45;//开始测距
float Tar_Position_angle=50; //位置环模式，期望角度
int32 Tar_Position_Velcity_Degree_S=51; //位置环模式，速度(m/min)
} }

2
Core/FSM/Inc/robot_move_actions.h
View File

@ -23,7 +23,7 @@ void Turn_Left(void);
/* 状态机驱动函数（需外部调用） */ /* 状态机驱动函数（需外部调用） */
void Move_Horizontal_Auto_Sub_Func(void); /* 水平自动运行主状态机 */ void Move_Horizontal_Auto_Sub_Func(void); /* 水平自动运行主状态机 */
void Vertical_Left_Func(void); /* 垂直左换道状态机（注意：该函数实际使用换道状态表，需确认命名） */ void Change_Road_Func(void); /* 垂直左换道状态机（注意：该函数实际使用换道状态表，需确认命名） */
void horizontal_forward(void); /* 水平前进（PID控制） */ void horizontal_forward(void); /* 水平前进（PID控制） */
void vertical_forward(void); /* 垂直前进（PID控制） */ void vertical_forward(void); /* 垂直前进（PID控制） */

4
Core/FSM/Inc/swing_action.h
View File

@ -9,8 +9,8 @@
#define FSM_INC_SWING_ACTION_H_ #define FSM_INC_SWING_ACTION_H_
extern int limit_record; extern int limit_record;
extern int left_compare_value; extern float left_compare_value;
extern int right_compare_value; extern float right_compare_value;
extern void Robot_Swing_Operation_Function(); extern void Robot_Swing_Operation_Function();

1
Core/FSM/Src/Handset_Status_Setting.c
View File

@ -154,6 +154,7 @@ static InputEvent CalculateRockerEvent(void)
static InputEvent GetHaltModeEvents(void) static InputEvent GetHaltModeEvents(void)
{ {
InputEvent key = CheckAllKeys(); InputEvent key = CheckAllKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent(); return (key != INPUT_NONE) ? key : CalculateRockerEvent();
} }

2
Core/FSM/Src/fsm_state_control.c
View File

@ -161,7 +161,7 @@ static void horizontal_auto_group(void)
} }
static void change_Road_group(void) static void change_Road_group(void)
{ {
Vertical_Left_Func(); Change_Road_Func();
/* 若需喷枪控制，可在此添加 */ /* 若需喷枪控制，可在此添加 */
} }

5
Core/FSM/Src/motor.c
View File

@ -17,6 +17,9 @@ char TT_Motor_Need_To_Activate = 1;
char TT_Motor_Need_To_Activate_1 = 1; char TT_Motor_Need_To_Activate_1 = 1;
float Move_Base_Speed_Count_m_Min=201.7*7.64;//天太电机1m/min float Move_Base_Speed_Count_m_Min=201.7*7.64;//天太电机1m/min
float Swing_Speed_Deg_Sencond_motor=201.7;//HJ32-121
int middle_position_motor=-944334;
#define TT_One_Deg_Count_motor 11014///32768*121/360(减速比121)=11014
//MotorParameters *TT_Motor[7]; //MotorParameters *TT_Motor[7];
@ -120,6 +123,8 @@ void MotorCommandsLoop()
void MotorCommandsLoop_2_Position() void MotorCommandsLoop_2_Position()
{ {
GV.SwingMotor.Tar_Position_count=(int32_t)(middle_position_motor-GV.SwingMotor.Tar_Position_angle*TT_One_Deg_Count_motor);
GV.SwingMotor.Tar_Position_Velcity_RPM=GV.SwingMotor.Tar_Position_Velcity_Degree_S*Swing_Speed_Deg_Sencond_motor;
if (TT_Motor_Need_To_Activate_1 == 1) if (TT_Motor_Need_To_Activate_1 == 1)
{ {

150
Core/FSM/Src/robot_move_actions.c
View File

@ -20,6 +20,7 @@
/*=========================== 2.宏定义 ===========================*/ /*=========================== 2.宏定义 ===========================*/
/* 姿态控制常量（用于 Robot_Posture_Adjus_Gravity） */ /* 姿态控制常量（用于 Robot_Posture_Adjus_Gravity） */
#define TT_One_Deg_Count2 11014
#define POSTURE_ERROR_THRESHOLD 5.0f /* 姿态调整误差阈值（度） */ #define POSTURE_ERROR_THRESHOLD 5.0f /* 姿态调整误差阈值（度） */
#define ANGLE_NORMALIZE_BOUNDARY 180.0f /* 角度归一化边界 */ #define ANGLE_NORMALIZE_BOUNDARY 180.0f /* 角度归一化边界 */
#define SPEED_FACTOR_LARGE 1.0f /* 大误差速度系数 */ #define SPEED_FACTOR_LARGE 1.0f /* 大误差速度系数 */
@ -76,6 +77,9 @@ enum swing_state {
////局部静态变量 ////局部静态变量
/* 状态机当前状态（多个状态机共用，可能存在潜在冲突，但是现在还不知道咋搞呢） */ /* 状态机当前状态（多个状态机共用，可能存在潜在冲突，但是现在还不知道咋搞呢） */
int s_currentState = STATE_ATTITUDE_JUDGE; int s_currentState = STATE_ATTITUDE_JUDGE;
int horizontal_s_currentState = STATE_ATTITUDE_JUDGE;
int vertical_s_currentState = STATE_ATTITUDE_JUDGE;
int ChangeRoad_currentState = STATE_ATTITUDE_JUDGE;
int swing_currentState = SWING_START_STATE; int swing_currentState = SWING_START_STATE;
/* 换道计时变量 */ /* 换道计时变量 */
@ -87,12 +91,20 @@ static float LEFT_TARGET_ANGLES[2];
int Change_stop_flag=0; int Change_stop_flag=0;
static int S1_Last_Value;
static int S2_Last_Value;
int center_angle;
int offset_angle;
int center_position; //中间位置-944334
/*===========================5. 静态函数声明 ===========================*/ /*===========================5. 静态函数声明 ===========================*/
/* 水平/垂直状态机函数 */ /* 水平/垂直状态机函数 */
static void handleAttitudeJudge_horizontal(void); static void handleAttitudeJudge_horizontal(void);
static void handleAttitudeJudge_vertical(void); static void handleAttitudeJudge_vertical(void);
static void handleAttitudeAdjust(void); static void handleAttitudeAdjustHorizontal(void);
static void handleAttitudeAdjustVertical(void);
static void handleAttitudeAdjustChangeRoad(void);
static void handleWorkWay(void); static void handleWorkWay(void);
void horizontal_work(void); void horizontal_work(void);
@ -116,6 +128,7 @@ void swing_work(void);
static void swing_start_move(void); static void swing_start_move(void);
static void swing_left_move(void); static void swing_left_move(void);
static void swing_right_move(void); static void swing_right_move(void);
static void get_swing_mode(void);
/* PID系数 */ /* PID系数 */
@ -129,7 +142,7 @@ int factor_2= -1;
*/ */
static const void (*const horizontal_single_lane_auto_operation[])(void) = { static const void (*const horizontal_single_lane_auto_operation[])(void) = {
[STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_horizontal, [STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_horizontal,
[STATE_ATTITUDE_ADJUST] = handleAttitudeAdjust, [STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustHorizontal,
[STATE_WORK_WAY_OR_DIRECT_MOVE] = handleWorkWay, [STATE_WORK_WAY_OR_DIRECT_MOVE] = handleWorkWay,
[STATE_FIGHT_ALTERNATELY] = NULL, /* 待实现 */ [STATE_FIGHT_ALTERNATELY] = NULL, /* 待实现 */
[STATE_FIGHT_RETREATING] = Fight_Countinus_Function_Horizontal, /* 待实现 */ [STATE_FIGHT_RETREATING] = Fight_Countinus_Function_Horizontal, /* 待实现 */
@ -141,7 +154,7 @@ static const void (*const horizontal_single_lane_auto_operation[])(void) = {
*/ */
static const void (*const robot_horizontal_drive[])(void) = { static const void (*const robot_horizontal_drive[])(void) = {
[STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_horizontal, [STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_horizontal,
[STATE_ATTITUDE_ADJUST] = handleAttitudeAdjust, [STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustHorizontal,
[STATE_WORK_WAY_OR_DIRECT_MOVE] = auto_drive_pid_horizontal, [STATE_WORK_WAY_OR_DIRECT_MOVE] = auto_drive_pid_horizontal,
[STATE_HALT] = handleHalt, [STATE_HALT] = handleHalt,
}; };
@ -153,7 +166,7 @@ static const void (*const robot_horizontal_drive[])(void) = {
*/ */
static const void (*const robot_vertical_drive[])(void) = { static const void (*const robot_vertical_drive[])(void) = {
[STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_vertical, [STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_vertical,
[STATE_ATTITUDE_ADJUST] = handleAttitudeAdjust, [STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustVertical,
[STATE_WORK_WAY_OR_DIRECT_MOVE] = auto_drive_pid_vertical, [STATE_WORK_WAY_OR_DIRECT_MOVE] = auto_drive_pid_vertical,
[STATE_HALT] = handleHalt, [STATE_HALT] = handleHalt,
}; };
@ -163,10 +176,10 @@ static const void (*const robot_vertical_drive[])(void) = {
*/ */
static const void (*const vertical_lane_change_left[])(void) = { static const void (*const vertical_lane_change_left[])(void) = {
[LANE_CHANGE_STATE_ATTITUDE_JUDGE] = handleLaneChangeAttitudeJudgeLeft, [LANE_CHANGE_STATE_ATTITUDE_JUDGE] = handleLaneChangeAttitudeJudgeLeft,
[LANE_CHANGE_STATE_ATTITUDE_ADJUST] = handleAttitudeAdjust, [LANE_CHANGE_STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustChangeRoad,
[LANE_CHANGE_STATE_CALC_RETREAT_TIME] = handleLaneChangeCalcRetreatTime, [LANE_CHANGE_STATE_CALC_RETREAT_TIME] = handleLaneChangeCalcRetreatTime,
[LANE_CHANGE_STATE_CONTINUOUS_RETREAT] = handleLaneChangeContinuousRetreat, [LANE_CHANGE_STATE_CONTINUOUS_RETREAT] = handleLaneChangeContinuousRetreat,
[LANE_CHANGE_STATE_RETREAT_DONE_SWITCH] = handleAttitudeAdjust, [LANE_CHANGE_STATE_RETREAT_DONE_SWITCH] = handleAttitudeAdjustChangeRoad,
[STATE_HALT] = handleHalt, [STATE_HALT] = handleHalt,
}; };
@ -213,6 +226,7 @@ void Turn_Right(void)
void Robot_Stop(void) void Robot_Stop(void)
{ {
get_swing_mode();
Robot_Swing_Operation_Function(); Robot_Swing_Operation_Function();
GV.Left_Speed_M_min = 0; GV.Left_Speed_M_min = 0;
GV.Right_Speed_M_min = 0; GV.Right_Speed_M_min = 0;
@ -249,12 +263,31 @@ void Robot_Posture_Adjus_Gravity(float *pError)
} }
static void handleAttitudeAdjust(void) static void handleAttitudeAdjustHorizontal(void)
{
float errorAbs;
Robot_Posture_Adjus_Gravity(&errorAbs);
if (errorAbs < ANGLE_ERROR_THRESHOLD) {
horizontal_s_currentState++;
}
}
static void handleAttitudeAdjustVertical(void)
{
float errorAbs;
Robot_Posture_Adjus_Gravity(&errorAbs);
if (errorAbs < ANGLE_ERROR_THRESHOLD) {
vertical_s_currentState++;
}
}
static void handleAttitudeAdjustChangeRoad(void)
{ {
float errorAbs; float errorAbs;
Robot_Posture_Adjus_Gravity(&errorAbs); Robot_Posture_Adjus_Gravity(&errorAbs);
if (errorAbs < ANGLE_ERROR_THRESHOLD) { if (errorAbs < ANGLE_ERROR_THRESHOLD) {
s_currentState++; ChangeRoad_currentState++;
} }
} }
/*----------------------------------------------------------------- /*-----------------------------------------------------------------
@ -269,7 +302,7 @@ static void handleAttitudeJudge_horizontal(void)
} else { } else {
GV.Robot_Angle_Desire = 90.0f + (float)GV.Left_Compensation / 100.0f; GV.Robot_Angle_Desire = 90.0f + (float)GV.Left_Compensation / 100.0f;
} }
s_currentState = STATE_ATTITUDE_ADJUST; horizontal_s_currentState = STATE_ATTITUDE_ADJUST;
} }
static void handleAttitudeJudge_vertical(void) static void handleAttitudeJudge_vertical(void)
@ -284,18 +317,18 @@ static void handleAttitudeJudge_vertical(void)
GV.Robot_Angle_Desire -= 360.0f; GV.Robot_Angle_Desire -= 360.0f;
} }
} }
s_currentState = STATE_ATTITUDE_ADJUST; vertical_s_currentState = STATE_ATTITUDE_ADJUST;
} }
static void handleWorkWay(void) static void handleWorkWay(void)
{ {
int mode = (int)GV.PV.Robot_backMode; int mode = GV.PV.Robot_backMode;
if (mode == 1) { if (mode == 1) {
s_currentState = STATE_FIGHT_ALTERNATELY; horizontal_s_currentState = STATE_FIGHT_ALTERNATELY;
} else if (mode == 2) { } else if (mode == 2) {
s_currentState = STATE_FIGHT_RETREATING; horizontal_s_currentState = STATE_FIGHT_RETREATING;
} }
/* 其他模式保持当前状态？目前没有 else，可能默认留在该状态 */ /* 其他模式保持当前状态？目前没有 else，可能默认留在该状态 */
} }
@ -310,7 +343,7 @@ static void handleLaneChangeAttitudeJudgeLeft(void)
Change_stop_flag=0; Change_stop_flag=0;
get_ChangeAngle(); get_ChangeAngle();
GV.Robot_Angle_Desire = LEFT_TARGET_ANGLES[0]; GV.Robot_Angle_Desire = LEFT_TARGET_ANGLES[0];
s_currentState = LANE_CHANGE_STATE_ATTITUDE_ADJUST; ChangeRoad_currentState = LANE_CHANGE_STATE_ATTITUDE_ADJUST;
} }
} }
@ -345,16 +378,19 @@ void get_ChangeAngle(void)
} }
} }
float distanceM;
static void handleLaneChangeCalcRetreatTime(void) static void handleLaneChangeCalcRetreatTime(void)
{ {
float distanceM; //float distanceM;
if(GV.PV.Robot_Operation_Mode==2)//水平 if(GV.PV.Robot_Operation_Mode==2)//水平
{ {
distanceM = ((float)GV.LaneChangeDistance * 0.01f-B_OF_DIS_HOR)/K_OF_DIS_HOR; /* 厘米 → 米 */ distanceM = ((float)GV.LaneChangeDistance-B_OF_DIS_HOR)/K_OF_DIS_HOR; /* 厘米 → 米 */
distanceM=distanceM * 0.01f;
} }
else if(GV.PV.Robot_Operation_Mode==4||GV.PV.Robot_Operation_Mode==5) //竖直 else if(GV.PV.Robot_Operation_Mode==4||GV.PV.Robot_Operation_Mode==5) //竖直
{ {
distanceM = ((float)GV.LaneChangeDistance * 0.01f-B_OF_DIS_VER)/K_OF_DIS_VER; /* 厘米 → 米 */ distanceM = ((float)GV.LaneChangeDistance-B_OF_DIS_VER)/K_OF_DIS_VER; /* 厘米 → 米 */
distanceM=distanceM * 0.01f;
} }
float timeSec = distanceM / LANE_CHANGE_SPEED; /* 秒 */ float timeSec = distanceM / LANE_CHANGE_SPEED; /* 秒 */
float timeMs = timeSec * 60.0f * 1000.0f; /* 毫秒 */ float timeMs = timeSec * 60.0f * 1000.0f; /* 毫秒 */
@ -363,11 +399,11 @@ static void handleLaneChangeCalcRetreatTime(void)
uint32_t tickNow = HAL_GetTick(); uint32_t tickNow = HAL_GetTick();
s_retreatStartTick = tickNow; s_retreatStartTick = tickNow;
s_currentState = LANE_CHANGE_STATE_CONTINUOUS_RETREAT; ChangeRoad_currentState = LANE_CHANGE_STATE_CONTINUOUS_RETREAT;
if(P_MK32->CH4_SA == -1000&&Change_stop_flag==1) if(P_MK32->CH4_SA == -1000&&Change_stop_flag==1)
{ {
s_currentState = LANE_CHANGE_STATE_ATTITUDE_JUDGE; ChangeRoad_currentState = LANE_CHANGE_STATE_ATTITUDE_JUDGE;
} }
} }
@ -378,7 +414,7 @@ static void handleLaneChangeContinuousRetreat(void)
auto_drive_pid_vertical(); auto_drive_pid_vertical();
if ((tickNow - s_retreatStartTick) >= s_retreatDurationTicks) { if ((tickNow - s_retreatStartTick) >= s_retreatDurationTicks) {
s_currentState++; ChangeRoad_currentState++;
GV.Robot_Angle_Desire = LEFT_TARGET_ANGLES[1]; GV.Robot_Angle_Desire = LEFT_TARGET_ANGLES[1];
} }
} }
@ -404,10 +440,11 @@ void Move_Horizontal_Vertical_Task_Backwards_Do_Backward(void)
int present_angle;
static void swing_start_move(void) static void swing_start_move(void)
{ {
if(GV.SwingMotor.Real_Position<=left_compare_value-20000) present_angle=-(GV.SwingMotor.Real_Position+944334)/11014;
if(present_angle>=left_compare_value+2)
{ {
swing_currentState=SWING_LEFT_MOVE; swing_currentState=SWING_LEFT_MOVE;
Move_Swing_Left_Func_Do_imm(); Move_Swing_Left_Func_Do_imm();
@ -422,7 +459,8 @@ static void swing_start_move(void)
static void swing_left_move(void) static void swing_left_move(void)
{ {
if(GV.SwingMotor.Real_Position>left_compare_value-10000) present_angle=-(GV.SwingMotor.Real_Position+944334)/11014;
if(present_angle<=left_compare_value+1)
{ {
Move_Swing_Right_Func_Do_lay(); Move_Swing_Right_Func_Do_lay();
swing_currentState=SWING_RIGHT_MOVE; swing_currentState=SWING_RIGHT_MOVE;
@ -433,7 +471,8 @@ static void swing_left_move(void)
static void swing_right_move(void) static void swing_right_move(void)
{ {
if(GV.SwingMotor.Real_Position<right_compare_value+10000) present_angle=-(GV.SwingMotor.Real_Position+944334)/11014;
if(present_angle>right_compare_value-1)
{ {
Move_Swing_Left_Func_Do_lay(); Move_Swing_Left_Func_Do_lay();
swing_currentState=SWING_LEFT_MOVE; swing_currentState=SWING_LEFT_MOVE;
@ -441,6 +480,37 @@ static void swing_right_move(void)
} }
static void get_swing_mode(void)
{
if(GV.PV.Robot_symmetricalOrNot==1)
{
if(GV.PV.Robot_Swing_Range_Angle!=offset_angle)
{
offset_angle=GV.PV.Robot_Swing_Range_Angle;
center_angle=-(GV.SwingMotor.Real_Position+944334)/TT_One_Deg_Count2;
}
left_compare_value=center_angle-offset_angle/2;
right_compare_value=center_angle+offset_angle/2;
}
else if(GV.PV.Robot_symmetricalOrNot==2) //非对称式
{
if(P_MK32->CH12_S1!=S1_Last_Value)
{
left_compare_value=-(GV.SwingMotor.Real_Position+944334)/11014;
}
else if(P_MK32->CH13_S2!=S2_Last_Value)
{
right_compare_value=-(GV.SwingMotor.Real_Position+944334)/11014;
}
S1_Last_Value=P_MK32->CH12_S1;
S2_Last_Value=P_MK32->CH13_S2;
}
}
/*----------------------------------------------------------------- /*-----------------------------------------------------------------
* PID驱动及停机处理 * PID驱动及停机处理
*-----------------------------------------------------------------*/ *-----------------------------------------------------------------*/
@ -473,20 +543,23 @@ static void handleHalt(void)
GV.Right_Speed_M_min = 0; GV.Right_Speed_M_min = 0;
GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Target_Velcity=0; GV.SwingMotor.Target_Velcity=0;
horizontal_s_currentState = 0;
vertical_s_currentState = 0;
ChangeRoad_currentState = 0;
s_currentState = 0; s_currentState = 0;
} }
/*=========================== 8.对外接口函数（需外部调用） ===========================*/ /*=========================== 8.对外接口函数（需外部调用） ===========================*/
void Move_Horizontal_Auto_Init(void) void Move_Horizontal_Auto_Init(void)
{ {
s_currentState = STATE_ATTITUDE_JUDGE; horizontal_s_currentState = STATE_ATTITUDE_JUDGE;
} }
void Move_Horizontal_Auto_Sub_Func(void) void Move_Horizontal_Auto_Sub_Func(void)
{ {
if (s_currentState >= 0 && s_currentState < STATE_COUNT) { if (horizontal_s_currentState >= 0 && horizontal_s_currentState < STATE_COUNT) {
if (horizontal_single_lane_auto_operation[s_currentState] != NULL) { if (horizontal_single_lane_auto_operation[horizontal_s_currentState] != NULL) {
horizontal_single_lane_auto_operation[s_currentState](); horizontal_single_lane_auto_operation[horizontal_s_currentState]();
} else { } else {
/* 遇到未实现的状态，停机处理 */ /* 遇到未实现的状态，停机处理 */
handleHalt(); handleHalt();
@ -496,11 +569,11 @@ void Move_Horizontal_Auto_Sub_Func(void)
} }
} }
void Vertical_Left_Func(void) void Change_Road_Func(void)
{ {
/* 注意：此函数实际使用换道状态表，但命名可能引起混淆，建议后续改为垂直换道相关名称 */ /* 注意：此函数实际使用换道状态表，但命名可能引起混淆，建议后续改为垂直换道相关名称 */
if (s_currentState >= 0 && s_currentState < LANE_CHANGE_STATE_COUNT) { if (ChangeRoad_currentState >= 0 && ChangeRoad_currentState < LANE_CHANGE_STATE_COUNT) {
vertical_lane_change_left[s_currentState](); vertical_lane_change_left[ChangeRoad_currentState]();
} else { } else {
handleHalt(); handleHalt();
} }
@ -510,16 +583,16 @@ void Vertical_Left_Func(void)
void vertical_forward(void) void vertical_forward(void)
{ {
if (s_currentState >= 0 && s_currentState < STATE_COUNT) { if (vertical_s_currentState >= 0 && vertical_s_currentState < STATE_COUNT) {
robot_vertical_drive[s_currentState](); robot_vertical_drive[vertical_s_currentState]();
} else { } else {
handleHalt(); handleHalt();
} }
} }
void horizontal_forward(void) void horizontal_forward(void)
{ {
if (s_currentState >= 0 && s_currentState < STATE_COUNT) { if (horizontal_s_currentState >= 0 && horizontal_s_currentState < STATE_COUNT) {
robot_horizontal_drive[s_currentState](); robot_horizontal_drive[horizontal_s_currentState]();
} else { } else {
handleHalt(); handleHalt();
} }
@ -527,8 +600,8 @@ void horizontal_forward(void)
void horizontal_work(void) void horizontal_work(void)
{ {
if (s_currentState >= 0 && s_currentState < STATE_COUNT) { if (horizontal_s_currentState >= 0 && horizontal_s_currentState < STATE_COUNT) {
horizontal_single_lane_auto_operation[s_currentState](); horizontal_single_lane_auto_operation[horizontal_s_currentState]();
} else { } else {
handleHalt(); handleHalt();
} }
@ -552,6 +625,9 @@ void flag_reset(void)
&&P_MK32->CH3_LY_H==0) &&P_MK32->CH3_LY_H==0)
{ {
s_currentState = 0; s_currentState = 0;
horizontal_s_currentState = 0;
vertical_s_currentState = 0;
ChangeRoad_currentState = 0;
swing_currentState = 0; swing_currentState = 0;
} }

55
Core/FSM/Src/swing_action.c
View File

@ -16,13 +16,12 @@
#define RIGHT_LIMIT 2 #define RIGHT_LIMIT 2
float Swing_Speed_Deg_Sencond=201.7;//HJ32-121 float Swing_Speed_Deg_Sencond=201.7;//HJ32-121
int middle_position=-944334; int middle_position=-944334;
int center_angle; //int center_angle;
int offset_angle; //int offset_angle;
int present_angle; //int center_position; //中间位置-944334
int center_position; //中间位置-944334
int limit_record=0; int limit_record=0;
int left_compare_value; float left_compare_value;
int right_compare_value; float right_compare_value;
int lef_positon=-569580; int lef_positon=-569580;
int Right_positon=-1230420; int Right_positon=-1230420;
@ -32,22 +31,20 @@ void Robot_Swing_Operation_Function()
if(P_MK32->CH0_RY_H>300) if(P_MK32->CH0_RY_H>300)
{ {
GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Tar_Position_count=-944334-90*TT_One_Deg_Count; GV.SwingMotor.Tar_Position_angle=90;
GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed;
} }
else if(P_MK32->CH0_RY_H<-300) else if(P_MK32->CH0_RY_H<-300)
{ {
GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Tar_Position_count=-944334+90*TT_One_Deg_Count; GV.SwingMotor.Tar_Position_angle=-90;
GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed;
} }
else else
{ {
Move_Swing_Halt_Func_Do(); Move_Swing_Halt_Func_Do();
} }
/* *Swing_Speed = CV.PV.Swing_Speed;*/
// action_perfrom(Set_Swing_States,sizeof(Set_Swing_States) / sizeof(transition_t), CurrentSwingState);
} }
@ -56,8 +53,8 @@ void Robot_Swing_Operation_Function()
void Move_Swing_Left_Func_Do_lay(void) void Move_Swing_Left_Func_Do_lay(void)
{ {
GV.SwingMotor.Position_immediately1_Lag2=2; GV.SwingMotor.Position_immediately1_Lag2=2;
GV.SwingMotor.Tar_Position_count=left_compare_value; GV.SwingMotor.Tar_Position_angle=left_compare_value;
GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed;
} }
//摆臂电机右转延时生效 //摆臂电机右转延时生效
@ -65,24 +62,17 @@ void Move_Swing_Right_Func_Do_lay(void)
{ {
GV.SwingMotor.Position_immediately1_Lag2=2; GV.SwingMotor.Position_immediately1_Lag2=2;
GV.SwingMotor.Tar_Position_count=right_compare_value; GV.SwingMotor.Tar_Position_angle=right_compare_value;
GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed;
} }
//摆臂电机左转立即执行 //摆臂电机左转立即执行
void Move_Swing_Left_Func_Do_imm(void) void Move_Swing_Left_Func_Do_imm(void)
{ {
if(GV.PV.Robot_Swing_Range_Angle!=center_angle)
{
center_angle=GV.PV.Robot_Swing_Range_Angle;
center_position=GV.SwingMotor.Real_Position;
}
left_compare_value=center_position+(center_angle*TT_One_Deg_Count/2);
right_compare_value=center_position-(center_angle*TT_One_Deg_Count/2);
GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Tar_Position_count=left_compare_value; GV.SwingMotor.Tar_Position_angle=left_compare_value;
GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed;
} }
@ -90,18 +80,10 @@ void Move_Swing_Left_Func_Do_imm(void)
//摆臂电机右转立即执行 //摆臂电机右转立即执行
void Move_Swing_Right_Func_Do_imm(void) void Move_Swing_Right_Func_Do_imm(void)
{ {
if(GV.PV.Robot_Swing_Range_Angle!=offset_angle)
{
offset_angle=GV.PV.Robot_Swing_Range_Angle;
present_angle=-(GV.SwingMotor.Real_Position-middle_position)/TT_One_Deg_Count;
center_angle=GV.SwingMotor.Real_Position;
}
left_compare_value=center_position+(center_angle*TT_One_Deg_Count/2);
right_compare_value=center_position-(center_angle*TT_One_Deg_Count/2);
GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Tar_Position_count=right_compare_value; GV.SwingMotor.Tar_Position_angle=right_compare_value;
GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed;
} }
@ -109,7 +91,6 @@ void Move_Swing_Halt_Func_Do(void)
{ {
GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Tar_Position_count=0; GV.SwingMotor.Tar_Position_count=0;
GV.SwingMotor.Tar_Position_Velcity_RPM=0; GV.SwingMotor.Tar_Position_Velcity_Degree_S=0;
// GV.SwingMotor.Target_Velcity = 0;
} }

6
疑问记录.txt
View File

@ -44,4 +44,8 @@ GV_Robot_backMode=(int)CV.PV.Robot_backMode;
绑定电机 测距 调试摆臂 调内部PID 绑定电机 测距 调试摆臂 调内部PID
问题：摆臂CAN解析√ 变量的传导 报错信息 焊缝跟踪的验证√ 换道精度（最小二乘拟合）√ EPRROM的CV的配置 自动巡航√ 非对称摆臂 问题：摆臂CAN解析√ 变量的传导√ 报错信息 焊缝跟踪的验证√ 换道精度（最小二乘拟合）√ EPRROM的CV的配置 自动巡航√ 非对称摆臂 实现急停两个按键
变量的传导 用不同的标志位

Write Preview
Loading…
Cancel
Save