From 848701033b80ac42bf7636980bc544f18759a487 Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Tue, 15 Apr 2025 11:48:33 +0200 Subject: [PATCH] =?UTF-8?q?EgtMachKernel=202.7d2=20:=20-=20aggiunta=20poss?= =?UTF-8?q?ibilit=C3=A0=20di=20definire=20ordine=20movimenti=20tra=20testa?= =?UTF-8?q?=20e=20tavola=20per=20i=20link=20tra=20lavorazioni=20(LinkAxesM?= =?UTF-8?q?oveOrder=20di=20EmtGeneral=20con=20valori=20LKAMO=5F*).?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- EgtMachKernel.rc | Bin 11774 -> 11774 bytes Machine.cpp | 2 + Machine.h | 3 ++ MachineLua.cpp | 6 +++ Milling.cpp | 40 +++++++++++++++++++ Operation.cpp | 99 ++++++++++++++++++++++++++++++++++------------- Operation.h | 5 ++- 7 files changed, 126 insertions(+), 29 deletions(-) diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index ab4292a2a6f43d6d59542b2738cfd8db7b7c2625..516c665dea0d004969435dc72cea96f4ee19fe4d 100644 GIT binary patch delta 97 zcmewt{V#gMFE&P_&A-`fnHh~HKa|wnoW?bQ1uSxrY4Qod0+^@}R1`^_2jk{d(jGv4 NK<#LXK~l;gTmU8!BZvS1 delta 97 zcmewt{V#gMFE&QQ&A-`fnHdcyKa|wnoW?bQ1uSxrY4Qod0+^@}R1`^_2jk{d(jGv4 NK<#LXK~l;gTmU3rBY*$^ diff --git a/Machine.cpp b/Machine.cpp index 830a6ea..c767e6c 100644 --- a/Machine.cpp +++ b/Machine.cpp @@ -42,6 +42,7 @@ Machine::Machine( void) m_dExitMaxRotAdj = 10 * EPS_ANG_SMALL ; m_dAngDeltaMinForHome = INFINITO ; m_nMultiProcess = 0 ; + m_nLinkAxesMoveOrder = 0 ; m_nNewLinkMgr = 0 ; m_nCalcTabId = GDB_ID_NULL ; m_nCalcHeadId = GDB_ID_NULL ; @@ -50,6 +51,7 @@ Machine::Machine( void) m_dCalcRot1W = 1 ; m_bCalcMaxDeltaR2On1 = true ; m_nCalcSolCh = MCH_SCC_NONE ; + m_bSolChExact = false ; m_dCalcTLen = 0 ; m_dCalcTRad = 0 ; m_dCalcTOvLen = 0 ; diff --git a/Machine.h b/Machine.h index f322c1a..963d0be 100644 --- a/Machine.h +++ b/Machine.h @@ -88,6 +88,8 @@ class Machine { return m_dAngDeltaMinForHome ; } bool GetMultiProcess( int nOpt = 1) const { return ( m_nMultiProcess >= nOpt) ; } + int GetLinkAxesMoveOrder( void) const + { return m_nLinkAxesMoveOrder ; } bool GetNewLinkMgr( int nOpt = 1) const { return ( m_nNewLinkMgr >= nOpt) ; } bool LoadTool( const std::string& sHead, int nExit, const std::string& sTool) ; @@ -303,6 +305,7 @@ class Machine double m_dExitMaxRotAdj ; // massima rotazione di aggiustamento uscita da geometria a descrizione cinematica double m_dAngDeltaMinForHome ; // minima differenza angolare da valore precedente per scegliere di stare vicino a home int m_nMultiProcess ; // codice di macchina multi-processo (con stima speciale e simulazione ad hoc) + int m_nLinkAxesMoveOrder ; // codice ordine interpolazione assi nei collegamenti (0=interpolati, ...) int m_nNewLinkMgr ; // codice del nuovo gestore link tra lavorazioni (0=vecchio, 1=nuovo) INTVECTOR m_vLinkedRawParts ; // elenco dei grezzi agganciati a gruppi della macchina INTVECTOR m_vLinkedFixtures ; // elenco dei bloccaggi agganciati a gruppi della macchina diff --git a/MachineLua.cpp b/MachineLua.cpp index dc69c1a..3886d58 100644 --- a/MachineLua.cpp +++ b/MachineLua.cpp @@ -37,6 +37,7 @@ static const string FLD_EXITMAXADJUST = "ExitMaxAdjust" ; static const string FLD_EXITMAXROTADJ = "ExitMaxRotAdj" ; static const string FLD_ANGDELTAMINFORHOME = "AngDeltaMinForHome" ; static const string FLD_MULTIPROCESS = "MultiProcess" ; +static const string FLD_LINKAXESMOVEORDER = "LinkAxesMoveOrder" ; static const string FLD_NEWLINKMGR = "NewLinkMgr" ; static const string FLD_NAME = "Name" ; static const string FLD_PARENT = "Parent" ; @@ -351,6 +352,9 @@ Machine::LuaEmtGeneral( lua_State* L) // lettura eventuale campo 'MultiProcess' dalla tabella (0=no, 1=si, 2=si con simulazione MP) int nMultiProcess = 0 ; LuaGetTabFieldParam( L, 1, FLD_MULTIPROCESS, nMultiProcess) ; + // lettura eventuale campo "LinkAxesMoveOrder" dalla tabella(0=interpolati, ...) + int nLinkAxesMoveOrder = 0 ; + LuaGetTabFieldParam( L, 1, FLD_LINKAXESMOVEORDER, nLinkAxesMoveOrder) ; // lettura eventuale campo 'NewLinkMgr' dalla tabella (0=old, 1 =new) int nNewLinkMgr = 0 ; LuaGetTabFieldParam( L, 1, FLD_NEWLINKMGR, nNewLinkMgr) ; @@ -406,6 +410,8 @@ Machine::LuaEmtGeneral( lua_State* L) // imposto codice per macchina multiprocesso m_pMchLua->m_nMultiProcess = nMultiProcess ; + // imposto codice per ordine interpolazione assi nei collegamenti + m_pMchLua->m_nLinkAxesMoveOrder = nLinkAxesMoveOrder ; // imposto codice per gestione link tra lavorazioni m_pMchLua->m_nNewLinkMgr = nNewLinkMgr ; diff --git a/Milling.cpp b/Milling.cpp index 27b5418..f75fb58 100644 --- a/Milling.cpp +++ b/Milling.cpp @@ -6577,6 +6577,46 @@ Milling::CalcOffset( ICurveComposite* pCompo, double dSignOffs) const pCompo->ArcsBezierCurvesToArcsPerpExtr( LIN_TOL_MID, ANG_TOL_STD_DEG) ; return true ; } + else { + Point3d ptOffs ; + Vector3d vtOut ; + if ( OffsCrv.GetPointOffset( ptOffs, vtOut)) { + const double MIN_MOVE = 1 ; + Vector3d vtStart = vtOut ; + double dStaLen = 10 ; + Point3d ptStart ; + if ( pCompo->GetStartPoint( ptStart)) { + Vector3d vtTmp = ptOffs - ptStart ; + if ( vtTmp.Normalize()) { + double dCosA = abs( vtTmp * vtOut) ; + dStaLen = max( m_TParams.m_dDiam / 2 * sqrt( ( 1 - dCosA) / ( 1 + dCosA)), MIN_MOVE) ; + vtTmp.Rotate( vtExtr, 0, 1) ; + if ( vtTmp * vtOut < 0) + vtTmp.Invert() ; + vtStart = vtTmp ; + } + } + Vector3d vtEnd = vtOut ; + double dEndLen = 10 ; + Point3d ptEnd ; + if ( pCompo->GetEndPoint( ptEnd)) { + Vector3d vtTmp = ptOffs - ptEnd ; + if ( vtTmp.Normalize()) { + double dCosA = abs( vtTmp * vtOut) ; + dEndLen = max( m_TParams.m_dDiam / 2 * sqrt( ( 1 - dCosA) / ( 1 + dCosA)), MIN_MOVE) ; + vtTmp.Rotate( vtExtr, 0, 1) ; + if ( vtTmp * vtOut < 0) + vtTmp.Invert() ; + vtEnd = vtTmp ; + } + } + pCompo->Clear() ; + pCompo->AddPoint( ptOffs) ; + pCompo->AddLine( ptOffs + dStaLen * vtStart, false) ; + pCompo->AddLine( ptOffs + dEndLen * vtEnd) ; + return true ; + } + } } } return false ; diff --git a/Operation.cpp b/Operation.cpp index 3cdd8b7..19790aa 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -950,7 +950,7 @@ Operation::GetCurrRawsGlobBox( BBox3d& b3Raw) const //---------------------------------------------------------------------------- //---------------------------------------------------------------------------- -inline Vector3d +static Vector3d GetDirInFacePerpSide( int nFaceUse, const Vector3d& vtFaceUse, const Vector3d& vtN, ICurveComposite* pCrvCompo) { // determino la direzione di riferimento proiettata nella faccia @@ -3097,6 +3097,9 @@ Operation::AdjustOneStartEndMovement( int nClPathId, int nPrevClPathId, Operatio { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; + Machine* pMch = m_pMchMgr->GetCurrMachine() ; + if ( pMch == nullptr) + return false ; // elimino eventuali CLIMB già presenti RemoveClimb( nClPathId) ; // recupero la prima entità di questo percorso @@ -3184,7 +3187,8 @@ Operation::AdjustOneStartEndMovement( int nClPathId, int nPrevClPathId, Operatio // verifico se forzata risalita a Zmax bool bForcedZMax = ForcedZmax( vAxPrevTmp, vAxCurrTmp, b3Raws) ; // se interferisce - if ( bForcedZMax || ! TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) { + int nLKAMO = LKAMO_INTERP ; + if ( bForcedZMax || ! TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp, &nLKAMO)) { // recupero HomeZ double dHomeZ ; if ( ! m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ)) @@ -3272,18 +3276,21 @@ Operation::AdjustOneStartEndMovement( int nClPathId, int nPrevClPathId, Operatio else { // 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 o viceversa se HomeZ sotto else if ( ( ! bZHomeDown && vAxCurr[2] > vAxPrev[2]) || ( bZHomeDown && vAxCurr[2] < vAxPrev[2]) ) { // se vengo da precedente lavorazione non devo fare alcunché if ( nPrevClPathId == GDB_ID_NULL) - return true ; - // aggiungo risalita alla fine del precedente percorso - DBLVECTOR vAxNew ; - double dDelta = ( ! bZHomeDown ? max( vAxCurr[2] - vAxPrev[2], 0.) : min( vAxCurr[2] - vAxPrev[2], 0.)) ; - if ( ! AddRise( vAxNew, dDelta, nPrevClPathId)) - return false ; + ; + // altrimenti vengo da precedente percorso della stessa lavorazione + else { + // aggiungo risalita alla fine del precedente percorso + DBLVECTOR vAxNew ; + double dDelta = ( ! bZHomeDown ? max( vAxCurr[2] - vAxPrev[2], 0.) : min( vAxCurr[2] - vAxPrev[2], 0.)) ; + if ( ! AddRise( vAxNew, dDelta, nPrevClPathId)) + return false ; + } } // altrimenti Z corrente minore della precedente else { @@ -3322,6 +3329,28 @@ Operation::AdjustOneStartEndMovement( int nClPathId, int nPrevClPathId, Operatio pCamData->SetFlag( nFlagNew) ; pCamData->SetFlag2( nFlag2New) ; } + // se necessario, aggiungo eventuale movimento orizzontale all'inizio della corrente + if ( nLKAMO == LKAMO_HEAD_BEFORE || nLKAMO == LKAMO_HEAD_AFTER) { + // aggiungo prima della corrente + nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ; + if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL) + return false ; + // modifico l'entità originale (è la prima del percorso) + m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ; + DBLVECTOR vAxNew = pCamData->GetAxesVal() ; + for ( int i = 0 ; i < 2 ; ++ i) { + bool bLinear, bHead ; pMch->GetCurrAxisType( i, bLinear, bHead) ; + if ( ( ! bHead && nLKAMO == LKAMO_HEAD_BEFORE) || + ( bHead && nLKAMO == LKAMO_HEAD_AFTER)) + vAxNew[i] = vAxPrev[i] ; + } + // applico le modifiche + pCamData->SetAxes( CamData::AS_OK, vAxNew) ; + pCamData->ChangeAxesMask( CamData::MSK_L1 | CamData::MSK_L2 | CamData::MSK_L3 | CamData::MSK_R1 | CamData::MSK_R2 | CamData::MSK_R3) ; + pCamData->SetFlag( 0) ; + pCamData->SetFlag2( 0) ; + } + return true ; } } // se robot @@ -4315,7 +4344,7 @@ ResetMachine( Machine* pMch) //---------------------------------------------------------------------------- bool -Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd) const +Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, int* pnLKAMO) const { // Recupero macchina corrente Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ; @@ -4362,11 +4391,28 @@ Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEn return ( nRes == SCAV_AVOID) ; } - // Verifica standard + // Verifica + int nLinkAxisOrder = ( m_pMchMgr->GetCurrIsCenter() ? pMch->GetLinkAxesMoveOrder() : LKAMO_INTERP) ; + // se ordine dipende da variazione angolare, verifico quale ordine effettivo + if ( nLinkAxisOrder == LKAMO_HEAD_BEFORE_IF_SAME_ANG || nLinkAxisOrder == LKAMO_HEAD_AFTER_IF_SAME_ANG) { + // cerco massima variazione degli assi rotanti + double dRotDelta = 0 ; + for ( int i = 3 ; i < int( vAxStart.size()) ; ++ i) + dRotDelta = max( dRotDelta, abs( vAxEnd[i] - vAxStart[i])) ; + // se non si supera il limite + const double MAX_ROT_DELTA = 1 ; + if ( dRotDelta < MAX_ROT_DELTA) { + if ( nLinkAxisOrder == LKAMO_HEAD_BEFORE_IF_SAME_ANG) + nLinkAxisOrder = LKAMO_HEAD_BEFORE ; + else + nLinkAxisOrder = LKAMO_HEAD_AFTER ; + } + else + nLinkAxisOrder = LKAMO_INTERP ; + } bool bCollAvoid ; - int nLinkAxisOrder = LKAO_INTERP ; - // se ordine stabilito tra gli assi di tavola e testa (LKAO_HEAD_BEFORE(-1)=prima testa, LKAO_HEAD_AFTER(+1)=dopo testa) - if ( nLinkAxisOrder == LKAO_HEAD_BEFORE || nLinkAxisOrder == LKAO_HEAD_AFTER) { + // se ordine stabilito tra gli assi di tavola e testa (LKAMO_HEAD_BEFORE(-1)=prima testa, LKAMO_HEAD_AFTER(+1)=dopo testa) + if ( nLinkAxisOrder == LKAMO_HEAD_BEFORE || nLinkAxisOrder == LKAMO_HEAD_AFTER) { // assegno il valore degli assi nella posizione intermedia opportuna DBLVECTOR vAxMid ; GetAxisMidForTestCollisionAvoid( vAxStart, vAxEnd, nLinkAxisOrder, pMch, vAxMid) ; @@ -4374,23 +4420,22 @@ Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEn bCollAvoid = OneMoveTestCollisionAvoid( vAxName, vAxStart, vAxMid, pMch, vRawId, vFxtId) && OneMoveTestCollisionAvoid( vAxName, vAxMid, vAxEnd, pMch, vRawId, vFxtId) ; } - // se verifico entrambi gli ordini degli assi di tavola e testa LKAO_HEAD_BOTH(11) - else if ( nLinkAxisOrder == LKAO_HEAD_BOTH) { - // assegno il valore degli assi nelle posizioni intermedie - DBLVECTOR vAxMidB, vAxMidA ; - GetAxisMidForTestCollisionAvoid( vAxStart, vAxEnd, LKAO_HEAD_BEFORE, pMch, vAxMidB) ; - GetAxisMidForTestCollisionAvoid( vAxStart, vAxEnd, LKAO_HEAD_AFTER, pMch, vAxMidA) ; - // eseguo i test - bCollAvoid = OneMoveTestCollisionAvoid( vAxName, vAxStart, vAxMidB, pMch, vRawId, vFxtId) && - OneMoveTestCollisionAvoid( vAxName, vAxMidB, vAxEnd, pMch, vRawId, vFxtId) && - OneMoveTestCollisionAvoid( vAxName, vAxStart, vAxMidA, pMch, vRawId, vFxtId) && - OneMoveTestCollisionAvoid( vAxName, vAxMidA, vAxEnd, pMch, vRawId, vFxtId) ; - } - // altrimenti interpolo tutti gli assi tra loro LKAO_INTERP(0) + // altrimenti interpolo tutti gli assi tra loro LKAMO_INTERP(0) else { bCollAvoid = OneMoveTestCollisionAvoid( vAxName, vAxStart, vAxEnd, pMch, vRawId, vFxtId) ; } + // Se evitata collisione, assegno ordine di movimento usato + if ( bCollAvoid && pnLKAMO != nullptr) { + // assegno ordine movimenti necessari + *pnLKAMO = nLinkAxisOrder ; + // se non è movimento interpolato, verifico se anche questo la evita + if ( nLinkAxisOrder != LKAMO_INTERP) { + if ( OneMoveTestCollisionAvoid( vAxName, vAxStart, vAxEnd, pMch, vRawId, vFxtId)) + *pnLKAMO = LKAMO_INTERP ; + } + } + // Ripristino lo stato macchina ResetMachine( pMch) ; diff --git a/Operation.h b/Operation.h index f21d784..2675f11 100644 --- a/Operation.h +++ b/Operation.h @@ -237,7 +237,7 @@ class Operation : public IUserObj int GetUserNotesZmax( void) const ; double GetDeltaSafeZ( const Vector3d& vtTool) const ; bool GetZHomeDown( void) const ; - bool TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd) const ; + bool TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, int* pnLKAMO = nullptr) const ; int SpecialTestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd) const ; bool SpecialMoveZup( DBLVECTOR& vAx, Vector3d& vtTool, int& nFlag, int& nFlag2, bool& bModif) ; bool SpecialMoveRapid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, int nOutstroke, int nLinkType, @@ -331,4 +331,5 @@ enum { FACE_DOWN = 1, FACE_TOP = 2, FACE_FRONT = 3, FACE_BACK = 4, FACE_LEFT = 5 // risultato di SpecialTestCollisionAvoid enum { SCAV_ERROR = -1, SCAV_COLLIDE = 0, SCAV_AVOID = 1, SCAV_TOTEST = 2} ; // ordine di movimento assi nei link -enum { LKAO_INTERP = 0, LKAO_HEAD_BEFORE = -1, LKAO_HEAD_AFTER = +1, LKAO_HEAD_BOTH = 11} ; \ No newline at end of file +enum { LKAMO_INTERP = 0, LKAMO_HEAD_BEFORE = -1, LKAMO_HEAD_AFTER = +1, + LKAMO_HEAD_BEFORE_IF_SAME_ANG = -2, LKAMO_HEAD_AFTER_IF_SAME_ANG = +2} ; \ No newline at end of file