From 4957e09abe45fd1f7f005932b15ebd146545c5a7 Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Tue, 18 Aug 2026 13:33:41 +0200 Subject: [PATCH] =?UTF-8?q?EgtMachKernel=20:=20-=20al=20carico=20macchina?= =?UTF-8?q?=20correzione=20aggiustamento=20assi=20rotanti=20geometrici=20p?= =?UTF-8?q?er=20simulazione=20quando=20angolo=20di=20rotazione=20da=20teor?= =?UTF-8?q?ici=20=C3=A8=20molto=20piccolo=20-=20al=20carico=20macchina=20q?= =?UTF-8?q?uando=20si=20ruotano=20assi=20rotanti=20e=20richiesto=20ora=20s?= =?UTF-8?q?i=20ruota=20anche=20geometria=20ausiliaria.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Machine.cpp | 39 +++++++++++++++++++++++++++------------ MachineCalc.cpp | 12 ++++++------ 2 files changed, 33 insertions(+), 18 deletions(-) diff --git a/Machine.cpp b/Machine.cpp index cad7566..bd53f40 100644 --- a/Machine.cpp +++ b/Machine.cpp @@ -385,6 +385,10 @@ Machine::AdjustAxis( int nLay, const string& sPart, const string& sName, ptBase.ToGlob( frFrame) ; vtAxis.ToGlob( frFrame) ; + // per assi rotanti + Vector3d vtRotAx ; + double dAngRot = 0 ; + // se asse lineare devo verificarne solo la direzione if ( nType == MCH_AT_LINEAR) { Vector3d vtDirN = vtDir ; @@ -395,21 +399,28 @@ Machine::AdjustAxis( int nLay, const string& sPart, const string& sName, } } - // altrimenti asse rotante, devo verificarne direzione e coassialità (questa solo se non abilitato movimento) + // altrimenti asse rotante, devo verificarne direzione e coassialità (questa solo se non abilitato movimento) else { Vector3d vtDirN = vtDir ; - if ( ! vtDirN.Normalize() || ! AreSameVectorEpsilon( vtAxis, vtDirN, 10 * SIN_EPS_ANG_SMALL)) { - double dAngRot ; - if ( ! vtAxis.GetAngle( vtDirN, dAngRot) || abs( dAngRot) > m_dAxisMaxRotAdj) { - string sOut = " Wrong Axis Vector or Dir " + sPart ; + if ( ! vtDirN.Normalize() || ! AreSameVectorEpsilon( vtAxis, vtDirN, SIN_EPS_ANG_SMALL)) { + if ( ! vtAxis.GetAngle( vtDirN, dAngRot)) { + string sOut = " Wrong Axis Vector (" + ToString( vtDirN) + ")" + sPart ; + LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; + return false ; + } + else if ( abs( dAngRot) > m_dAxisMaxRotAdj) { + string sOut = " Wrong Axis Dir (rotation =" + ToString( dAngRot) + ")" + sPart ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } else { - Vector3d vtRotAx = vtAxis ^ vtDirN ; vtRotAx.Normalize() ; + vtRotAx = vtAxis ^ vtDirN ; vtRotAx.Normalize( EPS_ZERO) ; string sOut = " Axis " + sName + " rotation = (" + ToString( dAngRot) + "/" + ToString( vtRotAx) + ")" ; + if ( m_pGeomDB->RotateGlob( nId, ptBase, vtRotAx, dAngRot)) + sOut += " OK" ; + else + sOut += " ERROR" ; LOG_DBG_INFO( GetEMkLogger(), sOut.c_str()) ; - m_pGeomDB->RotateGlob( nId, ptBase, vtRotAx, dAngRot) ; } } Vector3d vtDelta = ptPos - ptBase ; @@ -427,13 +438,17 @@ Machine::AdjustAxis( int nLay, const string& sPart, const string& sName, } } } + // se richiesto, muovo la geometria ausiliaria if ( bAdjustAux) { Vector3d vtDelta = ptPos - ptBase ; int nIdAux = m_pGeomDB->GetFirstInGroup( nLay) ; while ( nIdAux != GDB_ID_NULL) { - if ( nIdAux != nId) + if ( nIdAux != nId) { + if ( abs( dAngRot) > EPS_ANG_SMALL) + m_pGeomDB->RotateGlob( nIdAux, ptBase, vtRotAx, dAngRot) ; m_pGeomDB->TranslateGlob( nIdAux, vtDelta) ; + } nIdAux = m_pGeomDB->GetNext( nIdAux) ; } } @@ -829,7 +844,7 @@ Machine::GetExit( int nGroup) const bool Machine::AddHeadToSet( const string& sHSet, const string& sName) { - // se il capo-insieme coincide con la testa, non devo fare alcunchè + // se il capo-insieme coincide con la testa, non devo fare alcunchè if ( sHSet == sName) return true ; // recupero la testa capo-insieme @@ -854,7 +869,7 @@ Machine::GetHSet( const string& sHead) const const STRVECTOR& vsTmp = pHead->GetHSet() ; if ( vsTmp.empty()) return vsNull ; - // se il primo membro coincide con la testa, allora è già l'insieme di teste + // se il primo membro coincide con la testa, allora è già l'insieme di teste if ( vsTmp[0] == sHead) return vsTmp ; // altrimenti cerco l'insieme della prima testa @@ -899,7 +914,7 @@ Machine::ModifyMachineHeadAuxDirection( const string& sHead, const Vector3d& vtA bool Machine::AdjustExitFrames( int nLay, const MUEXITVECTOR& vMuExit, const Vector3d& vtADir) { - // se il versore ausiliario non è definito, non sono necessari controlli + // se il versore ausiliario non è definito, non sono necessari controlli if ( vtADir.IsSmall()) return true ; // riferimento globale del gruppo testa @@ -1062,7 +1077,7 @@ Machine::ModifyMachineExitPosition( const string& sHead, int nExit, const Point3 bool Machine::SetLook( int nFlag) { - // verifico validità flag + // verifico validità flag if ( nFlag < MCH_LOOK_TAB || nFlag > MCH_LOOK_ALL) return false ; diff --git a/MachineCalc.cpp b/MachineCalc.cpp index 567313e..2353acb 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -1401,11 +1401,11 @@ Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, vtTool.Rotate( RotAx2.vtDir, dAngB1) ; vtTool.Rotate( RotAx1.vtDir, dAngA1) ; bool bDetC1 ; - if ( ! vtTool.GetRotation( vtDirTn, vtR3, EPS_SMALL, dAngC1, bDetC1) || ! bDetC1) { + if ( ! vtTool.GetRotation( vtDirTn, vtR3, SIN_EPS_ANG_SMALL, dAngC1, bDetC1) || ! bDetC1) { Vector3d vtAux = vtDirI ; vtAux.Rotate( RotAx2.vtDir, dAngB1) ; vtAux.Rotate( RotAx1.vtDir, dAngA1) ; - if ( ! vtAux.GetRotation( vtDirAn, vtR3, EPS_SMALL, dAngC1, bDetC1) || ! bDetC1) { + if ( ! vtAux.GetRotation( vtDirAn, vtR3, SIN_EPS_ANG_SMALL, dAngC1, bDetC1) || ! bDetC1) { LOG_ERROR( GetEMkLogger(), "Error : R3 not calculable (machine)") return false ; } @@ -2398,7 +2398,7 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto // calcolo il primo asse rotante Vector3d vtArm = ptCen - m_vCalcRotAx[0].ptPos ; double dAng1 ; bool bDet1 ; - if ( ! X_AX.GetRotation( vtArm, m_vCalcRotAx[0].vtDir, EPS_SMALL, dAng1, bDet1) || ! bDet1) { + if ( ! X_AX.GetRotation( vtArm, m_vCalcRotAx[0].vtDir, SIN_EPS_ANG_SMALL, dAng1, bDet1) || ! bDet1) { LOG_ERROR( GetEMkLogger(), "Error : direction unreachable (robot)") return false ; } @@ -2430,7 +2430,7 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto } double dAngB = acos( dCosB) * RADTODEG ; double dAng2 ; bool bDet2 ; - if ( ! Z_AX.GetRotation( vtR2Cen, vtR2, EPS_SMALL, dAng2, bDet2) || ! bDet2) { + if ( ! Z_AX.GetRotation( vtR2Cen, vtR2, SIN_EPS_ANG_SMALL, dAng2, bDet2) || ! bDet2) { LOG_ERROR( GetEMkLogger(), "Error : R2 not calculable (robot)") return false ; } @@ -2472,11 +2472,11 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto for ( int i = 4; i >= 0 ; --i) vtTool.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ; double dAng6 ; bool bDet6 ; - if ( ! vtTool.GetRotation( vtDirTL, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) { + if ( ! vtTool.GetRotation( vtDirTL, vtR6, SIN_EPS_ANG_SMALL, dAng6, bDet6) || ! bDet6) { Vector3d vtAux = m_vtCalcADir ; for ( int i = 4; i >= 0 ; --i) vtAux.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ; - if ( ! vtAux.GetRotation( vtDirAL, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) { + if ( ! vtAux.GetRotation( vtDirAL, vtR6, SIN_EPS_ANG_SMALL, dAng6, bDet6) || ! bDet6) { LOG_ERROR( GetEMkLogger(), "Error : R6 not calculable (robot)") return false ; }