From c7e009de0de5ea7c450a175b7fa8780f583294d4 Mon Sep 17 00:00:00 2001 From: "L1NG\\42961" <429616286@qq.com> Date: Thu, 12 Mar 2026 16:21:21 +0800 Subject: [PATCH] 20260312 --- .settings/language.settings.xml | 4 +- Core/BASE/Protobuf/PSource/bsp_GV.pb.h | 2 +- .../PSource/msp_ZQ_MotorParameters.pb.h | 14 +- .../Proto/msp_ZQ_MotorParameters.proto | 2 + Core/FSM/Inc/robot_move_actions.h | 2 +- Core/FSM/Inc/swing_action.h | 4 +- Core/FSM/Src/Handset_Status_Setting.c | 1 + Core/FSM/Src/fsm_state_control.c | 2 +- Core/FSM/Src/motor.c | 5 + Core/FSM/Src/robot_move_actions.c | 150 +++++++++++++----- Core/FSM/Src/swing_action.c | 55 +++---- 疑问记录.txt | 6 +- 12 files changed, 161 insertions(+), 86 deletions(-) diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index 11ae67e..d197bab 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h index d2ad31f..7150ff8 100644 --- a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h @@ -116,7 +116,7 @@ extern const pb_msgdesc_t GV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #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 } /* extern "C" */ diff --git a/Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h b/Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h index 83f036a..3dc86ed 100644 --- a/Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h +++ b/Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h @@ -39,6 +39,8 @@ typedef struct _TT_MotorParameters { int32_t Position_immediately1_Lag2; /* 位置环模式，立即生效1，延后生效2 */ int32_t Tar_Position_count; /* 位置环模式，期望位置 */ int32_t Tar_Position_Velcity_RPM; /* 位置环模式，速度 */ + float Tar_Position_angle; /* 位置环模式，期望角度 */ + int32_t Tar_Position_Velcity_Degree_S; /* 位置环模式，速度(m/min) */ } TT_MotorParameters; @@ -47,8 +49,8 @@ extern "C" { #endif /* 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_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_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, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define TT_MotorParameters_MotorID_tag 1 @@ -79,6 +81,8 @@ extern "C" { #define TT_MotorParameters_Position_immediately1_Lag2_tag 46 #define TT_MotorParameters_Tar_Position_count_tag 47 #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 */ #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, Position_immediately1_Lag2, 46) \ 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_DEFAULT NULL @@ -120,7 +126,7 @@ extern const pb_msgdesc_t TT_MotorParameters_msg; /* Maximum encoded size of messages (where known) */ #define MSP_ZQ_MOTORPARAMETERS_PB_H_MAX_SIZE TT_MotorParameters_size -#define TT_MotorParameters_size 328 +#define TT_MotorParameters_size 346 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto b/Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto index 3895bb4..d97761f 100644 --- a/Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto +++ b/Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto @@ -31,6 +31,8 @@ message TT_MotorParameters { int32 Tar_Position_Velcity_RPM=49; //位置环模式，速度 double Real_Disatnce=44; int32 Start_Measuring=45;//开始测距 + float Tar_Position_angle=50; //位置环模式，期望角度 + int32 Tar_Position_Velcity_Degree_S=51; //位置环模式，速度(m/min) } diff --git a/Core/FSM/Inc/robot_move_actions.h b/Core/FSM/Inc/robot_move_actions.h index 0df97fd..b610897 100644 --- a/Core/FSM/Inc/robot_move_actions.h +++ b/Core/FSM/Inc/robot_move_actions.h @@ -23,7 +23,7 @@ void Turn_Left(void); /* 状态机驱动函数（需外部调用） */ void Move_Horizontal_Auto_Sub_Func(void); /* 水平自动运行主状态机 */ -void Vertical_Left_Func(void); /* 垂直左换道状态机（注意：该函数实际使用换道状态表，需确认命名） */ +void Change_Road_Func(void); /* 垂直左换道状态机（注意：该函数实际使用换道状态表，需确认命名） */ void horizontal_forward(void); /* 水平前进（PID控制） */ void vertical_forward(void); /* 垂直前进（PID控制） */ diff --git a/Core/FSM/Inc/swing_action.h b/Core/FSM/Inc/swing_action.h index 142ef3b..1134ab8 100644 --- a/Core/FSM/Inc/swing_action.h +++ b/Core/FSM/Inc/swing_action.h @@ -9,8 +9,8 @@ #define FSM_INC_SWING_ACTION_H_ extern int limit_record; -extern int left_compare_value; -extern int right_compare_value; +extern float left_compare_value; +extern float right_compare_value; extern void Robot_Swing_Operation_Function(); diff --git a/Core/FSM/Src/Handset_Status_Setting.c b/Core/FSM/Src/Handset_Status_Setting.c index 92784ca..f46a35b 100644 --- a/Core/FSM/Src/Handset_Status_Setting.c +++ b/Core/FSM/Src/Handset_Status_Setting.c @@ -154,6 +154,7 @@ static InputEvent CalculateRockerEvent(void) static InputEvent GetHaltModeEvents(void) { + InputEvent key = CheckAllKeys(); return (key != INPUT_NONE) ? key : CalculateRockerEvent(); } diff --git a/Core/FSM/Src/fsm_state_control.c b/Core/FSM/Src/fsm_state_control.c index 3051c2d..18400e9 100644 --- a/Core/FSM/Src/fsm_state_control.c +++ b/Core/FSM/Src/fsm_state_control.c @@ -161,7 +161,7 @@ static void horizontal_auto_group(void) } static void change_Road_group(void) { - Vertical_Left_Func(); + Change_Road_Func(); /* 若需喷枪控制，可在此添加 */ } diff --git a/Core/FSM/Src/motor.c b/Core/FSM/Src/motor.c index 8629bb3..2a6d0f5 100644 --- a/Core/FSM/Src/motor.c +++ b/Core/FSM/Src/motor.c @@ -17,6 +17,9 @@ char TT_Motor_Need_To_Activate = 1; char TT_Motor_Need_To_Activate_1 = 1; 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]; @@ -120,6 +123,8 @@ void MotorCommandsLoop() 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) { diff --git a/Core/FSM/Src/robot_move_actions.c b/Core/FSM/Src/robot_move_actions.c index c0e70c8..35fd8b6 100644 --- a/Core/FSM/Src/robot_move_actions.c +++ b/Core/FSM/Src/robot_move_actions.c @@ -20,6 +20,7 @@ /*=========================== 2.宏定义 ===========================*/ /* 姿态控制常量（用于 Robot_Posture_Adjus_Gravity） */ +#define TT_One_Deg_Count2 11014 #define POSTURE_ERROR_THRESHOLD 5.0f /* 姿态调整误差阈值（度） */ #define ANGLE_NORMALIZE_BOUNDARY 180.0f /* 角度归一化边界 */ #define SPEED_FACTOR_LARGE 1.0f /* 大误差速度系数 */ @@ -76,6 +77,9 @@ enum swing_state { ////局部静态变量 /* 状态机当前状态（多个状态机共用，可能存在潜在冲突，但是现在还不知道咋搞呢） */ 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; /* 换道计时变量 */ @@ -87,12 +91,20 @@ static float LEFT_TARGET_ANGLES[2]; 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. 静态函数声明 ===========================*/ /* 水平/垂直状态机函数 */ static void handleAttitudeJudge_horizontal(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); void horizontal_work(void); @@ -116,6 +128,7 @@ void swing_work(void); static void swing_start_move(void); static void swing_left_move(void); static void swing_right_move(void); +static void get_swing_mode(void); /* PID系数 */ @@ -129,7 +142,7 @@ int factor_2= -1; */ static const void (*const horizontal_single_lane_auto_operation[])(void) = { [STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_horizontal, - [STATE_ATTITUDE_ADJUST] = handleAttitudeAdjust, + [STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustHorizontal, [STATE_WORK_WAY_OR_DIRECT_MOVE] = handleWorkWay, [STATE_FIGHT_ALTERNATELY] = NULL, /* 待实现 */ [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) = { [STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_horizontal, - [STATE_ATTITUDE_ADJUST] = handleAttitudeAdjust, + [STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustHorizontal, [STATE_WORK_WAY_OR_DIRECT_MOVE] = auto_drive_pid_horizontal, [STATE_HALT] = handleHalt, }; @@ -153,7 +166,7 @@ static const void (*const robot_horizontal_drive[])(void) = { */ static const void (*const robot_vertical_drive[])(void) = { [STATE_ATTITUDE_JUDGE] = handleAttitudeJudge_vertical, - [STATE_ATTITUDE_ADJUST] = handleAttitudeAdjust, + [STATE_ATTITUDE_ADJUST] = handleAttitudeAdjustVertical, [STATE_WORK_WAY_OR_DIRECT_MOVE] = auto_drive_pid_vertical, [STATE_HALT] = handleHalt, }; @@ -163,10 +176,10 @@ static const void (*const robot_vertical_drive[])(void) = { */ static const void (*const vertical_lane_change_left[])(void) = { [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_CONTINUOUS_RETREAT] = handleLaneChangeContinuousRetreat, - [LANE_CHANGE_STATE_RETREAT_DONE_SWITCH] = handleAttitudeAdjust, + [LANE_CHANGE_STATE_RETREAT_DONE_SWITCH] = handleAttitudeAdjustChangeRoad, [STATE_HALT] = handleHalt, }; @@ -213,6 +226,7 @@ void Turn_Right(void) void Robot_Stop(void) { + get_swing_mode(); Robot_Swing_Operation_Function(); GV.Left_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) { - s_currentState++; + 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; + Robot_Posture_Adjus_Gravity(&errorAbs); + if (errorAbs < ANGLE_ERROR_THRESHOLD) { + ChangeRoad_currentState++; } } /*----------------------------------------------------------------- @@ -269,7 +302,7 @@ static void handleAttitudeJudge_horizontal(void) } else { 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) @@ -284,18 +317,18 @@ static void handleAttitudeJudge_vertical(void) GV.Robot_Angle_Desire -= 360.0f; } } - s_currentState = STATE_ATTITUDE_ADJUST; + vertical_s_currentState = STATE_ATTITUDE_ADJUST; } static void handleWorkWay(void) { - int mode = (int)GV.PV.Robot_backMode; + int mode = GV.PV.Robot_backMode; if (mode == 1) { - s_currentState = STATE_FIGHT_ALTERNATELY; + horizontal_s_currentState = STATE_FIGHT_ALTERNATELY; } else if (mode == 2) { - s_currentState = STATE_FIGHT_RETREATING; + horizontal_s_currentState = STATE_FIGHT_RETREATING; } /* 其他模式保持当前状态？目前没有 else，可能默认留在该状态 */ } @@ -310,7 +343,7 @@ static void handleLaneChangeAttitudeJudgeLeft(void) Change_stop_flag=0; get_ChangeAngle(); 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) { - float distanceM; + //float distanceM; 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) //竖直 { - 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 timeMs = timeSec * 60.0f * 1000.0f; /* 毫秒 */ @@ -363,11 +399,11 @@ static void handleLaneChangeCalcRetreatTime(void) uint32_t tickNow = HAL_GetTick(); 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) { - 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(); if ((tickNow - s_retreatStartTick) >= s_retreatDurationTicks) { - s_currentState++; + ChangeRoad_currentState++; 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) { - 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; Move_Swing_Left_Func_Do_imm(); @@ -422,7 +459,8 @@ static void swing_start_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(); swing_currentState=SWING_RIGHT_MOVE; @@ -433,7 +471,8 @@ static void swing_left_move(void) static void swing_right_move(void) { - if(GV.SwingMotor.Real_Positionright_compare_value-1) { Move_Swing_Left_Func_Do_lay(); 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驱动及停机处理 *-----------------------------------------------------------------*/ @@ -473,20 +543,23 @@ static void handleHalt(void) GV.Right_Speed_M_min = 0; GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Target_Velcity=0; + horizontal_s_currentState = 0; + vertical_s_currentState = 0; + ChangeRoad_currentState = 0; s_currentState = 0; } /*=========================== 8.对外接口函数（需外部调用） ===========================*/ void Move_Horizontal_Auto_Init(void) { - s_currentState = STATE_ATTITUDE_JUDGE; + horizontal_s_currentState = STATE_ATTITUDE_JUDGE; } void Move_Horizontal_Auto_Sub_Func(void) { - if (s_currentState >= 0 && s_currentState < STATE_COUNT) { - if (horizontal_single_lane_auto_operation[s_currentState] != NULL) { - horizontal_single_lane_auto_operation[s_currentState](); + if (horizontal_s_currentState >= 0 && horizontal_s_currentState < STATE_COUNT) { + if (horizontal_single_lane_auto_operation[horizontal_s_currentState] != NULL) { + horizontal_single_lane_auto_operation[horizontal_s_currentState](); } else { /* 遇到未实现的状态，停机处理 */ 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) { - vertical_lane_change_left[s_currentState](); + if (ChangeRoad_currentState >= 0 && ChangeRoad_currentState < LANE_CHANGE_STATE_COUNT) { + vertical_lane_change_left[ChangeRoad_currentState](); } else { handleHalt(); } @@ -510,16 +583,16 @@ void Vertical_Left_Func(void) void vertical_forward(void) { - if (s_currentState >= 0 && s_currentState < STATE_COUNT) { - robot_vertical_drive[s_currentState](); + if (vertical_s_currentState >= 0 && vertical_s_currentState < STATE_COUNT) { + robot_vertical_drive[vertical_s_currentState](); } else { handleHalt(); } } void horizontal_forward(void) { - if (s_currentState >= 0 && s_currentState < STATE_COUNT) { - robot_horizontal_drive[s_currentState](); + if (horizontal_s_currentState >= 0 && horizontal_s_currentState < STATE_COUNT) { + robot_horizontal_drive[horizontal_s_currentState](); } else { handleHalt(); } @@ -527,8 +600,8 @@ void horizontal_forward(void) void horizontal_work(void) { - if (s_currentState >= 0 && s_currentState < STATE_COUNT) { - horizontal_single_lane_auto_operation[s_currentState](); + if (horizontal_s_currentState >= 0 && horizontal_s_currentState < STATE_COUNT) { + horizontal_single_lane_auto_operation[horizontal_s_currentState](); } else { handleHalt(); } @@ -552,6 +625,9 @@ void flag_reset(void) &&P_MK32->CH3_LY_H==0) { s_currentState = 0; + horizontal_s_currentState = 0; + vertical_s_currentState = 0; + ChangeRoad_currentState = 0; swing_currentState = 0; } diff --git a/Core/FSM/Src/swing_action.c b/Core/FSM/Src/swing_action.c index d4a3ce3..7130187 100644 --- a/Core/FSM/Src/swing_action.c +++ b/Core/FSM/Src/swing_action.c @@ -16,13 +16,12 @@ #define RIGHT_LIMIT 2 float Swing_Speed_Deg_Sencond=201.7;//HJ32-121 int middle_position=-944334; -int center_angle; -int offset_angle; -int present_angle; -int center_position; //中间位置-944334 +//int center_angle; +//int offset_angle; +//int center_position; //中间位置-944334 int limit_record=0; -int left_compare_value; -int right_compare_value; +float left_compare_value; +float right_compare_value; int lef_positon=-569580; int Right_positon=-1230420; @@ -32,22 +31,20 @@ void Robot_Swing_Operation_Function() if(P_MK32->CH0_RY_H>300) { GV.SwingMotor.Position_immediately1_Lag2=1; - GV.SwingMotor.Tar_Position_count=-944334-90*TT_One_Deg_Count; - GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; + GV.SwingMotor.Tar_Position_angle=90; + GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; } else if(P_MK32->CH0_RY_H<-300) { GV.SwingMotor.Position_immediately1_Lag2=1; - GV.SwingMotor.Tar_Position_count=-944334+90*TT_One_Deg_Count; - GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; + GV.SwingMotor.Tar_Position_angle=-90; + GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; } else { 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) { GV.SwingMotor.Position_immediately1_Lag2=2; - GV.SwingMotor.Tar_Position_count=left_compare_value; - GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; + GV.SwingMotor.Tar_Position_angle=left_compare_value; + 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.Tar_Position_count=right_compare_value; - GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; + GV.SwingMotor.Tar_Position_angle=right_compare_value; + GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; } //摆臂电机左转立即执行 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.Tar_Position_count=left_compare_value; - GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; + GV.SwingMotor.Tar_Position_angle=left_compare_value; + 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) { - 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.Tar_Position_count=right_compare_value; - GV.SwingMotor.Tar_Position_Velcity_RPM=GV.PV.Robot_Swing_Speed*Swing_Speed_Deg_Sencond; + GV.SwingMotor.Tar_Position_angle=right_compare_value; + 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.Tar_Position_count=0; - GV.SwingMotor.Tar_Position_Velcity_RPM=0; + GV.SwingMotor.Tar_Position_Velcity_Degree_S=0; -// GV.SwingMotor.Target_Velcity = 0; } diff --git a/疑问记录.txt b/疑问记录.txt index 0eeccbc..5dba9f7 100644 --- a/疑问记录.txt +++ b/疑问记录.txt @@ -44,4 +44,8 @@ GV_Robot_backMode=(int)CV.PV.Robot_backMode; 绑定电机 测距 调试摆臂 调内部PID - 问题：摆臂CAN解析√ 变量的传导 报错信息 焊缝跟踪的验证√ 换道精度（最小二乘拟合）√ EPRROM的CV的配置 自动巡航√ 非对称摆臂 \ No newline at end of file + 问题：摆臂CAN解析√ 变量的传导√ 报错信息 焊缝跟踪的验证√ 换道精度（最小二乘拟合）√ EPRROM的CV的配置 自动巡航√ 非对称摆臂 实现急停两个按键 + + 变量的传导 用不同的标志位 + +