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.
746 lines
19 KiB
746 lines
19 KiB
/*
|
|
* StateManager.c
|
|
*
|
|
* Created on: Mar 24, 2025
|
|
* Author: bihon
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include "BHBF_ROBOT.h"
|
|
#include "bsp_include.h"
|
|
#include "StateManager.h"
|
|
|
|
static uint8_t ManualControlMark = 0; //记录调用手动控制模式时的状态，便于恢复
|
|
static uint8_t RecoverMark = 0; //恢复标记，0表示正常调用，1表示恢复状态
|
|
|
|
// 当前状态变量
|
|
Paint_Mode currentModeCMD = STATE_STANDBY;//喷涂模式初始为待机模式
|
|
Paint_Control currentControlCMD = INVALID_CMD;//喷涂控制初始为无效指令
|
|
|
|
// 模拟接收停止信号
|
|
uint8_t stopSignalReceived = 0;
|
|
start_position_dir paintDir = notwork;
|
|
|
|
uint8_t InverseStart_Flag;
|
|
void InverseStartSet()
|
|
{
|
|
InverseStart_Flag = 1;
|
|
}
|
|
void InverseStartReset()
|
|
{
|
|
InverseStart_Flag = 0;
|
|
}
|
|
uint8_t InverseStartFlag()
|
|
{
|
|
return InverseStart_Flag;
|
|
}
|
|
|
|
uint8_t StopPaintingFlag;
|
|
void StopPaintingSet()
|
|
{
|
|
StopPaintingFlag = 1;
|
|
}
|
|
void StopPaintingReset()
|
|
{
|
|
StopPaintingFlag = 0;
|
|
}
|
|
uint8_t StopPainting()
|
|
{
|
|
return StopPaintingFlag;
|
|
}
|
|
/*
|
|
* @brief: 控制程序切换到手动控制状态
|
|
* @Param: CallMark: 用于记录调用时的程序状态，用于恢复
|
|
*/
|
|
void switch2ManualState()
|
|
{
|
|
Paint_Gun_Control(paint_gun_close);//喷枪关闭（默认值）
|
|
paintDir = notwork;//未作业（默认值）
|
|
currentControlCMD = INVALID_CMD;//无效状态（默认值）
|
|
currentModeCMD = STATE_MANUAL_CONTROL;//手动控制状态
|
|
}
|
|
|
|
/*
|
|
* @brief: 控制程序切换到喷漆作业状态
|
|
*/
|
|
void switch2PaintState()
|
|
{
|
|
Paint_Gun_Control(paint_gun_close);//喷枪关闭（默认值）
|
|
paintDir = notwork;//未作业（默认值）
|
|
|
|
currentModeCMD = STATE_PAINTING;//喷漆状态
|
|
switch(ManualControlMark)
|
|
{
|
|
case 0:
|
|
RecoverMark = 0;
|
|
currentControlCMD = INVALID_CMD;//无效状态（默认值）
|
|
break;
|
|
case 1:
|
|
RecoverMark = 1;
|
|
currentControlCMD = GOTO_STARTPOSITION;//初始位置
|
|
break;
|
|
case 7://洗枪位置
|
|
RecoverMark = 1;
|
|
currentControlCMD = GOTO_WASH_GUN_POSITION;//洗枪位置
|
|
break;
|
|
case 8:
|
|
RecoverMark = 1;
|
|
currentControlCMD = TRIAL_POSITION;//试枪位置
|
|
break;
|
|
default:
|
|
RecoverMark = 0;
|
|
currentControlCMD = INVALID_CMD;//无效状态（默认值）
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @brief: 控制程序切换到异常状态
|
|
*/
|
|
void switch2AbnormalState()
|
|
{
|
|
Paint_Gun_Control(paint_gun_close);//喷枪关闭（默认值）
|
|
paintDir = notwork;//未作业（默认值）
|
|
currentControlCMD = INVALID_CMD;//无效状态（默认值）
|
|
currentModeCMD = STATE_ABNORMAL;//异常状态
|
|
}
|
|
|
|
/*
|
|
* @brief: 控制程序切换到待机状态
|
|
*/
|
|
void switch2StandByState()
|
|
{
|
|
Paint_Gun_Control(paint_gun_close);//喷枪关闭（默认值）
|
|
paintDir = notwork;//未作业（默认值）
|
|
currentControlCMD = INVALID_CMD;//无效状态（默认值）
|
|
currentModeCMD = STATE_STANDBY;//待机状态
|
|
}
|
|
|
|
/*
|
|
* @brief 手动作业状态下的模式切换
|
|
* @note 除执行手动操作指令外，可切换至喷漆控制模式
|
|
*/
|
|
void handleManualControlState()
|
|
{
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0xFF; //位于停机位置
|
|
GV.Vehicle_Cmd.Target_Position = GV.Vehicle_Cmd.Paint_Gun_Control; //回传接收到的指令高字节
|
|
GV.Vehicle_Cmd.Paint_Process = GV.Vehicle_Cmd.Paint_Speed; //回传接收到的指令低字节
|
|
/*👆设备状态👆*/
|
|
|
|
Manual_Control_Mode();
|
|
|
|
//运行状态切换
|
|
|
|
if (GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x00)
|
|
{//切换到喷漆状态 RX:0x01 0x00 0x00 0x00
|
|
switch2PaintState();//根据ManualControlMark恢复
|
|
}
|
|
|
|
LOG("STATE: ManualState");
|
|
}
|
|
uint8_t ArmSpeedSet;
|
|
void WorkModeSet()
|
|
{
|
|
static uint8_t LastSpeed = 0;
|
|
|
|
if(LastSpeed != VehicleSetSpeedData)
|
|
{
|
|
Raspberry_Command_SpeedSet(VehicleSetSpeedData);
|
|
LastSpeed = VehicleSetSpeedData;
|
|
}
|
|
}
|
|
/*
|
|
* @Brief: 无效指令处理
|
|
* @Note: 机械臂上电默认处于无效指令状态，
|
|
* @Note: 此状态下仅可以接收【前往初始位置】命令
|
|
*/
|
|
void handleInvalidCMD()
|
|
{
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x00; //位于停机位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //未收到结束指令
|
|
/*👆设备状态👆*/
|
|
|
|
// WorkModeSet();
|
|
|
|
if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
|
|
{//前往初始位置 RX:0x01 0x01 0x00 0x00
|
|
currentControlCMD = GOTO_STARTPOSITION;
|
|
}
|
|
LOG("STATE: InvalidState");
|
|
}
|
|
|
|
uint8_t Trial_Mark;//试运行标志，0表示未试运行，1表示已进行试运行
|
|
|
|
void handleTrialPositionCMD()
|
|
{
|
|
static uint8_t state = 0;
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //试运行
|
|
switch(state)
|
|
{
|
|
case 0:
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x04; //前往目标位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x08; //试枪位置
|
|
LOG("PAINTSTATE: TrialOperation");
|
|
if(RecoverMark == 1)
|
|
{
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x08; //位于初始位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
RecoverMark = 0;
|
|
state++;
|
|
}
|
|
if(TrialOperation() == 1)
|
|
{
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x08; //位于初始位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: AtTrailPosition");
|
|
|
|
state++;
|
|
}
|
|
break;
|
|
case 1:
|
|
if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
|
|
{//切换到手动模式 RX:0x00 0xAA 0x00 0x00
|
|
switch2ManualState();
|
|
ManualControlMark = 8;
|
|
state = 0;
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
|
|
{//前往初始位置 RX:0x01 0x01 0x00 0x00
|
|
currentControlCMD = GOTO_STARTPOSITION;
|
|
state = 0;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @Brief: 前往初始位置指令处理
|
|
* @Note: 可通过该函数命令机械臂【前往上升喷涂起点】或【前往下降喷涂起点】
|
|
* @Note: 结束作业后可接收【前往停机位置】命令
|
|
*/
|
|
void handleStartPosCMD()
|
|
{
|
|
static uint8_t state = 0;
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //未收到结束指令
|
|
/*👆设备状态👆*/
|
|
WorkModeSet();
|
|
switch(state)
|
|
{
|
|
case 0://执行动作
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x04; //前往目标位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x01; //初始位置
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: GoToStartPosition");
|
|
if(RecoverMark == 1)
|
|
{
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x01; //位于初始位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
RecoverMark = 0;
|
|
state++;
|
|
}
|
|
if(GoToStartPosition() == 1)
|
|
{//已到达起始位置
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x01; //位于初始位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: AtStartPosition");
|
|
|
|
state++;
|
|
}
|
|
break;
|
|
case 1://等待指令
|
|
if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x02)
|
|
{//前往下降喷涂起点 RX:0x01 0x02 0x00 0x00
|
|
if(GV.Vehicle_Cmd.Paint_Gun_Control == 0xFF && GV.Vehicle_Cmd.Paint_Speed == 0xFF)
|
|
{
|
|
InverseStartSet();
|
|
}else
|
|
{
|
|
InverseStartReset();
|
|
}
|
|
currentControlCMD = GOTO_DESENDPAINTING_STARTPOSITION;
|
|
state = 0;
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x03)
|
|
{//前往上升喷涂起点 RX:0x01 0x03 0x00 0x00
|
|
if(GV.Vehicle_Cmd.Paint_Gun_Control == 0xFF && GV.Vehicle_Cmd.Paint_Speed == 0xFF)
|
|
{
|
|
InverseStartSet();
|
|
}else
|
|
{
|
|
InverseStartReset();
|
|
}
|
|
currentControlCMD = GOTO_ASCENDPAINTING_STARTPOSITION;
|
|
state = 0;
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x06)
|
|
{//前往停机位置 RX:0x01 0x06 0x00 0x00
|
|
currentControlCMD = GOTO_STOPPOSITION;
|
|
state = 0;
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x07)
|
|
{
|
|
currentControlCMD = GOTO_STOPPOSITION;
|
|
state = 0;
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
|
|
{//切换到手动模式 RX:0x00 0xAA 0x00 0x00
|
|
ManualControlMark = 7;
|
|
switch2ManualState();
|
|
state = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @Brief: 前往下降喷涂起点指令处理
|
|
* @Note: 机械臂执行前往下降喷涂起点动作
|
|
* @Note: 到达位置后可接收【前往初始位置】指令或【开始喷涂】指令
|
|
*/
|
|
void handleDesendPaintCMD()
|
|
{
|
|
static uint8_t state;
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //未收到结束指令
|
|
/*👆设备状态👆*/
|
|
|
|
switch(state)
|
|
{
|
|
case 0://执行动作
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x04; //前往目标位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x02; //下降喷涂起点
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: GoToDesendPaintPosition");
|
|
|
|
if(GoToPaintPosition(decend) == 1)
|
|
{//已到达下降喷涂起点
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x02; //位于下降喷涂起点
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: AtDesendPaintPosition");
|
|
|
|
state++;
|
|
}
|
|
break;
|
|
case 1://等待指令
|
|
if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
|
|
{//回到起始位置 RX:0x01 0x01 0x00 0x00
|
|
currentControlCMD = GOTO_STARTPOSITION;
|
|
state = 0;
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x04)
|
|
{//开始喷涂 RX:0x01 0x04 0x00 0x00
|
|
currentControlCMD = START_PAINTING;
|
|
paintDir = decend;
|
|
state = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @Brief: 前往上升喷涂起点指令处理
|
|
* @Note: 机械臂执行前往下上升喷涂起点动作
|
|
* @Note: 到达位置后可接收【前往初始位置】指令或【开始喷涂】指令
|
|
*/
|
|
void handleAscendPaintCMD()
|
|
{
|
|
static uint8_t state;
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //未收到结束指令
|
|
/*👆设备状态👆*/
|
|
|
|
switch(state)
|
|
{
|
|
case 0:
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x04; //前往目标位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x03; //上升喷涂起点
|
|
/*👆设备状态👆*/
|
|
LOG("PAINTSTATE: GoToAscendPaintPosition");
|
|
if(GoToPaintPosition(ascend) == 1)
|
|
{//已到达下降喷涂起点
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x03; //位于下降喷涂起点
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: AtAscendPaintPosition");
|
|
|
|
state++;
|
|
}
|
|
break;
|
|
case 1:
|
|
if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
|
|
{//回到起始位置 RX:0x01 0x01 0x00 0x00
|
|
currentControlCMD = GOTO_STARTPOSITION;
|
|
state = 0;
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x04)
|
|
{//开始喷涂 RX:0x01 0x04 0x00 0x00
|
|
currentControlCMD = START_PAINTING;
|
|
paintDir = ascend;
|
|
state = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @Brief: 开始喷涂指令处理
|
|
* @Note: 按照作业方向进行喷涂作业
|
|
* @Note: 可接受停止喷涂指令，
|
|
* 在执行完当前喷涂后进入结束喷涂状态
|
|
*/
|
|
void handleStartPaintCMD()
|
|
{
|
|
uint8_t paintstate;
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
|
|
if(paintDir == decend || paintDir == ascend)
|
|
{//作业方向为上升或下降
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x05; //正在进行喷涂
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //未收到结束指令
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: AutoPaintingRuning...");
|
|
|
|
paintstate = StartPainting(paintDir);
|
|
|
|
}else
|
|
{//收到错误方向
|
|
|
|
paintDir = notwork;//恢复默认值
|
|
|
|
LOG("ERROR: Invalid <paintDir> ");
|
|
|
|
}
|
|
|
|
if((GV.Vehicle_Cmd.Paint_Mode == 0x01) && (GV.Vehicle_Cmd.Paint_Control == 0x05))
|
|
{//收到结束喷涂指令
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x05; //正在进行喷涂
|
|
GV.Vehicle_Cmd.Paint_Process = 0x01; //收到结束指令（待执行）
|
|
/*👆设备状态👆*/
|
|
StopPaintingSet();
|
|
LOG("PAINTSTATE: Received Stop Command.");
|
|
}
|
|
|
|
if(paintstate == 1)
|
|
{//正常停止
|
|
|
|
currentControlCMD = FINISH_PAINTING;
|
|
paintDir = notwork;
|
|
StopPaintingReset();
|
|
LOG("PAINTSTATE: Paint Finished.");
|
|
|
|
}else if(paintstate == 2)
|
|
{//触发急停
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x65; //触发急停（101）
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x05; //正在进行喷涂
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //未收到结束指令（默认值）
|
|
/*👆设备状态👆*/
|
|
|
|
switch2AbnormalState();
|
|
|
|
LOG("PAINTSTATE: Emergency Stop.");
|
|
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @Brief: 结束喷涂指令处理
|
|
* @Note: 可接受【前往初始位置】指令
|
|
*/
|
|
void handleFinishPaintCMD()
|
|
{//无动作执行
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x06; //喷涂完成
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //未收到结束指令
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PaintSTATE: Finish.");
|
|
|
|
//等待指令
|
|
// if((GV.Vehicle_Cmd.Paint_Mode == 0x01) && (GV.Vehicle_Cmd.Paint_Control = 0x01))
|
|
// {//前往初始位置 RX:0x01 0x01 0x00 0x00
|
|
currentControlCMD = GOTO_STARTPOSITION;
|
|
// }
|
|
}
|
|
|
|
/*
|
|
* @Brief: 前往停机位置指令处理
|
|
* @Note: 机械臂执行前往停机位置动作
|
|
* @Note: 到达停机位置后可接受【前往初始位置】指令
|
|
*/
|
|
void handleStopPositionCMD()
|
|
{
|
|
static uint8_t state;
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //未收到结束指令
|
|
/*👆设备状态👆*/
|
|
|
|
switch(state)
|
|
{
|
|
case 0://执行动作
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x04; //前往目标位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x06; //停机位置
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: GoToStopPosition.");
|
|
|
|
if(GoToStopPosition() == 1)
|
|
{//已到达停机位置
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x00; //位于停机位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: AtStopPosition.");
|
|
|
|
state++;
|
|
}
|
|
break;
|
|
case 1://切换到待机状态
|
|
switch2StandByState();
|
|
state = 0;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @Brief: 前往洗枪位置指令处理
|
|
* @Note: 机械臂执行前往洗枪位置动作
|
|
* @Note: 到达洗枪位置后可接受【前往初始位置】指令或执行【切换为手动状态】
|
|
*/
|
|
void handleWashGunPositionCMD()
|
|
{
|
|
static uint8_t state;
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0x01; //设备正常
|
|
GV.Vehicle_Cmd.Paint_Process = 0x00; //未收到结束指令
|
|
/*👆设备状态👆*/
|
|
|
|
switch(state)
|
|
{
|
|
case 0://执行动作
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x04; //前往目标位置
|
|
GV.Vehicle_Cmd.Target_Position = 0x07; //下降喷涂起点
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: GoToDesendPaintPosition");
|
|
if(RecoverMark == 1)
|
|
{//执行状态恢复，已在洗枪位置
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x07; //位于下降喷涂起点
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
RecoverMark = 0;
|
|
state++;
|
|
}
|
|
if(GoToWashGunPosition() == 1)
|
|
{//已到达洗枪位置
|
|
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0x07; //位于下降喷涂起点
|
|
GV.Vehicle_Cmd.Target_Position = 0x00; //无目标位置
|
|
/*👆设备状态👆*/
|
|
|
|
LOG("PAINTSTATE: AtDesendPaintPosition");
|
|
|
|
state++;
|
|
}
|
|
break;
|
|
case 1://等待指令
|
|
if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
|
|
{//切换到手动模式 RX:0x00 0xAA 0x00 0x00
|
|
switch2ManualState();
|
|
ManualControlMark = 7;
|
|
state = 0;
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x01)
|
|
{//前往初始位置 RX:0x01 0x01 0x00 0x00
|
|
currentControlCMD = GOTO_STARTPOSITION;
|
|
state = 0;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* @Brief: 喷漆作业模式
|
|
* @Brief: 对喷漆流程进行控制，结束喷涂后主状态切换为待机状态
|
|
*/
|
|
void handlePaintingState(){
|
|
|
|
switch(currentControlCMD)
|
|
{
|
|
case INVALID_CMD://无效指令
|
|
handleInvalidCMD();
|
|
break;
|
|
case GOTO_STARTPOSITION://机械臂前往初始位置
|
|
handleStartPosCMD();
|
|
break;
|
|
case TRIAL_POSITION:
|
|
handleTrialPositionCMD();
|
|
break;
|
|
case GOTO_DESENDPAINTING_STARTPOSITION://机械臂前往下降喷涂起点
|
|
handleDesendPaintCMD();
|
|
break;
|
|
case GOTO_ASCENDPAINTING_STARTPOSITION://机械臂前往上升喷涂起点
|
|
handleAscendPaintCMD();
|
|
break;
|
|
case START_PAINTING://开始喷涂
|
|
handleStartPaintCMD();
|
|
break;
|
|
case FINISH_PAINTING://结束喷涂
|
|
handleFinishPaintCMD();
|
|
break;
|
|
case GOTO_STOPPOSITION://前往停机位置
|
|
handleStopPositionCMD();//到达停机位置程序自动切换到待机模式
|
|
break;
|
|
case GOTO_WASH_GUN_POSITION:
|
|
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
LOG("STATE: PaintingState");
|
|
}
|
|
|
|
/*
|
|
* @brief: 待机模式
|
|
* @note: 可将主状态切换到手动控制模式或喷漆作业模式
|
|
*/
|
|
void handleStandbyState()
|
|
{
|
|
// static uint8_t state;
|
|
static uint8_t paintgunstate = 0;
|
|
/*👇设备状态👇*/
|
|
GV.Vehicle_Cmd.Device_Status = 0xFF; //设备正在启动
|
|
GV.Vehicle_Cmd.Paint_Stutas = 0xFF; //位于停机位置
|
|
GV.Vehicle_Cmd.Target_Position = 0xFF; //无目标位置
|
|
GV.Vehicle_Cmd.Paint_Process = 0xFF; //未收到结束指令
|
|
/*👆设备状态👆*/
|
|
|
|
if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
|
|
{//切换为手动控制模式 RX:0x00 0xAA 0x00 0x00
|
|
switch2ManualState();
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x01 && GV.Vehicle_Cmd.Paint_Control == 0x00)
|
|
{//切换为喷漆作业模式 RX:0x01 0x00 0x00 0x00
|
|
ManualControlMark = 0;
|
|
switch2PaintState();
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x55 && GV.Vehicle_Cmd.Paint_Control == 0xFF && !paintgunstate)
|
|
{
|
|
Paint_Gun_Control(paint_gun_open);//开启喷枪
|
|
paintgunstate = 1;
|
|
}else if(GV.Vehicle_Cmd.Paint_Mode == 0x55 && GV.Vehicle_Cmd.Paint_Control == 0xAA && paintgunstate)
|
|
{
|
|
Paint_Gun_Control(paint_gun_close);//关闭喷枪
|
|
paintgunstate = 0;
|
|
}
|
|
|
|
LOG("STATE: StandbyState");
|
|
}
|
|
|
|
/*
|
|
* @brief: 异常状态处理
|
|
* @note: 处理急停
|
|
* @note: 该状态下仅可以进入手动控制模式
|
|
*/
|
|
void handleAbnormalState()
|
|
{
|
|
|
|
if(GV.Vehicle_Cmd.Paint_Mode == 0x00 && GV.Vehicle_Cmd.Paint_Control == 0xAA)
|
|
{//切换为手动控制模式 RX:0x00 0xAA 0x00 0x00
|
|
Elite_Command(CLEAR_ALARM_1);
|
|
switch2ManualState();
|
|
}
|
|
|
|
LOG("STATE: AbnormalState");
|
|
}
|
|
|
|
/*
|
|
* @brief: 状态机主循环函数
|
|
* @note: 在待机模式/手动控制模式/喷漆作业模式间进行切换
|
|
*/
|
|
void stateMachineLoop() {
|
|
switch (currentModeCMD) {
|
|
case STATE_MANUAL_CONTROL://手动控制
|
|
handleManualControlState();
|
|
break;
|
|
case STATE_PAINTING://喷漆作业
|
|
handlePaintingState();
|
|
break;
|
|
case STATE_STANDBY://待机状态
|
|
handleStandbyState();
|
|
break;
|
|
case STATE_ABNORMAL://异常状态
|
|
handleAbnormalState();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
LOG("FUNCTION: stateMachineLoop");
|
|
}
|
|
|