From d7234d57d1c39185cc8ed127ae4c655424ed4b0b Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Tue, 4 Oct 2016 07:33:38 +0000 Subject: [PATCH] EgtMachKernel : - gestione posizionamento pezzi su tavole con home assi non nullo - migliorata rotazione grezzi su centroide. --- Disposition.cpp | 27 ++++++++++++++++++++++++--- MachMgrRawParts.cpp | 24 ++++++++++++++++++++---- Machine.h | 1 + MachineCalc.cpp | 36 +++++++++++++++++++++++++++++++++++- MachineStruConst.h | 3 ++- 5 files changed, 82 insertions(+), 9 deletions(-) diff --git a/Disposition.cpp b/Disposition.cpp index c97e088..9bbb7d1 100644 --- a/Disposition.cpp +++ b/Disposition.cpp @@ -579,9 +579,15 @@ Disposition::MoveToCornerRawPart( int nRawId, const Point3d& ptP, int nFlag, boo b3Raw.Translate( vtMove) ; if ( ! m_b3Area1.EnclosesXY( b3Raw)) return false ; + // recupero eventuale spostamento corrente della tavola + Vector3d vtDelta1 ; + Machine* pMch = m_pMchMgr->GetCurrMachine() ; + if ( pMch == nullptr || ! pMch->GetCurrTableDeltaRef1( vtDelta1)) + return false ; // eseguo traslazione in globale - if ( ! vtMove.IsSmall()) - m_pGeomDB->TranslateGlob( nRawId, vtMove) ; + Vector3d vtTotMove = vtMove + vtDelta1 ; + if ( ! vtTotMove.IsSmall()) + m_pGeomDB->TranslateGlob( nRawId, vtTotMove) ; // se da aggiungere alla lista if ( bAddToList) { auto iIter = find_if( m_vMvrData.begin(), m_vMvrData.end(), @@ -675,8 +681,13 @@ Disposition::MoveRawPart( int nRawId, const Vector3d& vtMove) BBox3d b3Raw ; if ( ! m_pGeomDB->GetGlobalBBox( nRawSolId, b3Raw)) return false ; + // recupero eventuale spostamento corrente della tavola + Vector3d vtDelta1 ; + Machine* pMch = m_pMchMgr->GetCurrMachine() ; + if ( pMch == nullptr || ! pMch->GetCurrTableDeltaRef1( vtDelta1)) + return false ; // verifico se il grezzo nella posizione finale sarà nella tavola - b3Raw.Translate( vtMove) ; + b3Raw.Translate( vtMove - vtDelta1) ; if ( ! m_b3Area1.EnclosesXY( b3Raw)) return false ; // eseguo traslazione in globale @@ -724,11 +735,21 @@ Disposition::ApplyRotationToRawPart( int nRawId, double dAngCDeg, double dAngADe // verifica validità grezzo if ( ! VerifyRawPart( nRawId)) return false ; + // recupero centro del grezzo + Point3d ptCen ; + if ( ! m_pMchMgr->GetRawPartCenter( nRawId, ptCen)) + return false ; // recupero riferimento del grezzo (coincide con quello globale) Frame3d* pfrRaw = m_pGeomDB->GetGroupFrame( nRawId) ; // lo trasformo nel nuovo riferimento Point3d ptOrig = pfrRaw->Orig() ; // necessario copiarlo pfrRaw->Set( ptOrig, dAngCDeg, dAngADeg, dAngC1Deg) ; + // recupero nuovo centro del grezzo + Point3d ptNewCen ; + if ( ! m_pMchMgr->GetRawPartCenter( nRawId, ptNewCen)) + return false ; + // eseguo spostamento per riportare il centro nella posizione originale + pfrRaw->Translate( ptCen - ptNewCen) ; return true ; } diff --git a/MachMgrRawParts.cpp b/MachMgrRawParts.cpp index 5448495..ef59fe4 100644 --- a/MachMgrRawParts.cpp +++ b/MachMgrRawParts.cpp @@ -640,7 +640,11 @@ MachMgr::RotateRawPart( int nRawId, const Vector3d& vtAx, double dAngRotDeg) if ( pDisp == nullptr) return false ; // eseguo l'operazione - return pDisp->RotateRawPart( nRawId, vtAx, dAngRotDeg) ; + if ( ! pDisp->RotateRawPart( nRawId, vtAx, dAngRotDeg)) { + LOG_INFO( GetEMkLogger(), "Error on RotateRawPart") ; + return false ; + } + return true ; } //---------------------------------------------------------------------------- @@ -652,7 +656,11 @@ MachMgr::MoveToCornerRawPart( int nRawId, const Point3d& ptP, int nFlag) if ( pDisp == nullptr) return false ; // eseguo l'operazione - return pDisp->MoveToCornerRawPart( nRawId, ptP, nFlag) ; + if ( ! pDisp->MoveToCornerRawPart( nRawId, ptP, nFlag)) { + LOG_INFO( GetEMkLogger(), "Error on MoveToCornerRawPart") ; + return false ; + } + return true ; } //---------------------------------------------------------------------------- @@ -664,7 +672,11 @@ MachMgr::MoveToCenterRawPart( int nRawId, const Point3d& ptP, int nFlag) if ( pDisp == nullptr) return false ; // eseguo l'operazione - return pDisp->MoveToCenterRawPart( nRawId, ptP, nFlag) ; + if ( ! pDisp->MoveToCenterRawPart( nRawId, ptP, nFlag)) { + LOG_INFO( GetEMkLogger(), "Error on MoveToCenterRawPart") ; + return false ; + } + return true ; } //---------------------------------------------------------------------------- @@ -676,7 +688,11 @@ MachMgr::MoveRawPart( int nRawId, const Vector3d& vtMove) if ( pDisp == nullptr) return false ; // eseguo l'operazione - return pDisp->MoveRawPart( nRawId, vtMove) ; + if ( ! pDisp->MoveRawPart( nRawId, vtMove)) { + LOG_INFO( GetEMkLogger(), "Error on MoveRawPart") ; + return false ; + } + return true ; } //---------------------------------------------------------------------------- diff --git a/Machine.h b/Machine.h index 06d99a2..42ec076 100644 --- a/Machine.h +++ b/Machine.h @@ -65,6 +65,7 @@ class Machine bool GetCurrTable( std::string& sTable) const ; bool GetCurrTableRef1( Point3d& ptRef1) const ; bool GetCurrTableArea1( BBox3d& b3Area1) const ; + bool GetCurrTableDeltaRef1( Vector3d& vtDelta1) const ; bool SetCurrTool( const std::string& sTool, const std::string& sHead, int nExit) ; int GetCurrTool( void) const ; bool GetCurrTool( std::string& sTool) const ; diff --git a/MachineCalc.cpp b/MachineCalc.cpp index 18d4ef2..502c5e6 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -108,6 +108,31 @@ Machine::GetCurrTableArea1( BBox3d& b3Area1) const return true ; } +//---------------------------------------------------------------------------- +bool +Machine::GetCurrTableDeltaRef1( Vector3d& vtDelta1) const +{ + Table* pTab = GetTable( m_nCalcTabId) ; + if ( pTab == nullptr) + return false ; + // recupero la posizione corrente del riferimento 1 della tavola + // riferimento globale del gruppo tavola + Frame3d frTable ; + m_pGeomDB->GetGroupGlobFrame( m_nCalcTabId, frTable) ; + // recupero il primo riferimento della tavola + int nRef1 = m_pGeomDB->GetFirstNameInGroup( m_nCalcTabId, MCH_TREF + "1") ; + if ( nRef1 == GDB_ID_NULL || m_pGeomDB->GetGeoType( nRef1) != GEO_FRAME3D) + return false ; + // recupero frame + const Frame3d& frFrame = GetGeoFrame3d( m_pGeomDB->GetGeoObj( nRef1))->GetFrame() ; + // ne calcolo l'origine in globale + Point3d ptPos = frFrame.Orig() ; + ptPos.ToGlob( frTable) ; + // calcolo il delta + vtDelta1 = ptPos - pTab->GetRef1() ; + return true ; +} + //---------------------------------------------------------------------------- bool Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit) @@ -367,6 +392,7 @@ Machine::AddKinematicAxis( bool bOnHead, int nId) kAx.ptPos = pAx->GetPos() ; kAx.vtDir = pAx->GetDir() ; kAx.stroke = pAx->GetStroke() ; + kAx.dHomeVal = pAx->GetHomeVal() ; // se lineare di tavola, devo invertirlo if ( kAx.bLinear && ! kAx.bHead) kAx.vtDir.Invert() ; @@ -802,13 +828,21 @@ Machine::GetPositions( const Point3d& ptP, DBLVECTOR vAng, int& nStat, double& dX, double& dY, double& dZ) const { // la posizione deve essere espressa rispetto allo ZERO MACCHINA + // il punto è dato rispetto alla posizione home della tavola // aggiorno punto di lavoro mediante ciclo diretto sugli assi di tavola Point3d ptW = ptP ; + // annullo la posizione home degli assi lineari + for ( size_t i = 0 ; i < m_vCalcLinAx.size() ; ++ i) { + // se asse di tavola + if ( ! m_vCalcLinAx[i].bHead) + ptW.Translate( - m_vCalcLinAx[i].dHomeVal * ( - m_vCalcLinAx[i].vtDir)) ; + } + // effettuo rotazione diminuita della posizione home degli assi rotanti for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) { // se asse di tavola if ( ! m_vCalcRotAx[i].bHead) - ptW.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + ptW.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i] - m_vCalcRotAx[i].dHomeVal) ; } // aggiorno posizione e direzione fresa su testa a riposo mediante ciclo inverso sugli assi di testa diff --git a/MachineStruConst.h b/MachineStruConst.h index 0ec4a5e..8e3a1f5 100644 --- a/MachineStruConst.h +++ b/MachineStruConst.h @@ -47,10 +47,11 @@ struct KinAxis { Point3d ptPos ; Vector3d vtDir ; STROKE stroke ; + double dHomeVal ; bool bFixed ; double dFixVal ; KinAxis( void) - : nGrpId( GDB_ID_NULL), bLinear( true), bHead( true), ptPos(), vtDir(), bFixed( false), dFixVal( 0) + : nGrpId( GDB_ID_NULL), bLinear( true), bHead( true), ptPos(), vtDir(), dHomeVal( 0), bFixed( false), dFixVal( 0) { stroke.Min = 0 ; stroke.Max = 0 ; } } ; typedef std::vector KINAXISVECTOR ;