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.
601 lines
12 KiB
601 lines
12 KiB
/*
|
|
* Function_Booth.c
|
|
*
|
|
* Created on: Mar 21, 2025
|
|
* Author: bihon
|
|
*/
|
|
|
|
#include "BHBF_ROBOT.h"
|
|
#include "Function_Booth.h"
|
|
#include "Timer.h"
|
|
|
|
uint8_t flow_step; //用于控制自动喷漆程序流程
|
|
uint8_t stop_flag; //用于标志自动喷漆流程是否结束
|
|
void Elite_Motion_CMD(start_position_dir dir, uint8_t Order);
|
|
uint8_t Elite_Crash_Detect(uint8_t switchFlag, double Position);
|
|
uint8_t ElevatedCrashMark = 0;
|
|
/*
|
|
* @brief 检查是否通过设定的检查点
|
|
* @param current_Pos 当前位置值
|
|
* @param previous_Pos 初始位置值
|
|
* @param check_Pos 检查点位置值
|
|
* @return 通过检查点返回 1，未通过返回 0
|
|
*/
|
|
int Position_Check(
|
|
double current_Pos, //当前位置
|
|
double previous_Pos, //初始位置
|
|
double check_Pos //检查点
|
|
)
|
|
{
|
|
return ((current_Pos >= check_Pos && previous_Pos < check_Pos) ||
|
|
(current_Pos < check_Pos && previous_Pos >= check_Pos));
|
|
}
|
|
|
|
void VehicleConnectDetect()
|
|
{
|
|
static uint16_t TimeCount = 0;
|
|
static uint8_t LastState = 0xFF;
|
|
static uint8_t BreakCount = 0;
|
|
TimeCount++;
|
|
if(TimeCount % 50 == 0)
|
|
{//每100毫秒读取大臂车心跳数据
|
|
if(TimeCount >= 1500)
|
|
{//3S以上无异常复位
|
|
TimeCount = 0;//时间计数清零
|
|
if(BreakCount < 20)BreakCount = 0;//异常计数清零
|
|
}
|
|
|
|
if(LastState == VehicleHeartReceive)
|
|
{//心跳值未发生变化
|
|
BreakCount++;//异常计数增加
|
|
if(BreakCount > 20)
|
|
{//连续20次无变化（2S）报警
|
|
if(BreakCount % 20 == 0)
|
|
{
|
|
ElevatedCrashMark++;//记录通讯异常次数
|
|
}
|
|
Raspberry_Command(Stop_Painting);//向树莓派发送停止
|
|
}
|
|
}else
|
|
{//心跳值发生变化
|
|
BreakCount = 0;//异常计数清零
|
|
}
|
|
LastState = VehicleHeartReceive;
|
|
}
|
|
}
|
|
|
|
void OrbitOffsetSend()
|
|
{
|
|
static int OffsetSendCount = 0;
|
|
/*👇👇👇发送偏移量👇👇👇*/
|
|
OffsetSendCount++;
|
|
if(OffsetSendCount >= 1000)
|
|
{
|
|
OffsetSendCount = 0;
|
|
SendOffset2Raspberry();
|
|
}
|
|
/*👆👆👆发送偏移量👆👆👆*/
|
|
}
|
|
/**
|
|
* @brief 超声波测距
|
|
*/
|
|
void Position_Detect()
|
|
{
|
|
//读取四个传感器数据，
|
|
static uint8_t ID = 0x00;
|
|
static uint8_t state;
|
|
static uint16_t count = 0;
|
|
switch(state)
|
|
{
|
|
case 0://循环测距1
|
|
count++;
|
|
if(count >= 25)
|
|
{
|
|
ID++; //超声测距传感器ID
|
|
count = 0; //重新计数
|
|
state++;
|
|
}
|
|
break;
|
|
case 1://循环测距2
|
|
GF_Sonic_Inquiry(ID);
|
|
switch(ID)
|
|
{
|
|
//更新距离信息
|
|
case 0x01:
|
|
GV.Vehicle_Cmd.Ranging_Sensor_Data_1 = GV.SonicData.Sonic_1_Data;
|
|
break;
|
|
case 0x02:
|
|
GV.Vehicle_Cmd.Ranging_Sensor_Data_2 = GV.SonicData.Sonic_2_Data;
|
|
break;
|
|
case 0x03:
|
|
GV.Vehicle_Cmd.Ranging_Sensor_Data_3 = GV.SonicData.Sonic_3_Data;
|
|
break;
|
|
case 0x04:
|
|
GV.Vehicle_Cmd.Ranging_Sensor_Data_4 = GV.SonicData.Sonic_4_Data;
|
|
ID = 0;
|
|
break;
|
|
default:
|
|
ID = 0;
|
|
break;
|
|
}
|
|
state = 0;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @brief 手动操作模式
|
|
* @note 对于需机械臂完成的动作，直接向机械臂发送相应指令
|
|
* @note 开关喷枪命令调用开关喷枪函数完成
|
|
* */
|
|
void Manual_Control_Mode()
|
|
{
|
|
static int16_t last_mode;//保存上一次的操作指令
|
|
static int16_t mode; //用于读取当前操作指令
|
|
flow_step = 0; //自动喷漆模式流程置0
|
|
last_mode = mode;
|
|
mode = GV.Vehicle_Cmd.Robot_Mode;//读取当前操作指令
|
|
if(mode != last_mode)//操作指令发生变化
|
|
{
|
|
if(mode >= 1 && mode <= 18)
|
|
{//向机械臂发送对应指令
|
|
Elite_Command((BOperationState)mode);
|
|
}else if(mode == 40)
|
|
{//手动开喷枪
|
|
Paint_Gun_Control(paint_gun_open);//开启喷枪
|
|
}else if(mode == 41)
|
|
{//手动关喷枪
|
|
Paint_Gun_Control(paint_gun_close);//关闭喷枪
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @brief: 机械臂伺服上电
|
|
*/
|
|
void Elite_Power_On()
|
|
{
|
|
Elite_Command(SERVO_POWER_ON_3);
|
|
}
|
|
|
|
/*
|
|
* @brief: 机械臂清除报警
|
|
*/
|
|
void Elite_Clear_Alarm()
|
|
{
|
|
Elite_Command(CLEAR_ALARM_1);
|
|
}
|
|
|
|
/**
|
|
* @brief 正常作业模式
|
|
* @param dir 1：举升车上升；0：举升车下降
|
|
* @return 0:正在作业
|
|
* 1:作业停止
|
|
* 2:意外停机
|
|
*/
|
|
uint8_t StartPainting(start_position_dir dir)
|
|
{
|
|
static uint8_t Count = 0;
|
|
uint8_t ret = 0;
|
|
|
|
switch(flow_step)
|
|
{
|
|
case 0:
|
|
//喷涂开始
|
|
Raspberry_Command(Start_Painting);
|
|
flow_step++;
|
|
break;
|
|
case 1:
|
|
//喷涂结束
|
|
if(StopPainting())
|
|
{
|
|
Raspberry_Command(Stop_Painting);
|
|
flow_step = 2;
|
|
}else
|
|
{
|
|
OrbitOffsetSend();
|
|
}
|
|
break;
|
|
case 2:
|
|
Count++;
|
|
if(Count >= 25)
|
|
{
|
|
Count = 0;
|
|
Raspberry_Command(Stop_Painting);
|
|
flow_step = 3;
|
|
}
|
|
break;
|
|
case 3:
|
|
if(NextCmdPermission())
|
|
{
|
|
flow_step = 0;//流程置零
|
|
Raspberry_Echo_Received();
|
|
ret = 1;
|
|
}else
|
|
{
|
|
Count++;
|
|
if(Count >= 50)
|
|
{
|
|
Count = 0;
|
|
Raspberry_Command(Stop_Painting);
|
|
}
|
|
}
|
|
|
|
if(EliteArmCrashed())
|
|
{
|
|
EliteArmCrash_Reset();
|
|
flow_step = 6;
|
|
}
|
|
break;
|
|
case 6:
|
|
//意外停机
|
|
Raspberry_Command(Painting_Gun_Close);//关喷枪
|
|
flow_step = 0;//流程置零
|
|
ret = 2;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
* @brief 上电试运行
|
|
* @return 1：试运行结束
|
|
* 0：尚未完成试运行
|
|
*/
|
|
uint8_t TrialOperation()
|
|
{
|
|
static uint8_t state = 0;
|
|
uint8_t ret = 0;
|
|
switch(state)
|
|
{
|
|
case 0:
|
|
state++;
|
|
break;
|
|
case 1:
|
|
Raspberry_Command(Trial_Position);
|
|
state++;
|
|
break;
|
|
case 2:
|
|
if(NextCmdPermission() == 1)
|
|
{
|
|
Raspberry_Echo_Received();
|
|
ret = 1;
|
|
state = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* @brief 前往初始位置
|
|
* @return 到达位置（计时结束）返回1,
|
|
* 否则返回0。
|
|
*/
|
|
uint8_t GoToStartPosition()
|
|
{
|
|
static uint8_t state = 0;
|
|
uint8_t ret = 0;
|
|
switch(state)
|
|
{
|
|
case 0:
|
|
state++;
|
|
break;
|
|
case 1:
|
|
Raspberry_Command(Start_Position);//前往初始位置(洗枪位置)
|
|
state++;
|
|
//前往初始上电位置
|
|
break;
|
|
case 2:
|
|
if(NextCmdPermission() == 1)
|
|
{
|
|
Raspberry_Echo_Received();
|
|
ret = 1;
|
|
state = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
* @brief 机械臂前往洗枪位置
|
|
* @return 0:未到达位置
|
|
* 1:到达位置
|
|
*/
|
|
uint8_t GoToWashGunPosition()
|
|
{
|
|
static uint8_t state = 0;
|
|
uint8_t ret = 0;
|
|
switch(state)
|
|
{
|
|
case 0:
|
|
state++;
|
|
break;
|
|
case 1:
|
|
Raspberry_Command(Wash_Gun_Position);//前往初始位置(洗枪位置)
|
|
state++;
|
|
//前往初始上电位置
|
|
break;
|
|
case 2:
|
|
if(NextCmdPermission() == 1)
|
|
{
|
|
Raspberry_Echo_Received();
|
|
ret = 1;
|
|
state = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
* @brief 机械臂前往初始位置
|
|
* @param dir:decend前往初始正位置，ascend前往初始逆位置
|
|
* @return 0:未到达位置
|
|
* 1:到达位置
|
|
*/
|
|
uint8_t GoToPaintPosition(start_position_dir dir)
|
|
{
|
|
static uint8_t state = 0;
|
|
uint8_t ret = 0;
|
|
switch(state)
|
|
{
|
|
case 0:
|
|
if(dir == decend)
|
|
{
|
|
//前往下降喷涂起始位置
|
|
if(InverseStartFlag())
|
|
{//逆位置
|
|
Raspberry_Command(Decline_Painting_3);
|
|
}else
|
|
{//正位置
|
|
Raspberry_Command(Decline_Painting_1);
|
|
}
|
|
}else if(dir == ascend)
|
|
{
|
|
//前往初始逆位置
|
|
if(InverseStartFlag())
|
|
{//逆位置
|
|
Raspberry_Command(Rising_Painting_3);
|
|
}else
|
|
{//正位置
|
|
Raspberry_Command(Rising_Painting_1);
|
|
}
|
|
}
|
|
state++;
|
|
break;
|
|
case 1:
|
|
if(NextCmdPermission() == 1)
|
|
{
|
|
Raspberry_Echo_Received();
|
|
ret = 1;
|
|
state = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
* @brief 机械臂前往停机位置
|
|
* @return 0:未到达位置
|
|
* 1:到达位置
|
|
*/
|
|
uint8_t GoToStopPosition()
|
|
{
|
|
static uint8_t state;
|
|
uint8_t ret = 0;
|
|
switch(state)
|
|
{
|
|
case 0:
|
|
Raspberry_Command(Stopping_Position);
|
|
state++;
|
|
break;
|
|
case 1:
|
|
if(NextCmdPermission() == 1)
|
|
{
|
|
Raspberry_Echo_Received();
|
|
ret = 1;
|
|
state = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* @brief 向机械臂发送指令
|
|
* @param dir 1：举升车上升；0：举升车下降
|
|
*/
|
|
void Elite_Motion_CMD(start_position_dir dir, uint8_t Order)
|
|
{
|
|
if(dir == decend)//大臂车下降
|
|
{
|
|
if(Order == 0)
|
|
{
|
|
//第一段正位置
|
|
Raspberry_Command(Decline_Painting_2);
|
|
}else if(Order == 1)
|
|
{
|
|
//第二段正位置
|
|
Raspberry_Command(Decline_Painting_3);
|
|
}else if(Order == 2)
|
|
{
|
|
Raspberry_Command(Decline_Painting_4);
|
|
}else if(Order == 3)
|
|
{
|
|
Raspberry_Command(Decline_Painting_1);
|
|
}
|
|
}else if(dir == ascend)//大臂车上升
|
|
{
|
|
if(Order == 0)
|
|
{
|
|
//第一段逆位置
|
|
Raspberry_Command(Rising_Painting_2);
|
|
}else if(Order == 1)
|
|
{
|
|
//第二段逆位置
|
|
Raspberry_Command(Rising_Painting_3);
|
|
}else if(Order == 2)
|
|
{
|
|
Raspberry_Command(Rising_Painting_4);
|
|
}else if(Order == 3)
|
|
{
|
|
Raspberry_Command(Rising_Painting_1);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief 控制喷枪开关
|
|
* @param state 喷枪开关状态，paint_gun_close表示关闭，paint_gun_open表示开启
|
|
*/
|
|
void Paint_Gun_Control(paint_gun_state state)
|
|
{
|
|
switch(state)
|
|
{
|
|
case paint_gun_open:
|
|
//喷枪开启
|
|
Raspberry_Command(Painting_Gun_Open);
|
|
break;
|
|
case paint_gun_close:
|
|
//喷枪关闭
|
|
Raspberry_Command(Painting_Gun_Close);
|
|
break;
|
|
}
|
|
}
|
|
/*
|
|
* @brief: 远程启动函数
|
|
* @note： 1.伺服上电
|
|
* 2.清除报警
|
|
* 3.“精确”
|
|
*/
|
|
void Elite_Remote_PowerOn()
|
|
{
|
|
static uint8_t state;
|
|
static uint8_t cnt;
|
|
HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, GPIO_PIN_RESET);//RESET 远程启动继电器上电
|
|
// HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, GPIO_PIN_SET);//SET 远程启动继电器断电
|
|
switch(state)
|
|
{
|
|
case 0:
|
|
Elite_Command(SERVO_POWER_ON_3);//伺服上电
|
|
Timer_Set(10000, 0);
|
|
state++;
|
|
break;
|
|
case 1:
|
|
if(Timer_Start_Count() == 1)
|
|
{
|
|
Elite_Command(CLEAR_ALARM_1);//清除报警
|
|
cnt++;
|
|
if(cnt >= 3)
|
|
{
|
|
cnt = 0;
|
|
state = 2;
|
|
}else
|
|
{
|
|
Timer_Set(2000, 0);
|
|
state = 1;
|
|
}
|
|
}
|
|
break;
|
|
case 2:
|
|
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
void Elite_Remote_PowerOn2()
|
|
{
|
|
HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, GPIO_PIN_RESET);
|
|
HAL_Delay(3000);
|
|
HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, GPIO_PIN_SET);
|
|
HAL_Delay(30000);
|
|
for(int i = 0; i < 3; i++)
|
|
{
|
|
Elite_Command(CLEAR_ALARM_1); //清除报警
|
|
HAL_Delay(500);
|
|
}
|
|
for(int i = 0; i < 3; i++)
|
|
{
|
|
Elite_Command(SERVO_POWER_ON_3);//伺服上电
|
|
HAL_Delay(500);
|
|
}
|
|
Elite_Command(ARM_PRECISE_9); //"精确"
|
|
HAL_Delay(1000);
|
|
}
|
|
|
|
/*
|
|
* @brief 机械臂意外停止检测函数
|
|
* @param switchFlag：开启1/关闭0标志
|
|
* @return 0:未发生意外停止或未在检测
|
|
* 1：机械臂意外停止
|
|
* @note 通过检测机械臂关节电机1位置信息来判断机械臂是否发生碰撞报警或急停
|
|
* @note 仅可以检测机械臂运动过程中的意外停止
|
|
* @note 机械臂停止运动时会给出信号，也可以检测运动过程是否结束
|
|
*/
|
|
uint8_t Elite_Crash_Detect(uint8_t switchFlag, double Position)
|
|
{
|
|
// 计数变量，每调用20次函数进行一次位置比较
|
|
static int cycleCount = 0;
|
|
// 记录当前位置与前一次位置相等的次数
|
|
static int equalPositionCount = 0;
|
|
// 记录前一次的位置信息
|
|
static double previousPosition = 0.0;
|
|
// 记录当前的位置信息
|
|
static double currentPosition = 0.0;
|
|
if(switchFlag == 1)
|
|
{
|
|
//开启检测
|
|
cycleCount++;
|
|
if(cycleCount >= 20)
|
|
{
|
|
cycleCount = 0;//重新计数
|
|
// 每20次调用函数时(40ms)，对比关节位置信息
|
|
previousPosition = currentPosition;
|
|
currentPosition = Position;
|
|
|
|
// 定义一个位置变化的阈值，避免因微小误差导致误判
|
|
const double positionThreshold = 0.001;
|
|
if(fabs(currentPosition - previousPosition) < positionThreshold)
|
|
{
|
|
equalPositionCount++;
|
|
}else
|
|
{
|
|
equalPositionCount = 0;
|
|
}
|
|
if(equalPositionCount == 5)
|
|
{
|
|
// 连续5次位置未发生变化，判定为意外停止
|
|
equalPositionCount = 0;
|
|
return 1;
|
|
}else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}else
|
|
{
|
|
//停止检测
|
|
cycleCount = 0;
|
|
equalPositionCount = 0;
|
|
return 0;
|
|
}
|
|
return 0;
|
|
}
|
|
|