From 9319451494ee8e99abc51732241e2da82b8e83d0 Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Wed, 4 Nov 2020 16:12:48 +0000 Subject: [PATCH] EgtMachKernel : - corretti collegamenti tra lavorazioni con teste dal basso. --- MachineStruConst.h | 2 + Operation.cpp | 96 +++++++++++++++++++++++++++++++--------------- Operation.h | 3 +- 3 files changed, 69 insertions(+), 32 deletions(-) diff --git a/MachineStruConst.h b/MachineStruConst.h index b209370..92b21d6 100644 --- a/MachineStruConst.h +++ b/MachineStruConst.h @@ -104,6 +104,8 @@ const std::string MCH_EXIT = "T" ; const std::string MCH_ZMAXONROT = "ZMAXONROT" ; // Info di testa per una extra risalita in Z const std::string MCH_ZEXTRA = "ZEXTRA" ; +// Info di testa per dichiarazione Home Z in basso +const std::string MCH_ZHOMEDOWN = "ZHOMEDOWN" ; //---------------------------------------------------------------------------- // Dati aggregato rotante da sotto diff --git a/Operation.cpp b/Operation.cpp index d00a04c..e3d78cc 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -1623,6 +1623,8 @@ Operation::AdjustStartEndMovements( bool bVerifyPreviousLink) return false ; // se richiesta verifica collegamento con lavorazione precedente if ( bVerifyPreviousLink) { + // recupero se ZHome è in basso + bool bZHomeDown = GetZHomeDown() ; // elimino eventuali CLIMB già presenti RemoveClimb( m_pGeomDB->GetFirstGroupInGroup( nClId)) ; // recupero posizione iniziale lavorazione @@ -1631,23 +1633,24 @@ Operation::AdjustStartEndMovements( bool bVerifyPreviousLink) return false ; // se necessaria, aggiungo risalita parziale double dDeltaZ = ( vAxIni[2] - vAxVal[2]) ; - if ( dDeltaZ > 100 * EPS_SMALL) { - if ( ! pPrevOp->AddRise( vAxVal, dDeltaZ)) + if ( ( ! bZHomeDown && dDeltaZ > 100 * EPS_SMALL) || + ( bZHomeDown && dDeltaZ < -100 * EPS_SMALL)) { + if ( ! pPrevOp->AddRise( vAxVal, dDeltaZ, GDB_ID_NULL, bZHomeDown)) return false ; // aggiorno quota in Z della posizione iniziale ( anche se verrà aggiornata solo in seguito) vAxIni[2] = vAxVal[2] ; } - // Verifico non ci sia collisione a Zmax - double dSafeZ ; - if ( m_pMchMgr->GetCurrAxisHomePos( 2, dSafeZ)) { - DBLVECTOR vAxVal2 = vAxVal ; vAxVal2[2] = dSafeZ ; - DBLVECTOR vAxIni2 = vAxIni ; vAxIni2[2] = dSafeZ ; + // Verifico non ci sia collisione a HomeZ + double dHomeZ ; + if ( m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ)) { + DBLVECTOR vAxVal2 = vAxVal ; vAxVal2[2] = dHomeZ ; + DBLVECTOR vAxIni2 = vAxIni ; vAxIni2[2] = dHomeZ ; if ( ! TestCollisionAvoid( vAxVal2, vAxIni2)) { - double dExtraZ = GetExtraZ( dSafeZ) ; + double dExtraZ = GetExtraZ( dHomeZ) ; if ( dExtraZ > EPS_SMALL) { - dSafeZ += dExtraZ ; - vAxVal2[2] = dSafeZ ; - vAxIni2[2] = dSafeZ ; + dHomeZ += dExtraZ ; + vAxVal2[2] = dHomeZ ; + vAxIni2[2] = dHomeZ ; if ( ! TestCollisionAvoid( vAxVal2, vAxIni2)) return false ; } @@ -1671,19 +1674,28 @@ Operation::AdjustStartEndMovements( bool bVerifyPreviousLink) else if ( ! TestCollisionAvoid( vAxVal, vAxIni)) { // riprovo con risalita parziale bool bPartRise = false ; - double dUnsafeZ = max( vAxVal[2], vAxIni[2]) ; + double dSafeZ = dHomeZ ; + double dUnsafeZ = ( bZHomeDown ? min( vAxVal[2], vAxIni[2]) : max( vAxVal[2], vAxIni[2])) ; DBLVECTOR vAxVal2 = vAxVal ; DBLVECTOR vAxIni2 = vAxIni ; - for ( int i = 1 ; i <= 4 ; ++ i) { - double dTestSafeZ = ( dUnsafeZ + dSafeZ) / 2 ; - vAxVal2[2] = dTestSafeZ ; - vAxIni2[2] = dTestSafeZ ; - if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { - bPartRise = true ; - dSafeZ = dTestSafeZ ; - } - else { - dUnsafeZ = dTestSafeZ ; + vAxVal2[2] = dUnsafeZ ; + vAxIni2[2] = dUnsafeZ ; + if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { + bPartRise = true ; + dSafeZ = dUnsafeZ ; + } + else { + for ( int i = 1 ; i <= 4 ; ++ i) { + double dTestSafeZ = ( dUnsafeZ + dSafeZ) / 2 ; + vAxVal2[2] = dTestSafeZ ; + vAxIni2[2] = dTestSafeZ ; + if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { + bPartRise = true ; + dSafeZ = dTestSafeZ ; + } + else { + dUnsafeZ = dTestSafeZ ; + } } } if ( ! bPartRise) { @@ -1701,7 +1713,7 @@ Operation::AdjustStartEndMovements( bool bVerifyPreviousLink) // recupero le nuove quote finali (per calcolare il corretto delta) pPrevOp->GetFinalAxesValues( false, vAxVal) ; // aggiungo risalita a Zsafe - if ( ! pPrevOp->AddRise( vAxVal, dSafeZ - vAxVal[2])) + if ( ! pPrevOp->AddRise( vAxVal, dSafeZ - vAxVal[2], GDB_ID_NULL, bZHomeDown)) return false ; // aggiorno quota iniziale in Z vAxIni[2] = dSafeZ ; @@ -1839,8 +1851,10 @@ Operation::AdjustOneStartMovement( int nClPathId, int nPrevClPathId, Operation* } // verifico se la testa interferisce con i pezzi o i bloccaggi sulla tavola else { + // recupero se ZHome è in basso + bool bZHomeDown = GetZHomeDown() ; // determino la Z più alta tra le due posizioni - double dTopZ = max( vAxPrev[2], vAxCurr[2]) ; + double dTopZ = ( bZHomeDown ? min( vAxPrev[2], vAxCurr[2]) : max( vAxPrev[2], vAxCurr[2])) ; // per il test uso posizioni temporanee con questa Z DBLVECTOR vAxPrevTmp = vAxPrev ; vAxPrevTmp[2] = dTopZ ; DBLVECTOR vAxCurrTmp = vAxCurr ; vAxCurrTmp[2] = dTopZ ; @@ -1869,11 +1883,11 @@ Operation::AdjustOneStartMovement( int nClPathId, int nPrevClPathId, Operation* if ( nPrevClPathId == GDB_ID_NULL) { if ( pPrevOp == nullptr || ! pPrevOp->RemoveRise() || - ! pPrevOp->AddRise( vAxNew1, dSafeZ - vAxPrev[2])) + ! pPrevOp->AddRise( vAxNew1, dSafeZ - vAxPrev[2]), GDB_ID_NULL, bZHomeDown) return false ; } else { - if ( ! AddRise( vAxNew1, dSafeZ - vAxPrev[2], nPrevClPathId)) + if ( ! AddRise( vAxNew1, dSafeZ - vAxPrev[2], nPrevClPathId), bZHomeDown) return false ; } // aggiungo posizione elevata prima dell'inizio del percorso corrente @@ -1903,14 +1917,15 @@ Operation::AdjustOneStartMovement( int nClPathId, int nPrevClPathId, Operation* // se Z pressochè uguali non devo fare alcunché if ( abs( vAxCurr[2] - vAxPrev[2]) <= 100 * EPS_SMALL) return true ; - // se Z corrente maggiore della precedente - else if ( vAxCurr[2] > vAxPrev[2]) { + // se Z corrente maggiore della precedente o viceversa se HomeZ sotto + else if ( ( ! bZHomeDown && vAxCurr[2] > vAxPrev[2]) || + ( bZHomeDown && vAxCurr[2] < vAxPrev[2]) ) { // devo essere sulla stessa lavorazione if ( nPrevClPathId == GDB_ID_NULL) return false ; // aggiungo risalita alla fine del precedente percorso DBLVECTOR vAxNew ; - if ( ! AddRise( vAxNew, vAxCurr[2] - vAxPrev[2], nPrevClPathId)) + if ( ! AddRise( vAxNew, vAxCurr[2] - vAxPrev[2], nPrevClPathId, bZHomeDown)) return false ; } // altrimenti Z corrente minore della precedente @@ -1980,10 +1995,13 @@ Operation::RemoveClimb( int nClPathId) //---------------------------------------------------------------------------- bool -Operation::AddRise( DBLVECTOR& vAxVal, double dDelta, int nClPathId) +Operation::AddRise( DBLVECTOR& vAxVal, double dDelta, int nClPathId, bool bZHomeDown) { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; + // se risalita praticamente nulla, esco con successo + if ( abs( dDelta) < 10 * EPS_SMALL) + return true ; // recupero gruppo per geometria di lavorazione (Cutter Location) int nClId = m_pGeomDB->GetFirstNameInGroup( GetOwner(), MCH_CL) ; if ( nClId == GDB_ID_NULL) @@ -2029,7 +2047,7 @@ Operation::AddRise( DBLVECTOR& vAxVal, double dDelta, int nClPathId) // flag per tipo di movimento in rapido int nFlag = 0 ; // se delta positivo lo uso come incremento di Z - if ( dDelta > 0) { + if ( dDelta > 0 || bZHomeDown) { vAxVal[2] += dDelta ; nFlag = 0 ; // movimento standard } @@ -2339,6 +2357,22 @@ Operation::ForcedZmax( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd) const return false ; } +//---------------------------------------------------------------------------- +bool +Operation::GetZHomeDown( void) const +{ + // Recupero macchina corrente + Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ; + if ( pMch == nullptr) + return false ; + // Leggo da testa Info ZHOMEDOWN (0=falso[default], altrimenti vero) + int nHeadId = pMch->GetCurrHead() ; + bool bZHomeDown ; + if ( ! m_pGeomDB->GetInfo( nHeadId, MCH_ZHOMEDOWN, bZHomeDown)) + bZHomeDown = false ; + return bZHomeDown ; +} + //---------------------------------------------------------------------------- bool Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd) const diff --git a/Operation.h b/Operation.h index 1d60732..881a381 100644 --- a/Operation.h +++ b/Operation.h @@ -104,13 +104,14 @@ class Operation : public IUserObj bool AdjustOneStartMovement( int nClPathId, int nPrevClPathId, Operation* pPrevOp, const DBLVECTOR& vAxPrev, bool bMaxZ) ; bool ToolChangeNeeded( const Operation& Op1, const Operation& Op2) const ; bool RemoveClimb( int nClPathId) ; - bool AddRise( DBLVECTOR& vAxVal, double dDelta = - 1, int nClPathId = GDB_ID_NULL) ; + bool AddRise( DBLVECTOR& vAxVal, double dDelta = - 1, int nClPathId = GDB_ID_NULL, bool bZHomeDown = false) ; bool AddSpecialRise( const DBLVECTOR& vAxVal, bool bOk = true, int nClPathId = GDB_ID_NULL) ; bool RemoveRise( int nClPathId = GDB_ID_NULL) ; bool AddHome( void) ; bool CalcDeltaZForHeadRotation( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, double& dDeltaZ) const ; double GetExtraZ( double dSafeZ) const ; bool ForcedZmax( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd) const ; + bool GetZHomeDown( void) const ; bool TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd) const ; bool SpecialMoveZup( Vector3d& vtTool, DBLVECTOR& vAx, bool& bModif) ; bool SpecialMoveRapid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, DBLVECTOR& vAxNew, bool& bModif) ;