diff --git a/Core/BASE/Protobuf/PSource/bsp_Error.pb.h b/Core/BASE/Protobuf/PSource/bsp_Error.pb.h index ce16568..d4ff34d 100644 --- a/Core/BASE/Protobuf/PSource/bsp_Error.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_Error.pb.h @@ -18,7 +18,8 @@ typedef enum _ComError { ComError_Remote_Button_Reset_State = 4, ComError_Strain_Gauge = 5, ComError_Android_485 = 6, - ComError_Ground_Management = 7 + ComError_Ground_Management = 7, + ComError_Pressure_Detect_Alert = 8 } ComError; /* Struct definitions */ @@ -35,8 +36,8 @@ extern "C" { /* Helper constants for enums */ #define _ComError_MIN ComError_MK32_SBus -#define _ComError_MAX ComError_Ground_Management -#define _ComError_ARRAYSIZE ((ComError)(ComError_Ground_Management+1)) +#define _ComError_MAX ComError_Pressure_Detect_Alert +#define _ComError_ARRAYSIZE ((ComError)(ComError_Pressure_Detect_Alert+1)) diff --git a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h index 01e555a..a5ba5e6 100644 --- a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h @@ -111,7 +111,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 1497 +#define GV_struct_define_size 1506 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/BASE/Protobuf/PSource/bsp_PV.pb.h b/Core/BASE/Protobuf/PSource/bsp_PV.pb.h index c3e31c1..fb1cdb1 100644 --- a/Core/BASE/Protobuf/PSource/bsp_PV.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_PV.pb.h @@ -25,6 +25,7 @@ typedef struct _PV_struct_define { int64_t TimeStamp; /* */ int32_t DmkSpeed; /* RPM */ int32_t ToolRotationDirection; /* 1正向 2反向 */ + double Horizontal_Calibration; } PV_struct_define; @@ -33,8 +34,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define PV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#define PV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define PV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define PV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define PV_struct_define_RobotSpeed_tag 1 @@ -50,6 +51,7 @@ extern "C" { #define PV_struct_define_TimeStamp_tag 11 #define PV_struct_define_DmkSpeed_tag 12 #define PV_struct_define_ToolRotationDirection_tag 13 +#define PV_struct_define_Horizontal_Calibration_tag 14 /* Struct field encoding specification for nanopb */ #define PV_struct_define_FIELDLIST(X, a) \ @@ -65,7 +67,8 @@ X(a, STATIC, SINGULAR, DOUBLE, Vertical_Calibration, 9) \ X(a, STATIC, SINGULAR, INT32, RobotRestartAccepted, 10) \ X(a, STATIC, SINGULAR, INT64, TimeStamp, 11) \ X(a, STATIC, SINGULAR, INT32, DmkSpeed, 12) \ -X(a, STATIC, SINGULAR, INT32, ToolRotationDirection, 13) +X(a, STATIC, SINGULAR, INT32, ToolRotationDirection, 13) \ +X(a, STATIC, SINGULAR, DOUBLE, Horizontal_Calibration, 14) #define PV_struct_define_CALLBACK NULL #define PV_struct_define_DEFAULT NULL @@ -76,7 +79,7 @@ extern const pb_msgdesc_t PV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #define BSP_PV_PB_H_MAX_SIZE PV_struct_define_size -#define PV_struct_define_size 137 +#define PV_struct_define_size 146 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/BASE/Protobuf/Proto/bsp_Error.proto b/Core/BASE/Protobuf/Proto/bsp_Error.proto index 119af40..53c2bdf 100644 --- a/Core/BASE/Protobuf/Proto/bsp_Error.proto +++ b/Core/BASE/Protobuf/Proto/bsp_Error.proto @@ -19,6 +19,7 @@ enum ComError //枚举消息类型 Error Bit Define Strain_Gauge = 5; Android_485 = 6; Ground_Management = 7; + Pressure_Detect_Alert = 8; diff --git a/Core/BASE/Protobuf/Proto/bsp_PV.proto b/Core/BASE/Protobuf/Proto/bsp_PV.proto index 3bb5376..366201e 100644 --- a/Core/BASE/Protobuf/Proto/bsp_PV.proto +++ b/Core/BASE/Protobuf/Proto/bsp_PV.proto @@ -20,4 +20,5 @@ message PV_struct_define{ int64 TimeStamp=11;// int32 DmkSpeed= 12;//RPM int32 ToolRotationDirection=13;// 1正向 2反向 + double Horizontal_Calibration=14; }; diff --git a/Core/FSM/Src/change_line_control.c b/Core/FSM/Src/change_line_control.c index 193c64c..00e9f5c 100644 --- a/Core/FSM/Src/change_line_control.c +++ b/Core/FSM/Src/change_line_control.c @@ -295,9 +295,14 @@ void Horizontal_Lane_Change_Turn_To_Left_Control() } +/* + * 以下4个：竖直左、竖直右模式下，换道函数 + * 已改进项：将竖直微调数据同步应用于换道参数配置。 + * + * */ /*********************************************************************/ -/* 竖直从左往右作业 上端 向右换道 最终头朝上 +/* 竖直从左往右作业 上端 向左换道 最终头朝上 * */ void Vertical_Lane_Change_From_Left_To_Right_UP_Control() { @@ -342,7 +347,7 @@ void Vertical_Lane_Change_From_Left_To_Right_UP_Control() } case VerticalChange_TurnToUP: { - if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) + if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)) >= CV.Allowable_Error_For_Angle_Tracking) { fsm_state_set(¤t_robot_move_state, @@ -413,7 +418,8 @@ void Vertical_Lane_Change_From_Left_To_Right_Down_Control() } case VerticalChange_TurnToUP: { - if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) + // 这里判断条件从原来的 “当前值-9000” 改成了 “当前值-9000-竖直微调值”，否则回正时来回抖动 + if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)) >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内 { fsm_state_set(¤t_robot_move_state, @@ -484,7 +490,7 @@ void Vertical_Lane_Change_From_Right_To_Left_UP_Control() } case VerticalChange_TurnToUP: { - if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) + if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)) >= CV.Allowable_Error_For_Angle_Tracking) { fsm_state_set(¤t_robot_move_state, @@ -556,7 +562,7 @@ void Vertical_Lane_Change_From_Right_To_Left_Down_Control() } case VerticalChange_TurnToUP: { - if (abs(GV.Robot_Angle - CV.RobotUpAngleValue) + if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)) >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内 { fsm_state_set(¤t_robot_move_state, diff --git a/Core/FSM/Src/fsm_state_control.c b/Core/FSM/Src/fsm_state_control.c index a770b6d..1dc6698 100644 --- a/Core/FSM/Src/fsm_state_control.c +++ b/Core/FSM/Src/fsm_state_control.c @@ -36,6 +36,7 @@ int LaneChangeControl(); void DHRougheningControl(); //拉毛前端控制 void Mannual_TiltControl(); +void Mannual_TiltControl1(); int Auto_TiltControl(); @@ -144,51 +145,331 @@ void GF_Dispatch() -void MoveControl() -{ - if (MotorErrorDetect() == 1) - { - fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/ - fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/ +//void MoveControl() +//{ +// if (MotorErrorDetect() == 1) +// { +// fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/ +// fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/ +// +// } +// // 压力值预警 +// if (IV.Press >= 500) +// { +// fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/ +// fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/ +// SET_BIT_1(SystemErrorCode,ComError_Pressure_Detect_Alert); +//// return; +// } +// else +// { +// SET_BIT_0(SystemErrorCode,ComError_Pressure_Detect_Alert); +// } +// Lcompensation_control(); //调节水平走时候的补偿值 +// Rcompensation_control(); +// +// /* 速度选择索引 *0-30*/ +// speed_selection = 2.0 * (P_MK32->CH11_RD1 + 1000) / 200; +// GV.Robot_Move_Speed = speed_M_min_toE01_M_min(GetVehicleSpeed(speed_selection));/*获取旋钮速度*/ +// IV.RobotMoveSpeed = GetVehicleSpeed(speed_selection); +// +// Mannual_TiltControl();/*推杆控制*/ +// DHRougheningControl(); /*拉毛盘控制*/ +// +// if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) +// { +// Rough_RegionAutoMoveControl(); +// return; //返回零 不属于自动作业范畴 +// } +// +// //换道优先级优于普通的运行; +// if (LaneChangeControl_Rough() != 0) +// { +// return; +// } +// +// //自动巡航 +// if (P_MK32->CH5_SB == -1000) //自动巡航前进 +// { +// robot_forwards(); +// return; +// } +// if (P_MK32->CH5_SB == 1000) //自动巡航后退 +// { +// robot_backwards(); +// return; +// } +// joysticker_manual_control(); //摇杆手动控制 +// +//} + + + +// +///* +// * MoveControl版本2：配置了两种模式：到达压力预警之前和之后 +// * 之前——所有功能都能正常运行； +// * 之后——仅保留推杆手动操作，其余全部关停，且手动操作仅支持推杆抬升 +// * */ +//// 全局定义 +//volatile uint8_t g_IsPressureLocked = 0; +// +//void MoveControl() +//{ +// // 1. 故障与压力检测 +// if (MotorErrorDetect() == 1 || IV.Press >= 500) +// { +// fsm_state_set(¤t_robot_move_state, &robot_halt_state); +// fsm_state_set(¤t_roughening_state, &roughening_halt_state); +// +// if (IV.Press >= 500) { +// SET_BIT_1(SystemErrorCode, ComError_Pressure_Detect_Alert); +// g_IsPressureLocked = 1; // 进入过载锁定状态 +// } +// if (MotorErrorDetect() == 1) { +// g_IsPressureLocked = 1; +// } +// } +// else +// { +// SET_BIT_0(SystemErrorCode, ComError_Pressure_Detect_Alert); +// // 增加迟滞，防止在500附近频繁切换，比如低于480才解除 +// if (g_IsPressureLocked == 1 && IV.Press < 480) { +// g_IsPressureLocked = 0; // 解除锁定，恢复正常 +// } +// } +// +// // ... (补偿控制和速度计算代码保持不变) ... +// Lcompensation_control(); +// Rcompensation_control(); +// speed_selection = 2.0 * (P_MK32->CH11_RD1 + 1000) / 200; +// GV.Robot_Move_Speed = speed_M_min_toE01_M_min(GetVehicleSpeed(speed_selection)); +// IV.RobotMoveSpeed = GetVehicleSpeed(speed_selection); +// +// // ============================================================ +// // 根据锁定状态选择摇杆控制函数 +// // ============================================================ +// if (g_IsPressureLocked == 1) +// { +// // 压力过载状态：调用受限的摇杆控制（只能抬，不能压） +// Mannual_TiltControl1(); +// } +// else +// { +// // 正常工作状态：调用正常的摇杆控制 +// Mannual_TiltControl(); +// } +// +// // ============================================================ +// // 【运动互锁】：如果锁定，禁止机器人和拉毛盘运动，直接返回 +// // ============================================================ +// if (g_IsPressureLocked == 1) +// { +// // 再次确保状态机处于停止态（双重保险） +// fsm_state_set(¤t_robot_move_state, &robot_halt_state); +// fsm_state_set(¤t_roughening_state, &roughening_halt_state); +// +// // 直接退出，不执行后面的自动巡航、换道、手动行走等代码 +// return; +// } +// +// // --- 以下代码只有在未锁定 (g_IsPressureLocked == 0) 时才会执行 --- +// +// DHRougheningControl(); /* 拉毛盘控制 */ +// +// if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) +// { +// Rough_RegionAutoMoveControl(); +// return; +// } +// +// if (LaneChangeControl_Rough() != 0) +// { +// return; +// } +// +// if (P_MK32->CH5_SB == -1000) +// { +// robot_forwards(); +// return; +// } +// if (P_MK32->CH5_SB == 1000) +// { +// robot_backwards(); +// return; +// } +// +// joysticker_manual_control(); // 摇杆手动控制底盘 +//} - } - Lcompensation_control(); //调节水平走时候的补偿值 - Rcompensation_control(); - /* 速度选择索引 *0-30*/ - speed_selection = 2.0 * (P_MK32->CH11_RD1 + 1000) / 200; - GV.Robot_Move_Speed = speed_M_min_toE01_M_min(GetVehicleSpeed(speed_selection));/*获取旋钮速度*/ - IV.RobotMoveSpeed = GetVehicleSpeed(speed_selection); - Mannual_TiltControl();/*推杆控制*/ - DHRougheningControl(); /*拉毛盘控制*/ - if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) - { - Rough_RegionAutoMoveControl(); - return; //返回零 不属于自动作业范畴 - } - //换道优先级优于普通的运行; - if (LaneChangeControl_Rough() != 0) - { - return; - } +/* + * MoveControl版本7：彻底解决状态机惯性问题 + * 核心逻辑： + * 1. 在调用摇杆函数 BEFORE，先根据压力状态判断是否需要“强制急停”推杆。 + * 2. 如果需要保护，先手动 fsm_state_set(..., tilt_halt_state)，切断之前的下压指令。 + * 3. 然后再调用 Mannual_TiltControl1()，此时它将在“停止”的基础上工作，只能响应“上抬”。 + */ - //自动巡航 - if (P_MK32->CH5_SB == -1000) //自动巡航前进 - { - robot_forwards(); - return; - } - if (P_MK32->CH5_SB == 1000) //自动巡航后退 - { - robot_backwards(); - return; - } - joysticker_manual_control(); //摇杆手动控制 +// 全局定义 +volatile uint8_t g_IsPressureLocked = 0; +int PRESSURE_LOCK_THRESHOLD = 1500; +void MoveControl() +{ + // ============================================================ + // 【配置参数】 + // ============================================================ +// const int PRESSURE_LOCK_THRESHOLD = 1500; + const int PRESSURE_UNLOCK_THRESHOLD = 1450; + const int COUNT_LIMIT = 10; // 0.2s + + static int pressure_over_count = 0; + bool needs_restricted_mode = false; // 标记是否需要受限模式 + + // ============================================================ + // 1. 压力检测与锁定逻辑 + // ============================================================ + + if (IV.Press >= PRESSURE_LOCK_THRESHOLD) + { + pressure_over_count++; + if (pressure_over_count >= COUNT_LIMIT) + { + g_IsPressureLocked = 1; + if (pressure_over_count > COUNT_LIMIT + 50) pressure_over_count = COUNT_LIMIT + 50; + } + } + else + { + if (g_IsPressureLocked == 1) + { + if (IV.Press < PRESSURE_UNLOCK_THRESHOLD) + { + g_IsPressureLocked = 0; + pressure_over_count = 0; + } + else + { + // 保持在锁定状态，压力在 4500-5000 之间 + pressure_over_count = 0; + } + } + else + { + pressure_over_count = 0; + } + } + +// 电机错误 +// if (MotorErrorDetect() == 1) +// { +// fsm_state_set(¤t_robot_move_state, &robot_halt_state); +// fsm_state_set(¤t_roughening_state, &roughening_halt_state); +// g_IsPressureLocked = 1; +// } + + // 报警位 + if (g_IsPressureLocked == 1) + { + SET_BIT_1(SystemErrorCode, ComError_Pressure_Detect_Alert); + fsm_state_set(¤t_robot_move_state, &robot_halt_state); + fsm_state_set(¤t_roughening_state, &roughening_halt_state); + } + else + { + SET_BIT_0(SystemErrorCode, ComError_Pressure_Detect_Alert); + } + + Lcompensation_control(); + Rcompensation_control(); + speed_selection = 2.0 * (P_MK32->CH11_RD1 + 1000) / 200; + GV.Robot_Move_Speed = speed_M_min_toE01_M_min(GetVehicleSpeed(speed_selection)); + IV.RobotMoveSpeed = GetVehicleSpeed(speed_selection); + + // ============================================================ + // 2. 决定模式并强制中断旧状态 + // ============================================================ + + // 判断是否需要受限模式 (锁定中 OR 压力过高预警) + // 把阈值设在 4800，一旦超过，就算没锁定，也先限制下压，防止冲过5000 + if (g_IsPressureLocked == 1 || IV.Press >= 1480) + { + needs_restricted_mode = true; + + // 核心动作：在切换函数前，强制将推杆状态机复位为“停止” ：否则，即使超限，因为右摇杆一直没有松手，所以还是能keep下压 + // 这行代码会立即打断 Mannual_TiltControl 之前可能设置的 tilt_down_state + fsm_state_set(¤t_tilt_state, &tilt_halt_state); + } + else + { + needs_restricted_mode = false; + } + + // ============================================================ + // 3. 执行对应的摇杆控制函数 + // ============================================================ + if (needs_restricted_mode) + { + // 此时 current_tilt_state 已经是 tilt_halt_state 了 + // Mannual_TiltControl1 将在这个“停止”的基础上运行 + // 逻辑是：检测到下压角度 -> 忽略/保持停止；检测到上抬角度 -> 允许上抬 + Mannual_TiltControl1(); + } + else + { + // 正常模式，无干预 + Mannual_TiltControl(); + } + + // ============================================================ + // 4. 【运动互锁】：如果锁定，禁止机器人和拉毛盘运动，直接返回 + // ============================================================ + if (g_IsPressureLocked == 1) + { + fsm_state_set(¤t_robot_move_state, &robot_halt_state); + fsm_state_set(¤t_roughening_state, &roughening_halt_state); + return; + } + + // ============================================================ + // 5. 正常运动逻辑 + // ============================================================ + DHRougheningControl(); + + if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) + { + Rough_RegionAutoMoveControl(); + return; + } + + if (LaneChangeControl_Rough() != 0) + { + return; + } + + if (P_MK32->CH5_SB == -1000) + { + robot_forwards(); + return; + } + if (P_MK32->CH5_SB == 1000) + { + robot_backwards(); + return; + } + + joysticker_manual_control(); } + + + + + + /* @brief: 电机出现报警返回1，电机正常返回0 */ uint8_t MotorErrorDetect() { @@ -341,33 +622,101 @@ void joysticker_manual_control() void Mannual_TiltControl() { - if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return; - - - if (abs(P_MK32->CH1_RY_V) <= CV.Joy_Sticker_Value_Allowance - && abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance)/*停止*//*600*/ - { - fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*停止推杆*/ - return; - } - int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI; - - if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance) - { - fsm_state_set(¤t_tilt_state, &tilt_up_state);/*上升*/ - return; - } - if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)/*45° 下降*/ - { - if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet) - { - fsm_state_set(¤t_tilt_state, &tilt_down_state); - return; - } - fsm_state_set(¤t_tilt_state, &tilt_halt_state); - } + if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return; + + if (abs(P_MK32->CH1_RY_V) <= CV.Joy_Sticker_Value_Allowance + && abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance)/*停止*//*600*/ + { + fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*停止推杆*/ + return; + } + int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI; + + if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance) + { + fsm_state_set(¤t_tilt_state, &tilt_up_state);/*上升*/ + return; + } + // 原下压逻辑：无限位 + if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)/*45° 下降*/ + { + if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet) + { + fsm_state_set(¤t_tilt_state, &tilt_down_state); + return; + } + fsm_state_set(¤t_tilt_state, &tilt_halt_state); + } + + + +// // 下压+软限位。消抖计数，初始化为0 +// static int pressure_over_count = 0; +// +// const int SOFT_LIMIT_PRESSURE = 2000; // 压力阈值 +// const int COUNT_LIMIT = 1; // 200ms / 2ms = 100次计数 +// if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance) +// { +// int current_pressure = IV.Press; +// +// // --- 新增软限位 (带消抖) --- +// bool is_soft_safe = (current_pressure <= SOFT_LIMIT_PRESSURE); +// if (!is_soft_safe) +// { +// pressure_over_count++; +// +// // 判断是否达到连续时间阈值 (100次 * 2ms = 200ms) +// if (pressure_over_count >= COUNT_LIMIT) +// { +// fsm_state_set(¤t_tilt_state, &tilt_halt_state); +// // 此时计数器保持在高位，直到压力降低或摇杆回中才清零 +// } +// else +// { +// // 处于消抖过程中 (例如超压了 50ms)，视为干扰，允许继续下降 +// fsm_state_set(¤t_tilt_state, &tilt_down_state); +// } +// } +// else +// { +// // 压力正常， 计数器清零 (打破连续性) +// pressure_over_count = 0; +// +// // 允许下降 +// fsm_state_set(¤t_tilt_state, &tilt_down_state); +// } +// +// return; +// } +// +// // 其他角度情况，默认停止 +// fsm_state_set(¤t_tilt_state, &tilt_halt_state); + + +} + + +// 压力过载下的推杆控制：删除下压功能 +void Mannual_TiltControl1() +{ + + if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return; + + if (abs(P_MK32->CH1_RY_V) <= CV.Joy_Sticker_Value_Allowance + && abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance)/*停止*//*600*/ + { + fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*停止推杆*/ + return; + } + int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI; + + if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance) + { + fsm_state_set(¤t_tilt_state, &tilt_up_state);/*上升*/ + return; + } } diff --git a/Core/FSM/Src/robot_move_actions.c b/Core/FSM/Src/robot_move_actions.c index cd7a2db..a86e94c 100644 --- a/Core/FSM/Src/robot_move_actions.c +++ b/Core/FSM/Src/robot_move_actions.c @@ -119,7 +119,9 @@ void HALT_State_Exit(transition_t *p_this) * */ void Move_Horizontal_Task_Forwards_Right_Do(transition_t *p_this) { - Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation); +// Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation); + Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration); //+补偿+水平微调 + } void Move_Horizontal_Task_Forwards_Right_Paint_Do(transition_t *p_this) { @@ -130,7 +132,9 @@ void Move_Horizontal_Task_Forwards_Right_Paint_Do(transition_t *p_this) * */ void Move_Horizontal_Task_Backwards_Right_Do(transition_t *p_this) { - Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation); +// Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation); + Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration); + } /**和拉毛区分开 防止左右补偿加到喷漆上**/ void Move_Horizontal_Task_Backwards_Right_Paint_Do(transition_t *p_this) @@ -142,7 +146,9 @@ void Move_Horizontal_Task_Backwards_Right_Paint_Do(transition_t *p_this) * */ void Move_Horizontal_Task_Forwards_Left_Do(transition_t *p_this) { - Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation); +// Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation); + Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration); + } /**和拉毛区分开 防止左右补偿加到喷漆上**/ void Move_Horizontal_Task_Forwards_Left_Paint_Do(transition_t *p_this) @@ -155,7 +161,9 @@ void Move_Horizontal_Task_Forwards_Left_Paint_Do(transition_t *p_this) * */ void Move_Horizontal_Task_Backwards_Left_Do(transition_t *p_this) { - Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation); +// Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation); + Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration); + } void Move_Horizontal_Task_Backwards_Left_Paint_Do(transition_t *p_this) { @@ -168,8 +176,8 @@ void Move_Horizontal_Task_Backwards_Left_Paint_Do(transition_t *p_this) * */ void Move_Head_To_UP_Do(transition_t *p_this) { - Calbrate_Robot_Positon(CV.RobotUpAngleValue); - +// Calbrate_Robot_Positon(CV.RobotUpAngleValue); // 原始版本，转到固定的0/90度那种 + Calbrate_Robot_Positon((CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)); // 加入竖直微调项的转动角度 } /* 原地PID转向至目标角度 CV.RobotDownAngleValue @@ -202,11 +210,12 @@ void Move_Head_To_Right_Do(transition_t *p_this) * 底层前进，保持目标角度 纠偏 被调用 * Target_Angle: 0.1度 * */ +int aaa=0; int delt=0; void Move_Forwards_Do(int32_t Target_Angle) { - + aaa=abs(GV.Robot_Angle - Target_Angle); if (abs(GV.Robot_Angle - Target_Angle) <= CV.PID_mid.PID_Angle) //// 误差在正负x度内 { if (abs(GV.Robot_Angle - Target_Angle) < CV.PID_low.PID_Angle) //// 误差在正负0.3度内