diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index c7268ec..97de961 100644 Binary files a/EgtMachKernel.rc and b/EgtMachKernel.rc differ diff --git a/Machine.cpp b/Machine.cpp index c0da66c..413bab4 100644 --- a/Machine.cpp +++ b/Machine.cpp @@ -36,7 +36,9 @@ Machine::Machine( void) m_nGroupId = GDB_ID_NULL ; m_nTempGroupId = GDB_ID_NULL ; m_dAxisMaxAdjust = EPS_SMALL ; + m_dAxisMaxRotAdj = 10 * EPS_ANG_SMALL ; m_dExitMaxAdjust = EPS_SMALL ; + m_dExitMaxRotAdj = 10 * EPS_ANG_SMALL ; m_dAngDeltaMinForHome = INFINITO ; m_nCalcTabId = GDB_ID_NULL ; m_nCalcHeadId = GDB_ID_NULL ; @@ -376,10 +378,19 @@ Machine::AdjustAxis( int nLay, const string& sPart, const string& sName, // altrimenti asse rotante, devo verificarne direzione e coassialità (questa solo se non abilitato movimento) else { Vector3d vtDirN = vtDir ; - if ( ! vtDirN.Normalize() || ! AreSameVectorApprox( vtAxis, vtDirN)) { - string sOut = " Wrong Axis Vector or Dir " + sPart ; - LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; - return false ; + if ( ! vtDirN.Normalize() || ! AreSameVectorEpsilon( vtAxis, vtDirN, 10 * SIN_EPS_ANG_SMALL)) { + double dAngRot ; + if ( ! vtAxis.GetAngle( vtDirN, dAngRot) || abs( dAngRot) > m_dExitMaxRotAdj) { + string sOut = " Wrong Axis Vector or Dir " + sPart ; + LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; + return false ; + } + else { + Vector3d vtRotAx = vtAxis ^ vtDirN ; vtRotAx.Normalize() ; + string sOut = " Axis " + sName + " rotation = (" + ToString( dAngRot) + "/" + ToString( vtRotAx) + ")" ; + LOG_DBG_INFO( GetEMkLogger(), sOut.c_str()) ; + m_pGeomDB->RotateGlob( nId, ptBase, vtRotAx, dAngRot) ; + } } Vector3d vtDelta = ptPos - ptBase ; Vector3d vtDeltaPerp = vtDelta - ( vtDelta * vtDirN) * vtDirN ; @@ -390,7 +401,7 @@ Machine::AdjustAxis( int nLay, const string& sPart, const string& sName, return false ; } else { - string sOut = " Move = (" + ToString( vtDelta) + ")" ; + string sOut = " Axis " + sName + " move = (" + ToString( vtDelta) + ")" ; LOG_DBG_INFO( GetEMkLogger(), sOut.c_str()) ; m_pGeomDB->TranslateGlob( nId, vtDelta) ; } @@ -777,12 +788,12 @@ Machine::CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit) Vector3d vtDelta = vMuExit[i].ptPos - ptPos ; if ( ! vtDelta.IsSmall()) { if ( vtDelta.Len() > m_dExitMaxAdjust) { - string sOut = " Wrong Exit Position " + sName + " move = (" + ToString( vtDelta) +")" ; + string sOut = " Wrong Exit Position " + sName + " move = (" + ToString( vtDelta) + ")" ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } else { - string sOut = " Exit " + sName + " move = (" + ToString( vtDelta) +")" ; + string sOut = " Exit " + sName + " move = (" + ToString( vtDelta) + ")" ; LOG_DBG_INFO( GetEMkLogger(), sOut.c_str()) ; vtDelta.ToLoc( frHead) ; frFrame.Translate( vtDelta) ; @@ -790,10 +801,21 @@ Machine::CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit) } } Vector3d vtDirN = vMuExit[i].vtTDir ; - if ( ! vtDirN.Normalize() || ! AreSameVectorApprox( vtTDir, vtDirN)) { - string sOut = " Wrong Exit Vector " + sName ; - LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; - return false ; + if ( ! vtDirN.Normalize() || ! AreSameVectorEpsilon( vtTDir, vtDirN, 10 * SIN_EPS_ANG_SMALL)) { + double dAngRot ; + if ( ! vtTDir.GetAngle( vtDirN, dAngRot) || abs( dAngRot) > m_dExitMaxRotAdj) { + string sOut = " Wrong Exit Vector " + sName ; + LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; + return false ; + } + else { + Vector3d vtRotAx = vtTDir ^ vtDirN ; vtRotAx.Normalize() ; + string sOut = " Exit " + sName + " rotation = (" + ToString( dAngRot) + "/" + ToString( vtRotAx) + ")" ; + LOG_DBG_INFO( GetEMkLogger(), sOut.c_str()) ; + vtRotAx.ToLoc( frHead) ; + frFrame.Rotate( frFrame.Orig(), vtRotAx, dAngRot) ; + GetGeoFrame3d( m_pGeomDB->GetGeoObj( nT))->Set( frFrame) ; + } } // inserisco gruppo con riferimento del frame int nGT = m_pGeomDB->InsertGroup( GDB_ID_NULL, nT, GDB_BEFORE, frFrame) ; diff --git a/Machine.h b/Machine.h index 3ecda2e..e16c807 100644 --- a/Machine.h +++ b/Machine.h @@ -244,7 +244,9 @@ class Machine int m_nTempGroupId ; // Id del gruppo temporaneo macchina per carico STRINT_UMAP m_mapGroups ; // dizionario dei gruppi della macchina double m_dAxisMaxAdjust ; // massimo aggiustamento asse da geometria a descrizione cinematica + double m_dAxisMaxRotAdj ; // massima rotazione di aggiustamento asse da geometria a descrizione cinematica double m_dExitMaxAdjust ; // massimo aggiustamento uscita da geometria a descrizione cinematica + 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 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 420ede2..ad227e0 100644 --- a/MachineLua.cpp +++ b/MachineLua.cpp @@ -30,7 +30,9 @@ static const string FLD_OFFSET = "Offset" ; static const string FLD_SPECIAL = "Special" ; static const string FLD_PROCESSOR = "Processor" ; static const string FLD_AXISMAXADJUST = "AxisMaxAdjust" ; +static const string FLD_AXISMAXROTADJ = "AxisMaxRotAdj" ; static const string FLD_EXITMAXADJUST = "ExitMaxAdjust" ; +static const string FLD_EXITMAXROTADJ = "ExitMaxRotAdj" ; static const string FLD_ANGDELTAMINFORHOME = "AngDeltaMinForHome" ; static const string FLD_NAME = "Name" ; static const string FLD_PARENT = "Parent" ; @@ -275,9 +277,15 @@ Machine::LuaEmtGeneral( lua_State* L) // lettura eventuale campo 'AxisMaxAdjust' dalla tabella (default EPS_SMALL) double dAxisMaxAdjust = EPS_SMALL ; LuaGetTabFieldParam( L, 1, FLD_AXISMAXADJUST, dAxisMaxAdjust) ; + // lettura eventuale campo 'AxisMaxRotAdj' dalla tabella (default 10 * EPS_ANG_SMALL) + double dAxisMaxRotAdj = 10 * EPS_ANG_SMALL ; + LuaGetTabFieldParam( L, 1, FLD_AXISMAXROTADJ, dAxisMaxRotAdj) ; // lettura eventuale campo 'ExitMaxAdjust' dalla tabella (default EPS_SMALL) double dExitMaxAdjust = EPS_SMALL ; LuaGetTabFieldParam( L, 1, FLD_EXITMAXADJUST, dExitMaxAdjust) ; + // lettura eventuale campo 'ExitMaxRotAdj' dalla tabella (default 10 * EPS_ANG_SMALL) + double dExitMaxRotAdj = 10 * EPS_ANG_SMALL ; + LuaGetTabFieldParam( L, 1, FLD_EXITMAXROTADJ, dExitMaxRotAdj) ; // lettura eventuale campo 'AngDeltaForHome' dalla tabella (default INFINITO) double dAngDeltaMinForHome = INFINITO ; LuaGetTabFieldParam( L, 1, FLD_ANGDELTAMINFORHOME, dAngDeltaMinForHome) ; @@ -323,7 +331,9 @@ Machine::LuaEmtGeneral( lua_State* L) // imposto massimo spostamento assi e uscite tra definizione cinematica e geometria m_pMchLua->m_dAxisMaxAdjust = dAxisMaxAdjust ; + m_pMchLua->m_dAxisMaxRotAdj = dAxisMaxRotAdj ; m_pMchLua->m_dExitMaxAdjust = dExitMaxAdjust ; + m_pMchLua->m_dExitMaxRotAdj = dExitMaxRotAdj ; // imposto minima differenza angolare da posizione precedente per stare vivino a posizione home m_pMchLua->m_dAngDeltaMinForHome = dAngDeltaMinForHome ;