From 6bb714e581d85d3834ce5db87d5af4b50214978f Mon Sep 17 00:00:00 2001 From: DarioS Date: Tue, 19 Apr 2022 11:24:41 +0200 Subject: [PATCH] EgtMachKernel 2.4d2 : - modifiche per gestire macchina con asse rotante tipo ralla che porta due assi lineari ad esso perpendicolare (terzo lineare diretto come ralla). --- EgtMachKernel.rc | Bin 11774 -> 11774 bytes Machine.cpp | 5 ++ Machine.h | 7 +- MachineCalc.cpp | 201 ++++++++++++++++++++++++++++++--------------- MachineStruConst.h | 5 +- Milling.cpp | 4 +- 6 files changed, 151 insertions(+), 71 deletions(-) diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index 89f01b568c293feadd0d1aa2de63379f3f2bdd59..8c8683f667d2c82dc3edbbb32201d6ea8a1c31e6 100644 GIT binary patch delta 236 zcmewt{V#gMI%aKGhCGIJh8%`WhGK>c1_cHUhESkLCPN8BDnkm89}HwAf_cT0IklxX ze_&=~W;ELTUhX2(<& nF)y2}rOv|(a;PWRX*mp)AcsxGetInfo( m_nCalcTabId, MCH_TILTINGLIKE, bTiltingLike) && bTiltingLike) { + bTilting = true ; + return true ; + } // recupero gli eventuali assi rotanti della tavola int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ; if ( nTParId == GDB_ID_NULL) @@ -443,6 +449,11 @@ Machine::CalculateKinematicChain( void) if ( m_pGeomDB == nullptr) return false ; // azzero tutti gli assi della catena cinematica + m_nTabLinAxes = 0 ; + m_nTabRotAxes = 0 ; + m_nHeadLinAxes = 0 ; + m_nHeadRotAxes = 0 ; + m_nHeadSpecRotAxis = -1 ; m_vCalcLinAx.clear() ; m_vCalcRotAx.clear() ; // recupero gli assi di tavola @@ -451,6 +462,8 @@ Machine::CalculateKinematicChain( void) int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ; if ( nTParId == GDB_ID_NULL) return false ; + m_nTabLinAxes = 0 ; + m_nTabRotAxes = 0 ; while ( IsAxisGroup( nTParId)) { if ( ! AddKinematicAxis( false, nTParId)) return false ; @@ -462,12 +475,22 @@ Machine::CalculateKinematicChain( void) int nHParId = m_pGeomDB->GetParentId( m_nCalcHeadId) ; if ( nHParId == GDB_ID_NULL) return false ; + m_nHeadLinAxes = 0 ; + m_nHeadRotAxes = 0 ; while ( IsAxisGroup( nHParId)) { if ( ! AddKinematicAxis( true, nHParId)) return false ; nHParId = m_pGeomDB->GetParentId( nHParId) ; } + // verifiche sugli assi lineari : + // aggiusto gli indici di ordine sulla sua catena cinematica (1-based) + for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) { + if ( m_vCalcLinAx[i].bHead) + m_vCalcLinAx[i].nInd += m_nHeadLinAxes + m_nHeadRotAxes + 1 ; + else + m_vCalcLinAx[i].nInd *= -1 ; + } // devono essere 3 if ( m_vCalcLinAx.size() != 3) return false ; @@ -490,12 +513,21 @@ Machine::CalculateKinematicChain( void) return false ; } } + // verifiche sugli assi rotanti : + bool bOk = false ; + // aggiusto gli indici di ordine sulla sua catena cinematica (1-based) + for ( int i = 0 ; i < int( m_vCalcRotAx.size()) ; ++ i) { + if ( m_vCalcRotAx[i].bHead) + m_vCalcRotAx[i].nInd += m_nHeadLinAxes + m_nHeadRotAxes + 1 ; + else + m_vCalcRotAx[i].nInd *= -1 ; + } // se 0 o 1 va bene if ( m_vCalcRotAx.size() <= 1) - return true ; + bOk = true ; // se 2 va bene - if ( m_vCalcRotAx.size() == 2) { + else if ( m_vCalcRotAx.size() == 2) { // se entrambi di testa devo invertirne l'ordine if ( m_vCalcRotAx[0].bHead && m_vCalcRotAx[1].bHead) swap( m_vCalcRotAx[0], m_vCalcRotAx[1]) ; @@ -507,10 +539,10 @@ Machine::CalculateKinematicChain( void) m_vCalcRotAx[1].stroke.Max = min( m_vCalcRotAx[1].stroke.Max, pHead->GetRot2Stroke().Max) ; } } - return true ; + bOk = true ; } // se 3 va bene ( uno dovrà poi avere valore assegnato) - if ( m_vCalcRotAx.size() == 3) { + else if ( m_vCalcRotAx.size() == 3) { int n2ndHeadRotAx = - 1 ; // se tutti e tre di testa, devo invertire il primo con il terzo if ( m_vCalcRotAx[0].bHead && m_vCalcRotAx[1].bHead && m_vCalcRotAx[2].bHead) { @@ -530,10 +562,48 @@ Machine::CalculateKinematicChain( void) m_vCalcRotAx[n2ndHeadRotAx].stroke.Max = min( m_vCalcRotAx[n2ndHeadRotAx].stroke.Max, pHead->GetRot2Stroke().Max) ; } } - return true ; + bOk = true ; } + if ( ! bOk) + return false ; + // verifico esistenza eventuale asse rotante speciale di testa + if ( m_nHeadRotAxes > 0 && m_nHeadLinAxes > 0) { + // indice di posizione primo asse di testa + int nHeadFirst = 1 ; + // ricerco sui rotanti + for ( int i = 0 ; i < int( m_vCalcRotAx.size()) ; ++ i) { + // se asse di testa + if ( m_vCalcRotAx[i].bHead && m_vCalcRotAx[i].nInd <= nHeadFirst) { + // non sono ammessi due assi di questo tipo + if ( m_nHeadSpecRotAxis != -1) + return false ; + // la tavola non deve avere più di un asse lineare + if ( m_nTabLinAxes > 1) + return false ; + // se ha un asse lineare deve essere allineato con il rotante speciale + else if ( m_nTabLinAxes == 1) { + // ne recupero la direzione + Vector3d vtTabLinDir ; + for ( int j = 0 ; j < int( m_vCalcLinAx.size()) ; ++ j) { + if ( ! m_vCalcLinAx[i].bHead) { + vtTabLinDir = m_vCalcLinAx[i].vtDir ; + break ; + } + } + // la confronto con quella dell'asse rotante speciale + if ( ! AreSameOrOppositeVectorApprox( vtTabLinDir, m_vCalcRotAx[i].vtDir)) + return false ; + } + // ne salvo l'indice + m_nHeadSpecRotAxis = i ; + // incremento indice di posizione posibile primo asse lineare di testa + ++ nHeadFirst ; + } + } + } + return true ; - // altrimenti non ancora gestito, quindi errore + // altrimenti non ancora gestito, quindi errore LOG_ERROR( GetEMkLogger(), "Rotary Axes not manageable") return false ; } @@ -553,11 +623,20 @@ Machine::AddKinematicAxis( bool bOnHead, int nId) KinAxis kAx ; kAx.nGrpId = nId ; kAx.bLinear = ( pAx->GetType() != MCH_AT_ROTARY) ; - kAx.bHead = bOnHead ; // posizione su catena cinematica + kAx.bHead = bOnHead ; // catena cinematica di appartenenza (testa o tavola) kAx.ptPos = pAx->GetPos() ; kAx.vtDir = pAx->GetDir() ; kAx.stroke = pAx->GetStroke() ; kAx.dHomeVal = pAx->GetHomeVal() ; + // ne determino l'indice di posizione nella sua catena cinematica (assegno valore negato perchè provvisorio) + if ( kAx.bHead) { + ( kAx.bLinear ? ++ m_nHeadLinAxes : ++ m_nHeadRotAxes) ; + kAx.nInd = - ( m_nHeadLinAxes + m_nHeadRotAxes) ; + } + else { + ( kAx.bLinear ? ++ m_nTabLinAxes : ++ m_nTabRotAxes) ; + kAx.nInd = - ( m_nTabLinAxes + m_nTabRotAxes) ; + } // se lineare di tavola, devo invertirlo if ( kAx.bLinear && ! kAx.bHead) kAx.vtDir.Invert() ; @@ -1025,26 +1104,33 @@ Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng, // 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) { + for ( int i = 0 ; i < int( 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) { + for ( int i = 0 ; i < int( 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] - m_vCalcRotAx[i].dHomeVal) ; } + // effettuo rotazione inversa per asse rotante di testa speciale + if ( m_nHeadSpecRotAxis != -1) { + if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size())) + return false ; + int i = m_nHeadSpecRotAxis ; + ptW.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, -vAng[i]) ; + } // aggiorno posizione e direzione fresa su testa a riposo mediante ciclo inverso sugli assi di testa Point3d ptPosH = m_ptCalcPos ; Vector3d vtDirH = m_vtCalcDir ; - for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) { - // se asse di testa - if ( m_vCalcRotAx[i-1].bHead) { - ptPosH.Rotate( m_vCalcRotAx[i-1].ptPos, m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ; - vtDirH.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ; + for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) { + // se asse di testa non speciale + if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis) { + ptPosH.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + vtDirH.Rotate( m_vCalcRotAx[i].vtDir, vAng[i]) ; } } @@ -1067,46 +1153,6 @@ Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng, return true ; } -//---------------------------------------------------------------------------- -bool -Machine::GetHeadOffsetDelta( const DBLVECTOR& vAng, - double& dRecX, double& dRecY, double& dRecZ) const -{ - // ovviamente tutto è espresso nel riferimento ZERO MACCHINA - - // verifico che siano stati assegnati gli angoli necessari, altrimenti errore - if ( vAng.size() < m_vCalcRotAx.size()) - return false ; - - // aggiorno posizione e direzione fresa su testa a riposo mediante ciclo inverso sugli assi di testa - Point3d ptPosH = m_ptCalcPos ; - Vector3d vtDirH = m_vtCalcDir ; - for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) { - // se asse di testa - if ( m_vCalcRotAx[i-1].bHead) { - ptPosH.Rotate( m_vCalcRotAx[i-1].ptPos, m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ; - vtDirH.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ; - } - } - - // assegno l'offset testa - Vector3d vtDtHe = ORIG - m_ptCalcPos ; - - // calcolo il recupero degli assi : è l'opposto dello spostamento della posizione - Vector3d vtDtAx = m_ptCalcPos - ptPosH ; - - // calcolo il recupero di lunghezza utensile - Vector3d vtDtTL = vtDirH * m_dCalcTLen ; - - // calcolo le posizioni degli assi lineari - dRecX = vtDtHe.x + vtDtAx.x + vtDtTL.x ; - dRecY = vtDtHe.y + vtDtAx.y + vtDtTL.y ; - dRecZ = vtDtHe.z + vtDtAx.z + vtDtTL.z ; - - // tutto ok - return true ; -} - //---------------------------------------------------------------------------- bool Machine::GetDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& vtNew) const @@ -1166,17 +1212,25 @@ Machine::GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& // aggiorno posizione testa a riposo mediante ciclo inverso sugli assi rotanti di testa ptNose = m_ptCalcPos ; - for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) { - // se asse di testa - if ( m_vCalcRotAx[i-1].bHead) - ptNose.Rotate( m_vCalcRotAx[i-1].ptPos, m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ; + for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) { + // se asse di testa non speciale + if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis) + ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; } // aggiorno posizione testa con assi lineari di testa DBLVECTOR vMov( {dX, dY, dZ}) ; - for ( size_t i = 1 ; i <= m_vCalcLinAx.size() ; ++ i) { - if ( m_vCalcLinAx[i-1].bHead) - ptNose += m_vCalcLinAx[i-1].vtDir * vMov[i-1] ; + for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) { + if ( m_vCalcLinAx[i].bHead) + ptNose += m_vCalcLinAx[i].vtDir * vMov[i] ; } + // eseguo rotazione eventuale asse rotante speciale di testa + if ( m_nHeadSpecRotAxis != -1) { + if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size())) + return false ; + int i = m_nHeadSpecRotAxis ; + ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + } + return true ; } @@ -1188,11 +1242,26 @@ Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& // la posizione deve essere espressa rispetto allo ZERO MACCHINA // è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa) - // Calcoli recuperi della testa e lunghezza utensile per orientamento - double dRecX, dRecY, dRecZ ; - if ( ! GetHeadOffsetDelta( vAng, dRecX, dRecY, dRecZ)) - return false ; - ptTip.Set( dX - dRecX, dY - dRecY, dZ - dRecZ) ; + // aggiorno posizione tip utensile a riposo mediante ciclo inverso sugli assi rotanti di testa + ptTip = m_ptCalcPos - m_vtCalcDir * m_dCalcTLen ; + for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) { + // se asse di testa non speciale + if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis) + ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + } + // aggiorno posizione tip utensile con assi lineari di testa + DBLVECTOR vMov( {dX, dY, dZ}) ; + for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) { + if ( m_vCalcLinAx[i].bHead) + ptTip += m_vCalcLinAx[i].vtDir * vMov[i] ; + } + // eseguo rotazione eventuale asse rotante speciale di testa + if ( m_nHeadSpecRotAxis != -1) { + if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size())) + return false ; + int i = m_nHeadSpecRotAxis ; + ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + } // Se richiesto ingombro totale o punto sotto del tip utensile if ( bOverall || bBottom) { diff --git a/MachineStruConst.h b/MachineStruConst.h index 69d1223..7a85bf3 100644 --- a/MachineStruConst.h +++ b/MachineStruConst.h @@ -44,6 +44,7 @@ struct KinAxis { int nGrpId ; bool bLinear ; bool bHead ; + int nInd ; Point3d ptPos ; Vector3d vtDir ; STROKE stroke ; @@ -51,7 +52,7 @@ struct KinAxis { bool bFixed ; double dFixVal ; KinAxis( void) - : nGrpId( GDB_ID_NULL), bLinear( true), bHead( true), ptPos(), vtDir(), dHomeVal( 0), bFixed( false), dFixVal( 0) + : nGrpId( GDB_ID_NULL), bLinear( true), bHead( true), nInd( 0), ptPos(), vtDir(), dHomeVal( 0), bFixed( false), dFixVal( 0) { stroke.Min = 0 ; stroke.Max = 0 ; } } ; typedef std::vector KINAXISVECTOR ; @@ -112,6 +113,8 @@ const std::string MCH_ZHOMEDOWN = "ZHOMEDOWN" ; const std::string MCH_ABOVE = "ABOVE" ; // Info di testa per una variazione alla Z di sicurezza globale const std::string MCH_ZSAFEDELTA = "ZSAFEDELTA" ; +// Info di tavola per dichiarare che va considerata come basculante +const std::string MCH_TILTINGLIKE = "TiltingLike" ; //---------------------------------------------------------------------------- // Dati aggregato rotante da sotto diff --git a/Milling.cpp b/Milling.cpp index b2e5d2c..5611d60 100644 --- a/Milling.cpp +++ b/Milling.cpp @@ -2298,7 +2298,7 @@ Milling::AddStandardMilling( const ICurveComposite* pCompo, const Vector3d& vtTo Vector3d vtMove( vtCorr.x, vtCorr.y, 0) ; if ( vtMove.Normalize()) { Point3d ptTest = ptP1 - MIN_SAFEDIST * ( m_bAboveHead ? Z_AX : -Z_AX) ; - if ( GetElevation( m_nPhase, ptTest, vtTool, 0.5 * m_TParams.m_dDiam, vtMove, dSawStartElev) && dSawStartElev < EPS_SMALL) + if ( GetElevation( m_nPhase, ptTest, vtTool, 0.5 * m_TParams.m_dDiam, vtMove, dSawStartElev) && dSawStartElev > EPS_SMALL) bSideStart = true ; } } @@ -2431,7 +2431,7 @@ Milling::AddStandardMilling( const ICurveComposite* pCompo, const Vector3d& vtTo Vector3d vtMove( vtCorr.x, vtCorr.y, 0) ; if ( vtMove.Normalize()) { Point3d ptTest = ptP1 - MIN_SAFEDIST * ( m_bAboveHead ? Z_AX : -Z_AX) ; - if ( GetElevation( m_nPhase, ptTest, vtTool, 0.5 * m_TParams.m_dDiam, vtMove, dSawEndElev) && dSawEndElev < EPS_SMALL) + if ( GetElevation( m_nPhase, ptTest, vtTool, 0.5 * m_TParams.m_dDiam, vtMove, dSawEndElev) && dSawEndElev > EPS_SMALL) bSideEnd = true ; } }