XiaoPengfei
/
32_Roughening_BigBoard
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

新增压力限位

master
LAPTOPNUM\lapto 2 days ago
parent
d9cebc688f
commit
256fc82364
8 changed files with 452 additions and 82 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      Core/BASE/Protobuf/PSource/bsp_Error.pb.h
  2. 2
      Core/BASE/Protobuf/PSource/bsp_GV.pb.h
  3. 11
      Core/BASE/Protobuf/PSource/bsp_PV.pb.h
  4. 1
      Core/BASE/Protobuf/Proto/bsp_Error.proto
  5. 1
      Core/BASE/Protobuf/Proto/bsp_PV.proto
  6. 16
      Core/FSM/Src/change_line_control.c
  7. 473
      Core/FSM/Src/fsm_state_control.c
  8. 23
      Core/FSM/Src/robot_move_actions.c

7
Core/BASE/Protobuf/PSource/bsp_Error.pb.h
View File

@ -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))

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

@ -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" */

11
Core/BASE/Protobuf/PSource/bsp_PV.pb.h
View File

@ -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" */

1
Core/BASE/Protobuf/Proto/bsp_Error.proto
View File

@ -19,6 +19,7 @@ enum ComError //枚举消息类型 Error Bit Define
Strain_Gauge = 5;
Android_485 = 6;
Ground_Management = 7;
Pressure_Detect_Alert = 8;

1
Core/BASE/Protobuf/Proto/bsp_PV.proto
View File

@ -20,4 +20,5 @@ message PV_struct_define{
int64 TimeStamp=11;//
int32 DmkSpeed= 12;//RPM
int32 ToolRotationDirection=13;// 1正向 2反向
double Horizontal_Calibration=14;
};

16
Core/FSM/Src/change_line_control.c
View File

@ -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(&current_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(&current_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(&current_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(&current_robot_move_state,

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

@ -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(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
//void MoveControl()
//{
// if (MotorErrorDetect() == 1)
// {
// fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
// fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
//
// }
// // 压力值预警
// if (IV.Press >= 500)
// {
// fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
// fsm_state_set(&current_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(&current_robot_move_state, &robot_halt_state);
// fsm_state_set(&current_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(&current_robot_move_state, &robot_halt_state);
// fsm_state_set(&current_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(&current_robot_move_state, &robot_halt_state);
// fsm_state_set(&current_roughening_state, &roughening_halt_state);
// g_IsPressureLocked = 1;
// }
// 报警位
if (g_IsPressureLocked == 1)
{
SET_BIT_1(SystemErrorCode, ComError_Pressure_Detect_Alert);
fsm_state_set(&current_robot_move_state, &robot_halt_state);
fsm_state_set(&current_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(&current_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(&current_robot_move_state, &robot_halt_state);
fsm_state_set(&current_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(&current_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(&current_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(&current_tilt_state, &tilt_down_state);
return;
}
fsm_state_set(&current_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(&current_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(&current_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(&current_tilt_state, &tilt_down_state);
return;
}
fsm_state_set(&current_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(&current_tilt_state, &tilt_halt_state);
// // 此时计数器保持在高位，直到压力降低或摇杆回中才清零
// }
// else
// {
// // 处于消抖过程中 (例如超压了 50ms)，视为干扰，允许继续下降
// fsm_state_set(&current_tilt_state, &tilt_down_state);
// }
// }
// else
// {
// // 压力正常， 计数器清零 (打破连续性)
// pressure_over_count = 0;
//
// // 允许下降
// fsm_state_set(&current_tilt_state, &tilt_down_state);
// }
//
// return;
// }
//
// // 其他角度情况，默认停止
// fsm_state_set(&current_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(&current_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(&current_tilt_state, &tilt_up_state);/*上升*/
return;
}
}

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

@ -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度内

Write Preview
Loading…
Cancel
Save