195 lines
6.4 KiB
C++
195 lines
6.4 KiB
C++
#include "AxisState.h"
|
||
#include "../Communication/S7Command.h"
|
||
#include "../config/ConfigManager.h"
|
||
#include "../Logger.h"
|
||
|
||
AxisState::AxisState()
|
||
{
|
||
|
||
}
|
||
|
||
void AxisState::GetValue(AxisStateValue& value)
|
||
{
|
||
std::shared_lock<std::shared_mutex> lock(mtx);
|
||
value.ServoOn = m_ServoOn;
|
||
value.ServoHomeIndexOn = m_ServoHomeIndexOn;
|
||
value.ServoReset = m_ServoReset;
|
||
value.MoveP = m_MoveP;
|
||
value.MoveN = m_MoveN;
|
||
value.MovePContinue = m_MovePContinue;
|
||
value.MoveNContinue = m_MoveNContinue;
|
||
value.MoveAbsPos = m_MoveAbsPos;
|
||
value.MotionStop = m_MotionStop;
|
||
value.ServoRDY = m_ServoRDY;
|
||
value.ServoHomeDone = m_ServoHomeDone;
|
||
value.ResetDone = m_ResetDone;
|
||
value.ServoException = m_ServoException;
|
||
value.Runable = m_Runable;
|
||
value.ExceptionCode = m_ExceptionCode;
|
||
value.Pos = m_Pos;
|
||
value.Torque = m_Torque;
|
||
value.ServoBusy = m_ServoBusy;
|
||
}
|
||
|
||
void AxisState::Update(unsigned char* addr)
|
||
{
|
||
std::unique_lock<std::shared_mutex> lock(mtx);
|
||
m_ServoOn= ((addr[0] & 0x1)>0 ? true : false);
|
||
m_ServoHomeIndexOn= ((addr[0] & 0x2)>0 ? true : false);
|
||
m_ServoReset= ((addr[0] & 0x4)>0 ? true : false);
|
||
m_MoveP= ((addr[0] & 0x8)>0 ? true : false);
|
||
m_MoveN= ((addr[0] & 0x10)>0 ? true : false);
|
||
m_MovePContinue= ((addr[0] & 0x20)>0 ? true : false);
|
||
m_MoveNContinue= ((addr[0] & 0x40)>0 ? true : false);
|
||
m_MoveAbsPos= ((addr[0] & 0x80)>0 ? true : false);
|
||
m_MotionStop= ((addr[1] & 0x1)>0 ? true : false);
|
||
m_ServoRDY= ((addr[1] & 0x2)>0 ? true : false);
|
||
m_ServoHomeDone= ((addr[1] & 0x4)>0 ? true : false);
|
||
m_ResetDone= ((addr[1] & 0x8)>0 ? true : false);
|
||
m_ServoBusy= ((addr[1] & 0x10)>0 ? true : false);
|
||
m_ServoException= ((addr[1] & 0x20)>0 ? true : false);
|
||
m_Runable= ((addr[1] & 0x40)>0 ? true : false);
|
||
m_ExceptionCode = S7WORDDATA(addr[3], addr[2]).wValue;
|
||
}
|
||
|
||
void AxisState::UpdatePosAndLoad(unsigned char* addr)
|
||
{
|
||
int index = 0;
|
||
std::unique_lock<std::shared_mutex> lock(mtx);
|
||
m_Pos = (S7FLOATDATA(addr[index + 3], addr[index + 2], addr[index + 1], addr[index]).fValue); index += 4;
|
||
m_Torque = (S7FLOATDATA(addr[index + 3], addr[index + 2], addr[index + 1], addr[index]).fValue); index += 4;
|
||
|
||
if (!m_AxisCfg->m_ShowPosInv) {
|
||
m_ShowPos = m_Pos - m_AxisCfg->m_ShowRefZero;
|
||
}
|
||
else {
|
||
m_ShowPos = -(m_Pos - m_AxisCfg->m_ShowRefZero);
|
||
}
|
||
|
||
m_RPos = m_ShowPos / (m_AxisCfg->m_active_limit - m_AxisCfg->m_negactive_limit);
|
||
}
|
||
|
||
float AxisState::GetShowPos()
|
||
{
|
||
float showPos = 0.0f;
|
||
std::unique_lock<std::shared_mutex> lock(mtx);
|
||
showPos = m_ShowPos;
|
||
return showPos;
|
||
}
|
||
|
||
void AxisState::SetZeroPos()
|
||
{
|
||
m_AxisCfg->m_ShowRefZero = m_Pos;
|
||
ConfigManager::GetInstance()->UpdateZeroOffset(m_AxisCfg->m_axis_id, m_AxisCfg->m_ShowRefZero);
|
||
}
|
||
|
||
void MainAxisState::GetValue(AxisStateValue& value)
|
||
{
|
||
std::shared_lock<std::shared_mutex> lock(mtx);
|
||
value.ServoOn = m_ServoOn;
|
||
value.ServoHomeIndexOn = m_ServoHomeIndexOn;
|
||
value.ServoReset = m_ServoReset;
|
||
value.MoveP = m_MoveP;
|
||
value.MoveN = m_MoveN;
|
||
value.MovePContinue = m_MovePContinue;
|
||
value.MoveNContinue = m_MoveNContinue;
|
||
value.MoveAbsPos = m_MoveAbsPos;
|
||
value.MotionStop = m_MotionStop;
|
||
value.ServoRDY = m_ServoRDY;
|
||
value.ServoHomeDone = m_ServoHomeDone;
|
||
value.ResetDone = m_ResetDone;
|
||
value.ServoException = m_ServoException;
|
||
value.Runable = m_Runable;
|
||
value.ExceptionCode = m_ExceptionCode;
|
||
value.Pos = m_Pos;
|
||
value.Torque = m_Torque;
|
||
value.ShowPos = m_ShowPos;
|
||
value.RPos = m_RPos;
|
||
value.ServoBreakOn = m_ServoBreakOn;
|
||
value.BindSlaveOn = m_BindSlaveOn;
|
||
value.BindSlaveFinish = m_BindSlaveFinish;
|
||
value.ServoBusy = m_ServoBusy;
|
||
}
|
||
|
||
|
||
void MainAxisState::Update(unsigned char* addr)
|
||
{
|
||
std::unique_lock<std::shared_mutex> lock(mtx);
|
||
m_ServoOn= ((addr[0] & 0x1)>0 ? true : false);
|
||
m_ServoHomeIndexOn= ((addr[0] & 0x2)>0 ? true : false);
|
||
m_ServoReset= ((addr[0] & 0x4)>0 ? true : false);
|
||
m_ServoBreakOn= ((addr[0] & 0x8)>0 ? true : false);
|
||
m_BindSlaveOn= ((addr[0] & 0x10)>0 ? true : false);
|
||
m_MoveP= ((addr[0] & 0x20)>0 ? true : false);
|
||
m_MoveN= ((addr[0] & 0x40)>0 ? true : false);
|
||
m_MovePContinue= ((addr[0] & 0x80)>0 ? true : false);
|
||
m_MoveNContinue= ((addr[1] & 0x1)>0 ? true : false);
|
||
m_MoveAbsPos= ((addr[1] & 0x2)>0 ? true : false);
|
||
m_MotionStop= ((addr[1] & 0x4)>0 ? true : false);
|
||
m_ServoRDY= ((addr[1] & 0x8)>0 ? true : false);
|
||
m_ServoHomeDone= ((addr[1] & 0x10)>0 ? true : false);
|
||
m_ResetDone= ((addr[1] & 0x20)>0 ? true : false);
|
||
bool lastServoBusy = m_ServoBusy;
|
||
m_ServoBusy= ((addr[1] & 0x40)>0 ? true : false);
|
||
// if (lastServoBusy && !m_ServoBusy) {
|
||
// g_log->TraceInfo("<22><>æת<C3A6><D7AA>Ϊ<EFBFBD><CEAA>%d",m_ServoBusy?1:0);
|
||
// }
|
||
|
||
m_ServoException= ((addr[1] & 0x80)>0 ? true : false);
|
||
m_BindSlaveFinish= ((addr[2] & 0x1)>0 ? true : false);
|
||
m_Runable= ((addr[2] & 0x2)>0 ? true : false);
|
||
m_ExceptionCode = S7WORDDATA(addr[5], addr[4]).wValue;
|
||
}
|
||
|
||
void SlaveAxisState::GetValue(AxisStateValue& value)
|
||
{
|
||
std::shared_lock<std::shared_mutex> lock(mtx);
|
||
value.ServoOn = m_ServoOn;
|
||
value.ServoHomeIndexOn = m_ServoHomeIndexOn;
|
||
value.ServoReset = m_ServoReset;
|
||
value.MoveP = m_MoveP;
|
||
value.MoveN = m_MoveN;
|
||
value.MovePContinue = m_MovePContinue;
|
||
value.MoveNContinue = m_MoveNContinue;
|
||
value.MoveAbsPos = m_MoveAbsPos;
|
||
value.MotionStop = m_MotionStop;
|
||
value.ServoRDY = m_ServoRDY;
|
||
value.ServoHomeDone = m_ServoHomeDone;
|
||
value.ResetDone = m_ResetDone;
|
||
value.ServoException = m_ServoException;
|
||
value.Runable = m_Runable;
|
||
value.ExceptionCode = m_ExceptionCode;
|
||
value.Pos = m_Pos;
|
||
value.Torque = m_Torque;
|
||
value.ShowPos = m_ShowPos;
|
||
value.RPos = m_RPos;
|
||
value.SlaveServoBreakOn = m_ServoBreakOn;
|
||
value.SlaveUnbind = m_Unbind;
|
||
value.ServoBusy = m_ServoBusy;
|
||
}
|
||
|
||
void SlaveAxisState::Update(unsigned char* addr)
|
||
{
|
||
std::unique_lock<std::shared_mutex> lock(mtx);
|
||
m_ServoOn= ((addr[0] & 0x1)>0 ? true : false);
|
||
m_ServoHomeIndexOn= ((addr[0] & 0x2)>0 ? true : false);
|
||
m_ServoReset= ((addr[0] & 0x4)>0 ? true : false);
|
||
m_ServoBreakOn= ((addr[0] & 0x8)>0 ? true : false);
|
||
m_Unbind= ((addr[0] & 0x10)>0 ? true : false);
|
||
m_MoveP= ((addr[0] & 0x20)>0 ? true : false);
|
||
m_MoveN= ((addr[0] & 0x40)>0 ? true : false);
|
||
m_MovePContinue= ((addr[0] & 0x80)>0 ? true : false);
|
||
m_MoveNContinue= ((addr[1] & 0x1)>0 ? true : false);
|
||
m_MoveAbsPos= ((addr[1] & 0x2)>0 ? true : false);
|
||
m_MotionStop= ((addr[1] & 0x4)>0 ? true : false);
|
||
m_ServoRDY= ((addr[1] & 0x8)>0 ? true : false);
|
||
m_ServoHomeDone= ((addr[1] & 0x10)>0 ? true : false);
|
||
m_ResetDone= ((addr[1] & 0x20)>0 ? true : false);
|
||
m_ServoBusy= ((addr[1] & 0x40)>0 ? true : false);
|
||
m_ServoException= ((addr[1] & 0x80)>0 ? true : false);
|
||
m_Runable= ((addr[2] & 0x1)>0 ? true : false);
|
||
m_ExceptionCode = S7WORDDATA(addr[5], addr[4]).wValue;
|
||
}
|
||
|
||
|