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的配置 自动巡航√ 非对称摆臂 实现急停两个按键
+
+ 变量的传导 用不同的标志位
+
+