Egalware/EgtMachKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

EgtMachKernel :

Browse Source 
- modifiche per permettere assi lineari controversi con assi principali XYZ.
This commit is contained in:
DarioS
2022-08-29 07:24:34 +02:00
parent 6399c41598
commit cf6c628876
6 changed files with 52 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+22 -12
View File
@@ -215,7 +215,9 @@ Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
// recupero i dati della testa
Head* pHead = GetHead( nHeadId) ;
if ( pHead == nullptr) {
string sOut = "Missing head " + sHead + " for tool " + sTool ;
string sOutHead = ( IsEmptyOrSpaces( sHead) ? "???" : sHead) ;
string sOutTool = ( IsEmptyOrSpaces( sTool) ? "???" : sTool) ;
string sOut = "Missing head " + sOutHead + " for tool " + sOutTool ;
LOG_ERROR( GetEMkLogger(), sOut.c_str())
return false ;
}
@@ -495,24 +497,28 @@ Machine::CalculateKinematicChain( void)
if ( m_vCalcLinAx.size() != 3)
return false ;
// devono essere ordinabili come XYZ
if ( ! m_vCalcLinAx[0].vtDir.IsXplus()) {
if ( m_vCalcLinAx[1].vtDir.IsXplus())
if ( ! m_vCalcLinAx[0].vtDir.IsX()) {
if ( m_vCalcLinAx[1].vtDir.IsX())
swap( m_vCalcLinAx[0], m_vCalcLinAx[1]) ;
else if ( m_vCalcLinAx[2].vtDir.IsXplus())
else if ( m_vCalcLinAx[2].vtDir.IsX())
swap( m_vCalcLinAx[0], m_vCalcLinAx[2]) ;
else {
LOG_ERROR( GetEMkLogger(), "Linear Axes not aligned with Global XYZ")
LOG_ERROR( GetEMkLogger(), "Linear Axes are not aligned with Global XYZ")
return false ;
}
}
if ( ! m_vCalcLinAx[1].vtDir.IsYplus()) {
if ( m_vCalcLinAx[2].vtDir.IsYplus())
if ( ! m_vCalcLinAx[1].vtDir.IsY()) {
if ( m_vCalcLinAx[2].vtDir.IsY())
swap( m_vCalcLinAx[1], m_vCalcLinAx[2]) ;
else {
LOG_ERROR( GetEMkLogger(), "Linear Axes not aligned with Global XYZ")
LOG_ERROR( GetEMkLogger(), "Linear Axes are not aligned with Global XYZ")
return false ;
}
}
if ( ! m_frLinAx.Set( ORIG, m_vCalcLinAx[0].vtDir, m_vCalcLinAx[1].vtDir, m_vCalcLinAx[2].vtDir)) {
LOG_ERROR( GetEMkLogger(), "Linear Axes are not a Rigth-handed Frame")
return false ;
}
// verifiche sugli assi rotanti :
bool bOk = false ;
@@ -1143,10 +1149,14 @@ Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng,
// calcolo il recupero di lunghezza utensile
Vector3d vtDtTL = vtDirH * m_dCalcTLen ;
// calcolo le posizioni degli assi lineari
dX = ptW.x + vtDtHe.x + vtDtAx.x + vtDtTL.x ;
dY = ptW.y + vtDtHe.y + vtDtAx.y + vtDtTL.y ;
dZ = ptW.z + vtDtHe.z + vtDtAx.z + vtDtTL.z ;
// calcolo i valori degli assi lineari (posizioni)
Point3d ptPos( ptW.x + vtDtHe.x + vtDtAx.x + vtDtTL.x,
ptW.y + vtDtHe.y + vtDtAx.y + vtDtTL.y,
ptW.z + vtDtHe.z + vtDtAx.z + vtDtTL.z) ;
ptPos.ToLoc( m_frLinAx) ;
dX = ptPos.x ;
dY = ptPos.y ;
dZ = ptPos.z ;
// tutto ok
nStat = 0 ;