You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
787 lines
21 KiB
787 lines
21 KiB
/*
|
|
* fsm_state_control.c
|
|
*
|
|
* Created on: 2025年12月29日
|
|
* Author: xsq
|
|
*/
|
|
|
|
#include <bsp_tempature.h>
|
|
#include <math.h>
|
|
|
|
#include "BHBF_ROBOT.h"
|
|
#include "bsp_include.h"
|
|
#include "msp_PID.h"
|
|
#include "msp_MK32_1.h"
|
|
#include "motors.h"
|
|
#include "fsm_state.h"
|
|
#include "motors_power_action.h"
|
|
#include "fsm_state_control.h"
|
|
#include "roughening_end_actions.h"
|
|
#include "tilt_action.h"
|
|
#include "robot_move_actions.h"
|
|
|
|
#define Move_Horizontal_AngleThreshold_D 85
|
|
#define M_PI 3.14159265358979323846
|
|
double speed_selection;
|
|
char is_upper_computer_take_over_control = 0;
|
|
int32_t isAdjustCompensation = 1; //第一次将保存的补偿值赋给车体
|
|
int32_t isMannualAdjustPress = 1; //手动模式下 第一次自动调节压力
|
|
int index_counter = 0;
|
|
|
|
uint8_t MotorErrorDetect();
|
|
void robot_backwards();
|
|
void robot_forwards();
|
|
void joysticker_manual_control();
|
|
int LaneChangeControl();
|
|
|
|
void DHRougheningControl(); //拉毛前端控制
|
|
void Mannual_TiltControl();
|
|
//void Mannual_TiltControl1();
|
|
void Mannual_TiltControl2();
|
|
|
|
int Auto_TiltControl();
|
|
|
|
|
|
void IV_control();
|
|
|
|
void MoveControl();
|
|
void Pressure_Safety_Monitor(void);
|
|
|
|
|
|
int LaneChangeControl();
|
|
|
|
void Horizontal_Lane_Change_Turn_To_Left_Control();
|
|
void Horizontal_Lane_Change_Turn_To_Right_Control();
|
|
|
|
void Vertical_Lane_Change_Right_Control();
|
|
void Vertical_Lane_Change_Left_Control();
|
|
|
|
void RegionAuto_Horizontal_Distance_Err();
|
|
void RegionAuto_Vertical_Distance_Err();
|
|
|
|
void RegionAuto_Horizontal_RightToLeft();
|
|
void RegionAuto_Horizontal_LeftToRight();
|
|
|
|
void RegionAuto_Vertical_RightToLeft();
|
|
void RegionAuto_Vertical_LeftToRight();
|
|
|
|
void Manual_Mode_Move_Forwards();
|
|
void Manual_Mode_Move_Backwards();
|
|
|
|
int RegionAutoMoveControl();
|
|
|
|
|
|
void Manual_Mode_Joystick_Control();
|
|
void Manual_Mode_Joystick_Control1();
|
|
void Manual_Mode_Joystick_Control2();
|
|
void PV_Data_Reading();
|
|
|
|
|
|
|
|
int AbnormalDetect(); //异常检测
|
|
|
|
|
|
|
|
int EmegencyStop_Step = 10;
|
|
int EmegencyStop_StepNum = 10;
|
|
int RegionAuto_MoveTask_Test_Result = 0;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void Fsm_Init()
|
|
{
|
|
|
|
IV_control();
|
|
//机器人运动状态初始化
|
|
fsm_state_init(¤t_robot_move_state, &robot_halt_state);
|
|
//拉毛前端初始化
|
|
fsm_state_init(¤t_roughening_state, &roughening_halt_state);
|
|
//tilt初始化
|
|
fsm_state_init(¤t_tilt_state, &tilt_halt_state);
|
|
//机器人电机供电状态初始化
|
|
fsm_state_init(¤t_motor_power_state, &motor_power_off_state);
|
|
|
|
|
|
|
|
GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
|
|
IV.RobotRestart = 1; //机器人上电初始化，需要通知机器人
|
|
|
|
|
|
|
|
}
|
|
void GF_Dispatch()
|
|
{
|
|
|
|
fsm_state_run(¤t_robot_move_state); /*机器人移动*/
|
|
fsm_state_run(¤t_motor_power_state); /*供电*/
|
|
fsm_state_run(¤t_roughening_state); /*拉毛盘*/
|
|
fsm_state_run(¤t_tilt_state); /*推杆*/
|
|
|
|
PV_Data_Reading();
|
|
IV_control();
|
|
/*包含上电按钮检测 急停 SBUS 串口 等*/
|
|
if (AbnormalDetect() == 1)
|
|
{
|
|
return;
|
|
}
|
|
|
|
/* 移动到Fsm_Init()里，实现软急停断48V后，只能重新上电恢复。
|
|
* 但移上去出现有点问题：，电机、推杆首次无法正常启动，需排查初次上电完整启动流程后才能考虑插入
|
|
* */
|
|
// /***上电检测通过 电机上电**/
|
|
fsm_state_set(¤t_motor_power_state, &motor_power_on_state); /* 上电关 此时设为开 */
|
|
|
|
/*按下遥控 app无法选择*/
|
|
if ((fabs(P_MK32->CH2_LY_V) <= 200) && (fabs(P_MK32->CH3_LY_H) <= 200)
|
|
&& (fabs(P_MK32->CH0_RY_H) <= 200)
|
|
&& (fabs(P_MK32->CH1_RY_V) <= 200) && (P_MK32->CH4_SA == 0)
|
|
&& (P_MK32->CH5_SB == 0) && (P_MK32->CH6_SC != -1000)&& (P_MK32->CH7_SD != -1000))
|
|
{
|
|
IV.IsWorking = 0;
|
|
}
|
|
else
|
|
{
|
|
IV.IsWorking = 1;
|
|
}
|
|
|
|
MoveControl();
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
* 三级压力报警与震荡检测模块 V2.2
|
|
* 功能：适配低频传感器（10/40Hz），通过降采样实现高频控制下的震荡检测
|
|
*/
|
|
|
|
// 全局状态变量
|
|
volatile uint8_t g_IsPressureLocked = 0;
|
|
|
|
// 压力保护参数配置
|
|
//volatile int g_Pressure_Lock_Threshold = 5000; // 锁定阈值
|
|
volatile int g_Pressure_Lock_Threshold = 3500;
|
|
volatile int g_Pressure_Warn_Threshold = 0; // 预警阈值
|
|
volatile int g_Pressure_Unlock_Threshold = 0; // 解锁阈值
|
|
|
|
// 【新增】震荡检测配置参数
|
|
// 假设本函数每2ms调用一次：
|
|
// 设为 1 -> 每2ms检测一次 (适合高频传感器)
|
|
// 设为 5 -> 每10ms检测一次 (适合100Hz传感器)
|
|
// 设为 20 -> 每40ms检测一次 (适合25Hz传感器)
|
|
//volatile uint8_t g_Pressure_Oscillation_Step = 5;
|
|
volatile int g_Pressure_Oscillation_Step = 5;
|
|
|
|
|
|
volatile int OSCILLATION_THRESHOLD = 1000; // 震荡判定阈值：相邻两次采样差值超过此值视为异常 (根据实际传感器量程调整)
|
|
volatile int OSCILLATION_CONFIRM_COUNT = 5; // 震荡确认次数：连续5次(10ms)检测到剧烈跳变即触发
|
|
volatile int pressure_over_count = 0;
|
|
volatile int last_sampled_pressure = 0; // 上一次“有效采样点”的压力
|
|
volatile int oscillation_count = 0; // 震荡计数器
|
|
//volatile uint8_t sample_timer = 0; // 降采样计时器
|
|
volatile int sample_timer = 0;
|
|
|
|
volatile int diff = 120;//测试是否真的能做出震荡判断
|
|
|
|
|
|
void Pressure_Safety_Monitor(void)
|
|
{
|
|
// ============================================================
|
|
// 【配置参数】
|
|
// ============================================================
|
|
const int COUNT_LIMIT = 250; // 0.5s 持续超压滤波
|
|
// const int OSCILLATION_THRESHOLD = 1000; // 震荡判定阈值：差值绝对值
|
|
// const int OSCILLATION_CONFIRM_COUNT = 5; // 震荡确认次数
|
|
|
|
// 动态计算滞回区间：即三级压力预警的剩余两个值
|
|
g_Pressure_Warn_Threshold = g_Pressure_Lock_Threshold - 20;
|
|
g_Pressure_Unlock_Threshold = g_Pressure_Lock_Threshold - 50;
|
|
|
|
|
|
|
|
int current_press = IV.Press;
|
|
|
|
// ============================================================
|
|
// 1. 降采样震荡检测逻辑
|
|
// ============================================================
|
|
sample_timer++;
|
|
|
|
// 只有当计时器达到设定步长时，才进行一次“有效对比”
|
|
if (sample_timer >= g_Pressure_Oscillation_Step)
|
|
{
|
|
// int diff = current_press - last_sampled_pressure;
|
|
diff = current_press - last_sampled_pressure;
|
|
if (diff < 0) diff = -diff; // 取绝对值
|
|
|
|
// 判断是否剧烈跳变
|
|
if (diff >= OSCILLATION_THRESHOLD)
|
|
{
|
|
oscillation_count++;
|
|
}
|
|
else
|
|
{
|
|
// 波动恢复正常，震荡计数清零
|
|
oscillation_count = 0;
|
|
}
|
|
|
|
last_sampled_pressure = current_press;
|
|
sample_timer = 0;
|
|
}
|
|
|
|
// ============================================================
|
|
// 2. 状态机逻辑 (修复解锁漏洞)
|
|
// ============================================================
|
|
|
|
// --- 情况 A：如果当前已经锁定 ---
|
|
if (g_IsPressureLocked == 1)
|
|
{
|
|
// 判断是否可以解锁
|
|
// 逻辑：必须同时满足两个条件才算安全
|
|
// 1. 压力值已经降下来 (解除超压风险)
|
|
// 2. 震荡已经消失 (解除乱跳风险)
|
|
|
|
bool is_pressure_low = (current_press < g_Pressure_Unlock_Threshold);
|
|
bool is_oscillation_free = (oscillation_count == 0);
|
|
|
|
if (is_pressure_low && is_oscillation_free)
|
|
{
|
|
// 只有既不乱跳，压力又低，才允许解锁
|
|
g_IsPressureLocked = 0;
|
|
pressure_over_count = 0;
|
|
}
|
|
// 否则保持锁定状态，直接返回，不再执行下面的超压计数逻辑
|
|
return;
|
|
}
|
|
|
|
// --- 情况 B：当前未锁定，检查是否需要触发锁定 ---
|
|
|
|
// 1. 优先检查震荡 (震荡优先级高于超压)
|
|
if (oscillation_count >= OSCILLATION_CONFIRM_COUNT)
|
|
{
|
|
g_IsPressureLocked = 1;
|
|
return; // 锁定后直接返回
|
|
}
|
|
|
|
// 2. 检查持续超压
|
|
if (current_press >= g_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
|
|
{
|
|
pressure_over_count = 0;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// ============================================================
|
|
// 主控制函数
|
|
// ============================================================
|
|
void MoveControl()
|
|
{
|
|
|
|
|
|
// 3. 补偿与速度计算
|
|
Lcompensation_control();
|
|
Rcompensation_control();
|
|
speed_selection = 2.0 * (P_MK32->CH11_RD1 + 1000) / 200; // 旋钮设置移动速度；2是20/3是30
|
|
GV.Robot_Move_Speed = speed_M_min_toE01_M_min(GetVehicleSpeed(speed_selection));
|
|
IV.RobotMoveSpeed = GetVehicleSpeed(speed_selection);
|
|
|
|
if(IV.Press>3500)
|
|
{
|
|
Mannual_TiltControl2();
|
|
}
|
|
else{
|
|
Mannual_TiltControl();
|
|
}
|
|
|
|
|
|
// 6. 运动逻辑
|
|
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()
|
|
{
|
|
if (GV.LeftMotor.TT_Motor_Fault != 0 || GV.RightMotor.TT_Motor_Fault != 0)
|
|
{
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
* @brief 手动模式,自动巡航,前进，
|
|
* @function 前进
|
|
* @param 无
|
|
* @retval 无
|
|
*/
|
|
void robot_forwards()
|
|
{
|
|
|
|
/* 竖直向左 向右 */
|
|
if (GV.PV.RunMode == Move_Vertical_Move_To_Left
|
|
|| GV.PV.RunMode == Move_Vertical_Move_To_Right)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state,
|
|
&robot_move_vertical_task_forwards_state);/* 前进纠偏 */
|
|
return;
|
|
}
|
|
if (GV.PV.RunMode == Move_Horizontal_Move)
|
|
{
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
|
|
< Move_Horizontal_AngleThreshold_D * 100)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state,
|
|
&robot_move_horizontal_task_forwards_left_state);/* 水平朝左前进纠偏 */
|
|
|
|
}
|
|
else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
|
|
< Move_Horizontal_AngleThreshold_D * 100)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state,
|
|
&robot_move_horizontal_task_forwards_right_state);/* 水平朝右前进纠偏 */
|
|
|
|
}
|
|
else
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);/* 角度小于5° 不动 */
|
|
}
|
|
return;
|
|
}
|
|
fsm_state_set(¤t_robot_move_state, &robot_forwards_state);/* app 无 前进纠偏 */
|
|
return;
|
|
}
|
|
|
|
/**
|
|
* @brief 手动模式下自动巡航后退，
|
|
* @function 自动后退： 竖直模式下
|
|
* @param 无
|
|
* @retval 无
|
|
*/
|
|
void robot_backwards()
|
|
{
|
|
/*竖直*/
|
|
if (GV.PV.RunMode == Move_Vertical_Move_To_Left
|
|
|| GV.PV.RunMode == Move_Vertical_Move_To_Right)
|
|
{
|
|
|
|
fsm_state_set(¤t_robot_move_state,
|
|
&robot_move_vertical_task_backwards_state);/* 后退纠偏 */
|
|
return;
|
|
}
|
|
if (GV.PV.RunMode == Move_Horizontal_Move)
|
|
{
|
|
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
|
|
< Move_Horizontal_AngleThreshold_D * 100)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state,
|
|
&robot_move_horizontal_task_backwards_left_state);/* 水平头朝左后退纠偏 */
|
|
|
|
}
|
|
else if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
|
|
< Move_Horizontal_AngleThreshold_D * 100)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state,
|
|
&robot_move_horizontal_task_backwards_right_state);/* 水平朝右后退纠偏 */
|
|
|
|
}
|
|
else
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);/* 角度大 不动 */
|
|
}
|
|
return;
|
|
}
|
|
|
|
fsm_state_set(¤t_robot_move_state, &robot_backwards_state);/* 后退 */
|
|
return;
|
|
|
|
}
|
|
|
|
void joysticker_manual_control()
|
|
{
|
|
|
|
/*停止*/
|
|
if (abs(P_MK32->CH2_LY_V) <= CV.Joy_Sticker_Value_Allowance
|
|
&& abs(P_MK32->CH3_LY_H) <= CV.Joy_Sticker_Value_Allowance)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/
|
|
return;
|
|
}
|
|
int angle = atan2(P_MK32->CH2_LY_V, P_MK32->CH3_LY_H) * 180 / M_PI;
|
|
|
|
if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)
|
|
{
|
|
/*前进*/
|
|
robot_forwards(); //根据app模式选择纠偏与不纠偏
|
|
return;
|
|
}
|
|
/*后退*/
|
|
if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance)
|
|
{
|
|
robot_backwards();
|
|
return;
|
|
}
|
|
/*右转*/
|
|
if (abs(angle - 0) <= CV.Joy_Sticker_Angle_Allowance)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_turn_right_state); /*右转*/
|
|
IV.CurrentSpeed = CV.LeftTurnSpeed / 10;
|
|
return;
|
|
}
|
|
/*左转*/
|
|
if (abs(angle - 180) <= CV.Joy_Sticker_Angle_Allowance
|
|
|| abs(angle - (-180)) <= CV.Joy_Sticker_Angle_Allowance)
|
|
{
|
|
IV.CurrentSpeed = CV.LeftTurnSpeed / 10;
|
|
fsm_state_set(¤t_robot_move_state, &robot_turn_left_state); /*左转*/
|
|
return;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
//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)
|
|
// {
|
|
// 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)
|
|
// {
|
|
//
|
|
// fsm_state_set(¤t_tilt_state, &tilt_down_state);
|
|
//
|
|
//
|
|
// }
|
|
//}
|
|
|
|
|
|
volatile int g_AUTO_LIFT_RELEASE_PRESSURE = 1500; // 自动抬升的停止阈值，后续改成界面上压力设定值
|
|
// 压力过载下的推杆控制：自动模式下，一旦超出软限位值，
|
|
void Mannual_TiltControl2()
|
|
{
|
|
// 基础模式检查
|
|
if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return;
|
|
|
|
// const int g_AUTO_LIFT_RELEASE_PRESSURE = 1500; // 自动抬升的停止阈值
|
|
|
|
// 1. 自动抬升逻辑
|
|
// 只要压力还大于停止阈值，就一直保持抬升状态
|
|
if (IV.Press > GV.PV.PressSet)
|
|
{
|
|
fsm_state_set(¤t_tilt_state, &tilt_up_state); /*强制上升*/
|
|
return;
|
|
}
|
|
|
|
// 2. 停止逻辑
|
|
// 当压力降到停止阈值以下，停止推杆
|
|
fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*停止推杆*/
|
|
}
|
|
|
|
|
|
int Auto_TiltControl()
|
|
{
|
|
if (0 == isAutoAdjustPress)
|
|
{
|
|
return 0;
|
|
}
|
|
TiltWorking_Mode = Tilt_Auto_Mode;
|
|
/*存在一个问题，当显示大于设定值时需要上升，但是由于机械硬件无法上升，压力无法改变，会卡在推杆上升中，自动程序无法执行*/
|
|
if (GV.Strain_Gauge.Pressure >= GV.PV.PressSet * 0.8 )
|
|
{
|
|
fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*关闭推杆*/
|
|
/*正转开启拉毛盘*/
|
|
if(GV.PV.ToolRotationDirection==1){fsm_state_set(¤t_roughening_state, &roughening_swing_clockwise_state);}
|
|
//反转
|
|
if(GV.PV.ToolRotationDirection==2){fsm_state_set(¤t_roughening_state, &roughening_swing_anticlockwise_state);}
|
|
|
|
TiltWorking_Mode = Tilt_Manual_Mode;
|
|
isAutoAdjustPress = 0;
|
|
return 0;
|
|
}
|
|
// else if (GV.Strain_Gauge.Pressure >= GV.PV.PressSet * 0.8 + 50)
|
|
// {
|
|
// fsm_state_set(¤t_tilt_state, &tilt_up_state); /*上升推杆*/
|
|
// return 1;
|
|
//
|
|
// }
|
|
else if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet * 0.8)
|
|
{
|
|
fsm_state_set(¤t_tilt_state, &tilt_down_state); /*下降推杆*/
|
|
return 1;
|
|
}
|
|
|
|
}
|
|
|
|
/*
|
|
* 拉毛前端控制
|
|
|
|
*/
|
|
void DHRougheningControl()
|
|
{
|
|
if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) /*app 自动水平模式 无法控制*/
|
|
{
|
|
return;
|
|
}
|
|
if (P_MK32->CH6_SC == -1000) //SC上
|
|
{
|
|
// if (isMannualAdjustPress == 1)
|
|
// {
|
|
// if (GV.Strain_Gauge.Pressure < GV.PV.PressSet)/*开启气动马达时先自动下降至设定压力*/
|
|
// {
|
|
// fsm_state_set(¤t_tilt_state, &tilt_down_state);/*推杆下降*/
|
|
// return;
|
|
// }
|
|
// }
|
|
/*isMannualAdjustPress为0*/
|
|
if(GV.PV.ToolRotationDirection==1)
|
|
{
|
|
fsm_state_set(¤t_roughening_state,
|
|
&roughening_swing_clockwise_state); /*正转开启拉毛盘*/
|
|
isMannualAdjustPress = 0;
|
|
return;
|
|
}
|
|
if(GV.PV.ToolRotationDirection==2)
|
|
{
|
|
fsm_state_set(¤t_roughening_state,
|
|
&roughening_swing_anticlockwise_state); /*反转开启拉毛盘*/
|
|
isMannualAdjustPress = 0;
|
|
return;
|
|
}
|
|
|
|
}
|
|
else //SC回中下
|
|
{
|
|
isMannualAdjustPress = 1;
|
|
fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void IV_control()
|
|
{
|
|
|
|
/*除10是因为天太电机读出来的转速是×10的*/
|
|
IV.CurrentSpeed = GV.LeftMotor.Real_Velcity/10.0/CV.wheel_Reduction_Ratio*M_PI*CV.wheel_Diameter_m;
|
|
|
|
IV.LeftCompensation = GV.Left_Compensation;
|
|
IV.RightCompensation = GV.Right_Compensation;
|
|
IV.CurrentAngle = GV.Robot_Angle;
|
|
IV.Press = GV.Strain_Gauge.Pressure;
|
|
IV.SystemError = GV.SystemErrorData.Com_Error_Code;
|
|
IV.Left_Motor_Err = GV.LeftMotor.TT_Motor_Fault;
|
|
IV.Right_Motor_Err = GV.RightMotor.TT_Motor_Fault;
|
|
|
|
IV.Left_Motor_Temp = GV.LeftMotor.Tempature;
|
|
IV.Right_Motor_Temp = GV.RightMotor.Tempature;
|
|
IV.TimeStamp = SystemTimeMiliCount * 2; //2ms一个计数，这里标记的是单片机运行时间
|
|
IV.SBUS_State=GV.P_MK32.IsOnline;
|
|
if (decoded_PV.RobotRestartAccepted == 1)
|
|
{
|
|
IV.RobotRestart = 0;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* 暂存App传输的PV数据，用于在业务执行过程中进行数据隔离
|
|
*/
|
|
void PV_Data_Reading()
|
|
{
|
|
if (P_MK32->CH4_SA == 0 && P_MK32->CH5_SB == 0 && P_MK32->CH7_SD == 0
|
|
&& P_MK32->CH14_LT == 0 && P_MK32->CH15_RT == 0)
|
|
{
|
|
GV.PV = decoded_PV;
|
|
if (isAdjustCompensation == 1)
|
|
{
|
|
GV.Left_Compensation = GV.PV.LeftCompensation;
|
|
GV.Right_Compensation = GV.PV.RightCompensation;
|
|
isAdjustCompensation = 0;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
int AbnormalDetect()
|
|
{
|
|
|
|
//上位机此时，其他设备全部处于不可操作状态，喷漆停止、摆臂上抬，只保持轮子的基本操作（为处理遥控器无法使用的情况）
|
|
if (is_upper_computer_take_over_control == Taken_Over)
|
|
{
|
|
GV.PV.RunMode = Move_Manual;
|
|
fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
|
|
fsm_state_set(¤t_motor_power_state, &motor_power_on_state); //电机上电，上电后，激活电机，
|
|
return 1;
|
|
}
|
|
|
|
//SBUS出错
|
|
if (Get_BIT(SystemErrorCode, ComError_MK32_SBus) == DISCONNECTED && Get_BIT(SystemErrorCode, ComError_Android_485) == DISCONNECTED)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/
|
|
fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
|
|
fsm_state_set(¤t_motor_power_state, &motor_power_off_state); /*关闭电机电磁阀*/
|
|
return 1;
|
|
}
|
|
|
|
/*急停*/
|
|
if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000)
|
|
{
|
|
fsm_state_set(¤t_motor_power_state, &motor_power_off_state); /*关闭电机电磁阀*/
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/
|
|
fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
|
|
Is_All_Button_Has_Reset = 0; //急停之后必须复位才能动
|
|
IV.CurrentSpeed = 0;
|
|
return 1;
|
|
}
|
|
|
|
/* 按钮未复位 第一次上电 SystemErrorCode不为0 计算值为1 未复位 */
|
|
if (Get_BIT(SystemErrorCode, ComError_Remote_Button_Reset_State) //按钮未复位 SystemErrorCode 右移ComError_Remote_Button_Reset_State位
|
|
!= Has_Reset) //Has_Reset 0 复位 1未复位
|
|
{
|
|
fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
|
|
fsm_state_set(¤t_motor_power_state, &motor_power_off_state); /*关闭电机电磁阀*/
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/
|
|
return 1;
|
|
}
|
|
|
|
|
|
//陀螺仪无信号
|
|
if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED)
|
|
{
|
|
// fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/
|
|
fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
|
|
return 1;
|
|
}
|
|
// 预留：初次上电时，检测压力传感器值是否在零位附近如【-50，+50】
|
|
// if (IV.Press >= -50 && IV.Press <= 50)
|
|
// {
|
|
//// fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/
|
|
// fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
|
|
// return 1;
|
|
// }
|
|
|
|
|
|
|
|
if (P_MK32->IsOnline == 0) //等于0时 subus有数，但是遥控器关机了，或者失联
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/
|
|
fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
|
|
Is_All_Button_Has_Reset = 0; //遥控关机
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|