//---------------------------------------------------------------------------- // EgalTech 2015-2015 //---------------------------------------------------------------------------- // File : MachineCalc.cpp Data : 12.05.15 Versione : 1.6e3 // Contenuto : Implementazione gestione macchina : funzioni di calcolo. // // // // Modifiche : 12.05.15 DS Creazione modulo. // // //---------------------------------------------------------------------------- //--------------------------- Include ---------------------------------------- #include "stdafx.h" #include "MachMgr.h" #include "GeoCalc.h" #include "DllMain.h" #include "Table.h" #include "Axis.h" #include "Head.h" #include "Exit.h" #include "/EgtDev/Include/EMkToolConst.h" #include "/EgtDev/Include/EGkGeoVector3d.h" #include "/EgtDev/Include/EGnStringUtils.h" #include "/EgtDev/Include/EGnFileUtils.h" using namespace std ; //---------------------------------------------------------------------------- static const string EMC_VAR = "EMC" ; // tabella variabili locali per calcolo static const string EVAR_TABNAME = ".TABNAME" ; // (string) nome della tavola macchina static const string EVAR_HEAD = ".HEAD" ; // (string) nome della testa static const string EVAR_EXIT = ".EXIT" ; // (int) numero dell'uscita static const string EVAR_TOOL = ".TOOL" ; // (string) nome dell'utensile static const string EVAR_TOTDIAM = ".TOTDIAM" ; // (num) diametro di ingombro dell'utensile static const string EVAR_TOTLEN = ".TOTLEN" ; // (num) lunghezza di ingombro dell'utensile static const string EVAR_DIST = ".DIST" ; // (num) distanza dell'utensile (per seghe a catena) static const string EVAR_EXITPOS = ".EXITPOS" ; // (point) posizione attuale dell'uscita static const string EVAR_TCPOS = ".TCPOS" ; // (string) posizione nell'attrezzaggio static const string EVAR_L1 = ".L1" ; // (num) valore del primo asse lineare static const string EVAR_L2 = ".L2" ; // (num) valore del secondo asse lineare static const string EVAR_L3 = ".L3" ; // (num) valore del terzo asse lineare static const string EVAR_R1 = ".R1" ; // (num) valore del primo asse rotante static const string EVAR_R2 = ".R2" ; // (num) valore del secondo asse rotante static const string EVAR_R3 = ".R3" ; // (num) valore del terzo asse rotante static const string EVAR_R4 = ".R4" ; // (num) valore del quarto asse rotante static const string EVAR_ERROR = ".ERR" ; // OUT (int) codice di errore ( 0 = ok) static const string EVAR_STAT = ".STAT" ; // OUT (int) codice di stato ( 0 = ok) static const string EVAR_AUXINFO = ".AUXINFO" ; // OUT (string) stringa con info ausiliarie static const string EMC_VAR_BACKUP = "QQQ_EMC" ; // nome del backup della tabella sopra indicata static const string AXIS_NAME_PROTECTEDAREAS = "PRA" ; static const string ON_SET_TABLE = "OnSetTable" ; static const string ON_SET_HEAD = "OnSetHead" ; static const string ON_VERIFY_PROTECTEDAREAS = "OnVerifyProtectedAreas" ; //---------------------------------------------------------------------------- bool Machine::SetCurrTable( const string& sTable) { // controllo GeomDB if ( m_pGeomDB == nullptr) return false ; // recupero il gruppo della tavola m_nCalcTabId = GetGroup( sTable) ; if ( m_nCalcTabId == GDB_ID_NULL || ! IsTableGroup( m_nCalcTabId)) { m_nCalcTabId = GDB_ID_NULL ; return false ; } // lancio eventuale funzione lua di personalizzazione if ( LuaExistsFunction( ON_SET_TABLE)) { // salvo eventuale variabile EMC_VAR già presente bool bOldEMC = LuaChangeNameGlobVar( EMC_VAR, EMC_VAR_BACKUP) ; // definisco variabili bool bOk = LuaCreateGlobTable( EMC_VAR) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TABNAME, sTable) ; // chiamo funzione bOk = bOk && LuaCallFunction( ON_SET_TABLE) ; // reset variabili bOk = bOk && LuaResetGlobVar( EMC_VAR) ; // ripristino eventuale variabile EMC_VAR già presente if ( bOldEMC) LuaChangeNameGlobVar( EMC_VAR_BACKUP, EMC_VAR) ; // restituisco risultato return bOk ; } else return true ; } //---------------------------------------------------------------------------- bool Machine::ResetCurrTable( void) { m_nCalcTabId = GDB_ID_NULL ; return true ; } //---------------------------------------------------------------------------- int Machine::GetCurrTable( void) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return GDB_ID_NULL ; // recupero identificativo della tavola corrente return m_nCalcTabId ; } //---------------------------------------------------------------------------- bool Machine::GetCurrTable( string& sTable) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return false ; // recupero nome della tavola corrente return m_pGeomDB->GetName( m_nCalcTabId, sTable) ; } //---------------------------------------------------------------------------- bool Machine::GetCurrTableRef1( Point3d& ptRef1) const { Table* pTab = GetTable( m_nCalcTabId) ; if ( pTab == nullptr) return false ; ptRef1 = pTab->GetRef1() ; return true ; } //---------------------------------------------------------------------------- bool Machine::GetCurrTableArea1( BBox3d& b3Area1) const { Table* pTab = GetTable( m_nCalcTabId) ; if ( pTab == nullptr) return false ; b3Area1 = pTab->GetArea1() ; return true ; } //---------------------------------------------------------------------------- bool Machine::GetCurrTableDeltaRef1( Vector3d& vtDelta1) const { Table* pTab = GetTable( m_nCalcTabId) ; if ( pTab == nullptr) return false ; // recupero la posizione corrente del riferimento 1 della tavola // riferimento globale del gruppo tavola Frame3d frTable ; m_pGeomDB->GetGroupGlobFrame( m_nCalcTabId, frTable) ; // recupero il primo riferimento della tavola int nRef1 = m_pGeomDB->GetFirstNameInGroup( m_nCalcTabId, MCH_TREF + "1") ; if ( nRef1 == GDB_ID_NULL || m_pGeomDB->GetGeoType( nRef1) != GEO_FRAME3D) return false ; // recupero frame const Frame3d& frFrame = GetGeoFrame3d( m_pGeomDB->GetGeoObj( nRef1))->GetFrame() ; // ne calcolo l'origine in globale Point3d ptPos = frFrame.Orig() ; ptPos.ToGlob( frTable) ; // calcolo il delta vtDelta1 = ptPos - pTab->GetRef1() ; return true ; } //---------------------------------------------------------------------------- bool Machine::GetCurrTableIsTilting( bool& bTilting) const { // default bTilting = false ; // verifico esistenza tavola if ( m_nCalcTabId == GDB_ID_NULL) return false ; // verifico se presente flag che lo forza come tale bool bTiltingLike ; if ( m_pGeomDB->GetInfo( 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) return false ; while ( IsAxisGroup( nTParId)) { // recupero il gestore dell'asse Axis* pAx = GetAxis( nTParId) ; if ( pAx == nullptr) return false ; // verifico se asse rotante orizzontale if ( pAx->GetType() == MCH_AT_ROTARY && abs( pAx->GetDir().z) < EPS_SMALL) { bTilting = true ; return true ; } // risalgo lungo la catena nTParId = m_pGeomDB->GetParentId( nTParId) ; } return true ; } //---------------------------------------------------------------------------- bool Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit) { // azzero tutto ResetCurrTool() ; // controllo GeomDB if ( m_pGeomDB == nullptr) return false ; // recupero il gruppo della testa int nHeadId = GetGroup( sHead) ; // recupero i dati della testa Head* pHead = GetHead( nHeadId) ; if ( pHead == nullptr) { string sOutHead = ( IsEmptyOrSpaces( sHead) ? "???" : sHead) ; string sOutTool = ( IsEmptyOrSpaces( sTool) ? "???" : sTool) ; string sOut = "Missing head " + sOutHead + " for tool " + sOutTool ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) return false ; } // recupero il gruppo dell'uscita string sExit = MCH_EXIT + ToString( nExit) ; int nExitId = m_pGeomDB->GetFirstNameInGroup( nHeadId, sExit) ; // recupero i dati dell'uscita Exit* pExit = GetExit( nExitId) ; if ( pExit == nullptr) { string sOut = "Missing exit " + ToString( nExit) + " on head " + sHead ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) return false ; } // recupero i dati dell'utensile int nToolId = GDB_ID_NULL ; double dTLen = 0 ; double dTDiam = 0 ; double dTOvLen = 0 ; double dTOvDiam = 0 ; double dTDist = 0 ; string sTcPos = "" ; // se definito if ( ! sTool.empty()) { // carico anche gli utensili su eventuali altre uscite della testa LoadTools( sHead, nExit) ; // carico l'utensile (si sostituisce ad altro già presente sulla stessa uscita) if ( ! LoadTool( sHead, nExit, sTool)) { string sOut = "Missing tool " + sTool ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) return false ; } nToolId = m_pGeomDB->GetFirstNameInGroup( nExitId, sTool) ; if ( nToolId == GDB_ID_NULL || m_pGeomDB->GetGdbType( nToolId) != GDB_TY_GROUP) return false ; if ( ! m_pMchMgr->TdbSetCurrTool( sTool) || ! m_pMchMgr->TdbGetCurrToolParam( TPA_LEN, dTLen) || ! m_pMchMgr->TdbGetCurrToolParam( TPA_DIAM, dTDiam) || ! m_pMchMgr->TdbGetCurrToolParam( TPA_TOTLEN, dTOvLen) || ! m_pMchMgr->TdbGetCurrToolParam( TPA_TOTDIAM, dTOvDiam)) return false ; m_pMchMgr->TdbGetCurrToolParam( TPA_DIST, dTDist) ; // opzionale if ( ! m_pMchMgr->GetCurrSetupMgr().GetToolSetupPos( sTool, sTcPos)) m_pMchMgr->TdbGetCurrToolParam( TPA_TCPOS, sTcPos) ; } // altrimenti casi speciali senza utensile else { // tolgo eventuale utensile dalla testa if ( ! ResetHeadSet( sHead)) return false ; nToolId = GDB_ID_NULL ; dTLen = 0 ; dTDiam = 0 ; dTOvLen = 0 ; dTOvDiam = 0 ; dTDist = 0 ; sTcPos = "" ; m_pMchMgr->TdbSetCurrTool( "") ; } // assegno tutti i dati m_nCalcHeadId = nHeadId ; m_nCalcExitId = nExitId ; m_nCalcToolId = nToolId ; m_dCalcRot1W = pHead->GetRot1W() ; m_bCalcMaxDeltaR2On1 = pHead->GetMaxDeltaR2On1() ; m_nCalcSolCh = pHead->GetSolCh() ; m_ptCalcPos = pExit->GetPos() ; m_vtCalcDir = pExit->GetTDir() ; m_vtCalcADir = pHead->GetADir() ; m_dCalcTLen = dTLen ; m_dCalcTRad = dTDiam / 2 ; m_dCalcTOvLen = dTOvLen ; m_dCalcTOvRad = dTOvDiam / 2 ; // lancio eventuale funzione lua di personalizzazione if ( LuaExistsFunction( ON_SET_HEAD)) { // salvo eventuale variabile EMC_VAR già presente bool bOldEMC = LuaChangeNameGlobVar( EMC_VAR, EMC_VAR_BACKUP) ; // definisco variabili bool bOk = LuaCreateGlobTable( EMC_VAR) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_HEAD, sHead) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_EXIT, nExit) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TOOL, sTool) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_EXITPOS, m_ptCalcPos) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TOTDIAM, dTOvDiam) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TOTLEN, dTOvLen) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_DIST, dTDist) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TCPOS, sTcPos) ; // chiamo funzione bOk = bOk && LuaCallFunction( ON_SET_HEAD) ; // reset variabili bOk = bOk && LuaResetGlobVar( EMC_VAR) ; // ripristino eventuale variabile EMC_VAR già presente if ( bOldEMC) LuaChangeNameGlobVar( EMC_VAR_BACKUP, EMC_VAR) ; // in caso di errore esco if ( ! bOk) return false ; // aggiorno m_ptCalcPos = pExit->GetPos() ; } // determino la catena cinematica return CalculateKinematicChain() ; } //---------------------------------------------------------------------------- bool Machine::ResetCurrTool( void) { // azzero tutto m_nCalcHeadId = GDB_ID_NULL ; m_nCalcExitId = GDB_ID_NULL ; m_nCalcToolId = GDB_ID_NULL ; m_dCalcRot1W = 1 ; m_nCalcSolCh = MCH_SCC_NONE ; m_dCalcTLen = 0 ; m_dCalcTRad = 0 ; m_dCalcTOvLen = 0 ; m_dCalcTOvRad = 0 ; // ritorno return true ; } //---------------------------------------------------------------------------- int Machine::GetCurrTool( void) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return GDB_ID_NULL ; // recupero identificativo dell'utensile return m_nCalcToolId ; } //---------------------------------------------------------------------------- bool Machine::GetCurrTool( string& sTool) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return false ; // recupero nome gruppo dell'utensile return m_pGeomDB->GetName( m_nCalcToolId, sTool) ; } //---------------------------------------------------------------------------- int Machine::GetCurrHead( void) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return GDB_ID_NULL ; // recupero identificativo della testa return m_nCalcHeadId ; } //---------------------------------------------------------------------------- bool Machine::GetCurrHead( string& sHead) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return false ; // recupero nome gruppo della testa return m_pGeomDB->GetName( m_nCalcHeadId, sHead) ; } //---------------------------------------------------------------------------- int Machine::GetCurrExit( void) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return GDB_ID_NULL ; // recupero identificativo dell'uscita return m_nCalcExitId ; } //---------------------------------------------------------------------------- bool Machine::GetCurrExit( int& nExit) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return GDB_ID_NULL ; // recupero nome gruppo dell'uscita string sExit ; if ( ! m_pGeomDB->GetName( m_nCalcExitId, sExit)) return false ; // recupero indice dell'uscita TrimLeft( sExit, MCH_EXIT.c_str()) ; return FromString( sExit, nExit) ; } //---------------------------------------------------------------------------- bool Machine::GetCurrHeadCollGroups( INTVECTOR& vIds) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return false ; // recupero la testa corrente Head* pHead = GetHead( m_nCalcHeadId) ; if ( pHead == nullptr) return false ; vIds.push_back( m_nCalcHeadId) ; // recupero stringhe con gruppi ausiliari di collisione const STRVECTOR& vsOthColl = pHead->GetOtherCollGroups() ; for ( const auto& sOthColl : vsOthColl) { string sGrp, sSub ; Split( sOthColl, "/", true, sGrp, sSub) ; int nId = GetGroup( sGrp) ; if ( ! sSub.empty() && nId != GDB_ID_NULL) nId = m_pGeomDB->GetFirstNameInGroup( nId, sSub) ; if ( nId != GDB_ID_NULL) vIds.push_back( nId) ; } return true ; } //---------------------------------------------------------------------------- bool Machine::CalculateKinematicChain( void) { // controllo GeomDB 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 if ( m_nCalcTabId == GDB_ID_NULL) return false ; 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 ; nTParId = m_pGeomDB->GetParentId( nTParId) ; } // recupero gli assi di testa if ( m_nCalcHeadId == GDB_ID_NULL) return false ; 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 ; // devono essere ordinabili come XYZ if ( ! m_vCalcLinAx[0].vtDir.IsX()) { if ( m_vCalcLinAx[1].vtDir.IsX()) swap( m_vCalcLinAx[0], m_vCalcLinAx[1]) ; else if ( m_vCalcLinAx[2].vtDir.IsX()) swap( m_vCalcLinAx[0], m_vCalcLinAx[2]) ; else { LOG_ERROR( GetEMkLogger(), "Linear Axes are not aligned with Global XYZ") return false ; } } if ( ! m_vCalcLinAx[1].vtDir.IsY()) { if ( m_vCalcLinAx[2].vtDir.IsY()) swap( m_vCalcLinAx[1], m_vCalcLinAx[2]) ; else { LOG_ERROR( GetEMkLogger(), "Linear Axes are not aligned with Global XYZ") return false ; } } if ( ! m_frLinAx.Set( ORIG, m_vCalcLinAx[0].vtDir, m_vCalcLinAx[1].vtDir, m_vCalcLinAx[2].vtDir)) { LOG_ERROR( GetEMkLogger(), "Linear Axes are not a Rigth-handed Frame") 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) bOk = true ; // se 2 va bene 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]) ; // impongo limiti di corsa sul secondo asse rotante di testa if ( m_vCalcRotAx[1].bHead) { Head* pHead = GetHead( m_nCalcHeadId) ; if ( pHead != nullptr) { m_vCalcRotAx[1].stroke.Min = max( m_vCalcRotAx[1].stroke.Min, pHead->GetRot2Stroke().Min) ; m_vCalcRotAx[1].stroke.Max = min( m_vCalcRotAx[1].stroke.Max, pHead->GetRot2Stroke().Max) ; } } bOk = true ; } // se 3 va bene ( uno dovrà poi avere valore assegnato) 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) { swap( m_vCalcRotAx[0], m_vCalcRotAx[2]) ; n2ndHeadRotAx = 1 ; } // se altrimenti ultimi due di testa, devo invertirne l'ordine else if ( m_vCalcRotAx[1].bHead && m_vCalcRotAx[2].bHead) { swap( m_vCalcRotAx[1], m_vCalcRotAx[2]) ; n2ndHeadRotAx = 2 ; } // impongo limiti di corsa sul secondo asse rotante di testa if ( n2ndHeadRotAx > 0) { Head* pHead = GetHead( m_nCalcHeadId) ; if ( pHead != nullptr) { m_vCalcRotAx[n2ndHeadRotAx].stroke.Min = max( m_vCalcRotAx[n2ndHeadRotAx].stroke.Min, pHead->GetRot2Stroke().Min) ; m_vCalcRotAx[n2ndHeadRotAx].stroke.Max = min( m_vCalcRotAx[n2ndHeadRotAx].stroke.Max, pHead->GetRot2Stroke().Max) ; } } 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 LOG_ERROR( GetEMkLogger(), "Rotary Axes not manageable") return false ; } //---------------------------------------------------------------------------- bool Machine::AddKinematicAxis( bool bOnHead, int nId) { // controllo GeomDB if ( m_pGeomDB == nullptr) return false ; // recupero il gestore dell'asse Axis* pAx = GetAxis( nId) ; if ( pAx == nullptr) return false ; // ne recupero i dati KinAxis kAx ; kAx.nGrpId = nId ; kAx.bLinear = ( pAx->GetType() != MCH_AT_ROTARY) ; 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() ; // lo inserisco nella opportuna lista degli assi if ( kAx.bLinear) m_vCalcLinAx.emplace_back( kAx) ; else m_vCalcRotAx.emplace_back( kAx) ; return true ; } //---------------------------------------------------------------------------- string Machine::GetKinematicAxis( int nInd) const { // controllo GeomDB if ( m_pGeomDB == nullptr) return "" ; // controllo indice int nLinAxTot = int( m_vCalcLinAx.size()) ; int nRotAxTot = int( m_vCalcRotAx.size()) ; if ( nInd < 0 || nInd >= nLinAxTot + nRotAxTot) return "" ; // recupero nome string sName ; if ( nInd < nLinAxTot) m_pGeomDB->GetName( m_vCalcLinAx[nInd].nGrpId, sName) ; else m_pGeomDB->GetName( m_vCalcRotAx[nInd-nLinAxTot].nGrpId, sName) ; return sName ; } //---------------------------------------------------------------------------- bool Machine::BlockKinematicRotAxis( const string& sName, double dVal) { return BlockKinematicRotAxis( GetAxisId( sName), dVal) ; } //---------------------------------------------------------------------------- bool Machine::BlockKinematicRotAxis( int nId, double dVal) { // verifico identificativo if ( nId == GDB_ID_NULL) return false ; // cerco l'asse rotante di calcolo con questo identificativo for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) { if ( m_vCalcRotAx[i].nGrpId == nId) { if ( dVal < m_vCalcRotAx[i].stroke.Min || dVal > m_vCalcRotAx[i].stroke.Max) { m_vCalcRotAx[i].bFixed = false ; return false ; } m_vCalcRotAx[i].bFixed = true ; m_vCalcRotAx[i].dFixVal = dVal ; return true ; } } return false ; } //---------------------------------------------------------------------------- bool Machine::FreeKinematicRotAxis( const string& sName) { return FreeKinematicRotAxis( GetAxisId( sName)) ; } //---------------------------------------------------------------------------- bool Machine::FreeKinematicRotAxis( int nId) { // verifico identificativo if ( nId == GDB_ID_NULL) return false ; // cerco l'asse rotante di calcolo con questo identificativo for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) { if ( m_vCalcRotAx[i].nGrpId == nId) { m_vCalcRotAx[i].bFixed = false ; return true ; } } return false ; } //---------------------------------------------------------------------------- bool Machine::IsKinematicRotAxisBlocked( int nInd) const { int nRotAxTot = int( m_vCalcRotAx.size()) ; if ( nInd < 0 || nInd >= nRotAxTot) return false ; return m_vCalcRotAx[nInd].bFixed ; } //---------------------------------------------------------------------------- bool Machine::GetKinematicRotAxisBlocked( int nInd, string& sName, double& dVal) const { int nRotAxTot = int( m_vCalcRotAx.size()) ; if ( nInd < 0 || nInd >= nRotAxTot || ! m_vCalcRotAx[nInd].bFixed) return false ; m_pGeomDB->GetName( m_vCalcRotAx[nInd].nGrpId, sName) ; dVal = m_vCalcRotAx[nInd].dFixVal ; return true ; } //---------------------------------------------------------------------------- bool Machine::SetSolCh( int nScc, bool bExact) { // imposto se richiesta verifica esatta m_bSolChExact = bExact ; // se standard o nullo o suo opposto if ( nScc == MCH_SCC_STD || nScc == MCH_SCC_NONE || nScc == MCH_SCC_OPPOSITE) { // recupero i dati della testa Head* pHead = GetHead( m_nCalcHeadId) ; if ( pHead == nullptr) { LOG_ERROR( GetEMkLogger(), "Missing head in SetSolCh") return false ; } // assegno il criterio definito nella testa (standard) m_nCalcSolCh = pHead->GetSolCh() ; if ( nScc == MCH_SCC_STD || nScc == MCH_SCC_NONE) return true ; // essendo opposto, devo invertirlo switch ( m_nCalcSolCh) { case MCH_SCC_NONE : break ; case MCH_SCC_ADIR_XP : m_nCalcSolCh = MCH_SCC_ADIR_XM ; break ; case MCH_SCC_ADIR_XM : m_nCalcSolCh = MCH_SCC_ADIR_XP ; break ; case MCH_SCC_ADIR_YP : m_nCalcSolCh = MCH_SCC_ADIR_YM ; break ; case MCH_SCC_ADIR_YM : m_nCalcSolCh = MCH_SCC_ADIR_YP ; break ; case MCH_SCC_ADIR_ZP : m_nCalcSolCh = MCH_SCC_ADIR_ZM ; break ; case MCH_SCC_ADIR_ZM : m_nCalcSolCh = MCH_SCC_ADIR_ZP ; break ; case MCH_SCC_ADIR_NEAR : m_nCalcSolCh = MCH_SCC_ADIR_FAR ; break ; case MCH_SCC_ADIR_FAR : m_nCalcSolCh = MCH_SCC_ADIR_NEAR ; break ; default : m_nCalcSolCh = MCH_SCC_NONE ; break ; } return true ; } // altri casi if ( IsValidHeadScc( nScc)) { m_nCalcSolCh = nScc ; return true ; } // non previsto m_nCalcSolCh = MCH_SCC_NONE ; return false ; } //---------------------------------------------------------------------------- bool Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const { // assegno dati Vector3d vtDirH = m_vtCalcDir ; Vector3d vtDirI = m_vtCalcADir ; int nNumRotAx = 0 ; KinAxis RotAx1 ; if ( m_vCalcRotAx.size() >= 1) { ++ nNumRotAx ; RotAx1 = m_vCalcRotAx[0] ; } KinAxis RotAx2 ; if ( m_vCalcRotAx.size() >= 2) { ++ nNumRotAx ; RotAx2 = m_vCalcRotAx[1] ; } // eseguo calcolo return GetMyAngles( vtDirT, vtDirA, vtDirH, vtDirI, nNumRotAx, RotAx1, RotAx2, nStat, dAngA1, dAngB1, dAngA2, dAngB2) ; } //---------------------------------------------------------------------------- bool Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const { // verifiche e aggiustamenti degli assi rotanti Vector3d vtDirTm = vtDirT ; Vector3d vtDirAm = vtDirA ; Vector3d vtDirH = m_vtCalcDir ; Vector3d vtDirI = m_vtCalcADir ; int nNumRotAx = 0 ; KinAxis RotAx[2] ; INTVECTOR vBloHeAx ; for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) { // se asse libero if ( ! m_vCalcRotAx[i].bFixed) { // verifico di non superare il limite if ( nNumRotAx >= 2) return false ; // assegno l'asse RotAx[nNumRotAx] = m_vCalcRotAx[i] ; // se asse di testa, lo aggiorno con precedenti bloccati di testa (applicati in ordine contrario) if ( RotAx[nNumRotAx].bHead) { for ( size_t k = vBloHeAx.size() ; k >= 1 ; -- k) { int nA = vBloHeAx[k-1] ; RotAx[nNumRotAx].ptPos.Rotate( m_vCalcRotAx[nA].ptPos, m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ; RotAx[nNumRotAx].vtDir.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ; } } // incremento contatore ++ nNumRotAx ; } // altrimenti asse bloccato else { // se asse di tavola if ( ! m_vCalcRotAx[i].bHead) { // aggiorno direzioni utensile e ausiliaria richieste vtDirTm.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ; vtDirAm.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ; // aggiorno eventuali assi già inseriti (sicuramente di tavola) for ( size_t j = 0 ; int( j) < nNumRotAx ; ++ j) { RotAx[j].ptPos.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ; RotAx[j].vtDir.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ; } } // altrimenti asse di testa else { // inserisco in lista assi di testa bloccati vBloHeAx.push_back( int( i)) ; } } } // aggiorno direzioni utensile e ausiliaria su testa con eventuali assi bloccati di testa (applico in ordine contrario) for ( size_t k = vBloHeAx.size() ; k >= 1 ; -- k) { int nA = vBloHeAx[k-1] ; vtDirH.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ; vtDirI.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ; } // eseguo calcolo double dAngA1, dAngB1, dAngA2, dAngB2 ; if ( ! GetMyAngles( vtDirTm, vtDirAm, vtDirH, vtDirI, nNumRotAx, RotAx[0], RotAx[1], nStat, dAngA1, dAngB1, dAngA2, dAngB2)) return false ; // assegno gli angoli int nRotAxInd = 1 ; for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++i) { if ( m_vCalcRotAx[i].bFixed) { vAng1.push_back( m_vCalcRotAx[i].dFixVal) ; vAng2.push_back( m_vCalcRotAx[i].dFixVal) ; } else { if ( nRotAxInd == 1) { vAng1.push_back( dAngA1) ; vAng2.push_back( dAngA2) ; } else if ( nRotAxInd == 2) { vAng1.push_back( dAngB1) ; vAng2.push_back( dAngB2) ; } else return false ; ++ nRotAxInd ; } } return true ; } //---------------------------------------------------------------------------- bool Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, const Vector3d& vtDirH, const Vector3d& vtDirI, int nNumRotAx, const KinAxis& RotAx1, const KinAxis& RotAx2, int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const { // annullo tutti gli angoli nStat = 0 ; dAngA1 = 0 ; dAngB1 = 0 ; dAngA2 = 0 ; dAngB2 = 0 ; // direzione fresa normalizzata Vector3d vtDirTn = vtDirT ; if ( ! vtDirTn.Normalize()) return false ; // direzione ausiliaria normalizzata Vector3d vtDirAn = vtDirA ; vtDirAn.Normalize() ; // direzione fresa su testa a riposo Vector3d vtDirHn = vtDirH ; // direzione ausiliaria su testa a riposo Vector3d vtDirIn = vtDirI ; // se nessun asse rotante, devo solo verificare corrispondenza tra direzione richiesta e fresa if ( nNumRotAx == 0) { if ( AreSameVectorApprox( vtDirTn, vtDirHn)) nStat = 1 ; else nStat = 0 ; return true ; } // direzione primo asse rotante Vector3d vtAx1 = RotAx1.vtDir ; // se asse di tavola, ne inverto la direzione if ( ! RotAx1.bHead) vtAx1.Invert() ; // componente versore fresa desiderato su direzione primo asse rotante double dCompTSuAxR1 = vtDirTn * vtAx1 ; // se c'è secondo asse rotante, si calcola angolo per avere il componente appena calcolato bool bDet = true ; Vector3d vtDirH1, vtDirH2 ; Vector3d vtDirI1, vtDirI2 ; if ( nNumRotAx == 2) { // direzione secondo asse rotante Vector3d vtAx2 = RotAx2.vtDir ; // se asse di tavola, ne inverto la direzione if ( ! RotAx2.bHead) vtAx2.Invert() ; // calcolo secondo angolo di rotazione nStat = GetRotationComponent( vtDirHn, dCompTSuAxR1, vtAx1, vtAx2, dAngB1, dAngB2, bDet) ; // se indeterminato con richiesta direzione ausiliaria esatta, ricalcolo con direzione aux if ( nStat >= 1 && ! bDet && m_bSolChExact) { // componente versore ausiliario desiderato su direzione primo asse rotante Vector3d vtSccDir ; if ( GetSccDir( m_nCalcSolCh, vtDirAn, vtSccDir)) { double dCompASuAxR1 = vtSccDir * vtAx1 ; // ricalcolo secondo angolo di rotazione nStat = GetRotationComponent( vtDirIn, dCompASuAxR1, vtAx1, vtAx2, dAngB1, dAngB2, bDet) ; } } // aggiornamento direzioni fresa e ausiliaria su testa if ( nStat >= 1) { // se indeterminato lo azzero if ( ! bDet) dAngB1 = 0 ; // eseguo aggiornamento vtDirH1 = vtDirHn ; vtDirH1.Rotate( vtAx2, dAngB1) ; vtDirI1 = vtDirIn ; vtDirI1.Rotate( vtAx2, dAngB1) ; } if ( nStat == 2) { vtDirH2 = vtDirHn ; vtDirH2.Rotate( vtAx2, dAngB2) ; vtDirI2 = vtDirIn ; vtDirI2.Rotate( vtAx2, dAngB2) ; } } // altrimenti verifico se compatibili else { // componente versore utensile su direzione primo asse double dCompHSuAxR1 = vtDirHn * vtAx1 ; // componenti versori fresa e utensile perpendicolari direzione primo asse double dTemp = 1 - dCompTSuAxR1 * dCompTSuAxR1 ; double dCompTOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ; dTemp = 1 - dCompHSuAxR1 * dCompHSuAxR1 ; double dCompHOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ; // verifica ( controllo verso e max delta angolare < 0.002 deg) if ( abs( dCompTSuAxR1 - dCompHSuAxR1) < EPS_SMALL && abs( dCompTOrtAxR1 * dCompHSuAxR1 - dCompHOrtAxR1 * dCompTSuAxR1) < 2 * SIN_EPS_ANG_SMALL) { nStat = 1 ; vtDirH1 = vtDirHn ; vtDirI1 = vtDirIn ; // reset secondo angolo dAngB1 = 0 ; } } // calcolo primo angolo di rotazione per seconda soluzione bool bDet2 = true ; if ( nStat == 2) { if ( ! vtDirH2.GetRotation( vtDirTn, vtAx1, dAngA2, bDet2) ) nStat = 1 ; else { // se indeterminato, provo a determinarlo con la direzione ausiliaria if ( ! bDet2) { bool bDetX ; Vector3d vtSccDir ; bool bSccDir = GetSccDir( m_nCalcSolCh, vtDirAn, vtSccDir) ; vtDirI2.GetRotation( vtSccDir, vtAx1, dAngA2, bDetX) ; if ( bDetX) bDet2 = bSccDir ; else dAngA2 = 0 ; } } // aggiornamento direzioni fresa e ausiliaria su testa vtDirH2.Rotate( vtAx1, dAngA2) ; vtDirI2.Rotate( vtAx1, dAngA2) ; } // calcolo primo angolo di rotazione per prima soluzione bool bDet1 = true ; if ( nStat >= 1) { if ( ! vtDirH1.GetRotation( vtDirTn, vtAx1, dAngA1, bDet1) ) nStat = 0 ; else { // se indeterminato, provo a determinarlo con la direzione ausiliaria if ( ! bDet1) { bool bDetX ; Vector3d vtSccDir ; bool bSccDir = GetSccDir( m_nCalcSolCh, vtDirAn, vtSccDir) ; vtDirI1.GetRotation( vtSccDir, vtAx1, dAngA1, bDetX) ; if ( bDetX) bDet1 = bSccDir ; else dAngA1 = 0 ; } } // aggiornamento direzioni fresa e ausiliaria su testa vtDirH1.Rotate( vtAx1, dAngA1) ; vtDirI1.Rotate( vtAx1, dAngA1) ; } // verifiche per criterio scelta soluzione if ( nStat >= 2) { if ( ! VerifyScc( vtDirI2, vtDirAn, m_nCalcSolCh, m_bSolChExact)) -- nStat ; } if ( nStat >= 1) { if ( ! VerifyScc( vtDirI1, vtDirAn, m_nCalcSolCh, m_bSolChExact)) { -- nStat ; // riloco eventuale soluzione rimasta if ( nStat >= 1) { dAngA1 = dAngA2 ; dAngB1 = dAngB2 ; } } } // verifiche dei limiti di corsa if ( nStat >= 2) { // se non riesco ad aggiustare, elimino if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA2) || ( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, dAngB2))) -- nStat ; } if ( nStat >= 1) { // se non riesco ad aggiustare, elimino if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA1) || ( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, dAngB1))) { -- nStat ; // riloco eventuale soluzione rimasta if ( nStat >= 1) { dAngA1 = dAngA2 ; dAngB1 = dAngB2 ; } } } // modifico stato per angolo indeterminato if ( ( nStat >= 2 && ! bDet2) || ( nStat >= 1 && ! bDet1)) nStat = - nStat ; return true ; } //---------------------------------------------------------------------------- bool Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng, int& nStat, double& dX, double& dY, double& dZ) const { // la posizione deve essere espressa rispetto allo ZERO MACCHINA // il punto è dato rispetto alla posizione home della tavola // verifico che siano stati assegnati gli angoli necessari, altrimenti errore if ( vAng.size() < m_vCalcRotAx.size()) return false ; // aggiorno punto di lavoro mediante ciclo diretto sugli assi di tavola Point3d ptW = ptP ; // annullo la posizione home degli assi lineari 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 ( 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 ( 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]) ; } } // 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 i valori degli assi lineari (posizioni) Point3d ptPos( ptW.x + vtDtHe.x + vtDtAx.x + vtDtTL.x, ptW.y + vtDtHe.y + vtDtAx.y + vtDtTL.y, ptW.z + vtDtHe.z + vtDtAx.z + vtDtTL.z) ; ptPos.ToLoc( m_frLinAx) ; dX = ptPos.x ; dY = ptPos.y ; dZ = ptPos.z ; // tutto ok nStat = 0 ; return true ; } //---------------------------------------------------------------------------- bool Machine::GetDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& vtNew) const { // è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa) // verifico dimensione vettore angoli rispetto al numero di assi rotanti if ( vAng.size() < m_vCalcRotAx.size()) return false ; // direzione a riposo vtNew = vtDir ; // ciclo sugli assi di testa dall'ultimo (prima assi di tavola, poi di testa) for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) { // se asse di testa if ( m_vCalcRotAx[i-1].bHead) vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ; } return true ; } //---------------------------------------------------------------------------- bool Machine::GetBackDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& vtNew) const { // è espressa nel riferimento del pezzo (tiene conto delle rotazioni di testa e al contrario di quelle di tavola) // verifico dimensione vettore angoli rispetto al numero di assi rotanti if ( vAng.size() < m_vCalcRotAx.size()) return false ; // direzione a riposo vtNew = vtDir ; // ciclo sugli assi dall'ultimo (prima assi di tavola, poi di testa) for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) { // se asse di testa if ( m_vCalcRotAx[i-1].bHead) vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ; // altrimenti di tavola else vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, - vAng[i-1]) ; } return true ; } //---------------------------------------------------------------------------- bool Machine::GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, Point3d& ptNose) const { // la posizione deve essere espressa rispetto allo ZERO MACCHINA // è espressa nel riferimento di macchina (tiene conto delle sole traslazioni e rotazioni di testa) // aggiorno posizione testa a riposo mediante ciclo inverso sugli assi rotanti di testa ptNose = m_ptCalcPos ; 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 ( 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 ; } //---------------------------------------------------------------------------- bool Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, bool bOverall, bool bBottom, Point3d& ptTip) const { // la posizione deve essere espressa rispetto allo ZERO MACCHINA // è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa) // 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) { // calcolo la direzione fresa Vector3d vtDirT ; if ( ! GetDirection( m_vtCalcDir, vAng, vtDirT)) return false ; // se richiesto ingombro totale if ( bOverall) ptTip -= vtDirT * max( m_dCalcTOvLen - m_dCalcTLen, 0.) ; // se richiesto punto più basso e direzione fresa non esattamente verticale if ( bBottom && ! vtDirT.IsZplus() && ! vtDirT.IsZminus()) { // calcolo la direzione perpendicolare più verticale possibile Vector3d vtCorr = FromUprightOrtho( vtDirT) ; // correggo il tip if ( bOverall) ptTip -= vtCorr * max( m_dCalcTOvRad, m_dCalcTRad) ; else ptTip -= vtCorr * m_dCalcTRad ; } } return true ; } //---------------------------------------------------------------------------- bool Machine::GetToolDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const { return GetDirection( m_vtCalcDir, vAng, vtDir) ; } //---------------------------------------------------------------------------- bool Machine::GetAuxDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const { return GetDirection( m_vtCalcADir, vAng, vtDir) ; } //---------------------------------------------------------------------------- bool Machine::GetPartDirFromAngles( const Vector3d& vtPart, const DBLVECTOR& vAng, Vector3d& vtDir) const { // è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di tavola e asse rotante speciale di testa) // verifico dimensione vettore angoli rispetto al numero di assi rotanti if ( vAng.size() < m_vCalcRotAx.size()) return false ; // direzione a riposo vtDir = vtPart ; // ciclo sugli assi dal primo (solo assi di tavola) for ( size_t i = 1 ; i <= m_vCalcRotAx.size() ; ++ i) { // se asse di tavola if ( ! m_vCalcRotAx[i-1].bHead) vtDir.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ; } // eseguo rotazione inversa 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 ; vtDir.Rotate( m_vCalcRotAx[i].vtDir, -vAng[i]) ; } return true ; } //---------------------------------------------------------------------------- bool Machine::GetBackToolDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const { return GetBackDirection( m_vtCalcDir, vAng, vtDir) ; } //---------------------------------------------------------------------------- bool Machine::GetBackAuxDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const { return GetBackDirection( m_vtCalcADir, vAng, vtDir) ; } //---------------------------------------------------------------------------- bool Machine::GetSccDir( int nSolCh, const Vector3d& vtDirA, Vector3d& vtDirScc) const { switch ( nSolCh) { case MCH_SCC_ADIR_XP : vtDirScc = X_AX ; return true ; case MCH_SCC_ADIR_XM : vtDirScc = - X_AX ; return true ; case MCH_SCC_ADIR_YP : vtDirScc = Y_AX ; return true ; case MCH_SCC_ADIR_YM : vtDirScc = - Y_AX ; return true ; case MCH_SCC_ADIR_ZP : vtDirScc = Z_AX ; return true ; case MCH_SCC_ADIR_ZM : vtDirScc = - Z_AX ; return true ; case MCH_SCC_ADIR_NEAR : vtDirScc = vtDirA ; return true ; case MCH_SCC_ADIR_FAR : vtDirScc = - vtDirA ; return true ; } vtDirScc = vtDirA ; return false ; } //---------------------------------------------------------------------------- bool Machine::VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh, bool bExact) const { switch ( nSolCh) { default : return true ; case MCH_SCC_ADIR_XP : if ( bExact) return vtDirI.IsXplus() ; else return ( vtDirI.x > - EPS_ZERO) ; case MCH_SCC_ADIR_XM : if ( bExact) return vtDirI.IsXminus() ; else return ( vtDirI.x < EPS_ZERO) ; case MCH_SCC_ADIR_YP : if ( bExact) return vtDirI.IsYplus() ; else return ( vtDirI.y > - EPS_ZERO) ; case MCH_SCC_ADIR_YM : if ( bExact) return vtDirI.IsYminus() ; else return ( vtDirI.y < EPS_ZERO) ; case MCH_SCC_ADIR_ZP : if ( bExact) return vtDirI.IsZplus() ; else return ( vtDirI.z > - EPS_ZERO) ; case MCH_SCC_ADIR_ZM : if ( bExact) return vtDirI.IsZminus() ; else return ( vtDirI.z < EPS_ZERO) ; case MCH_SCC_ADIR_NEAR : if ( bExact) return ( ! vtDirA.IsSmall() && ( vtDirI * vtDirA) > cos( 0.1 * DEGTORAD)) ; else return ( vtDirA.IsSmall() || ( vtDirI * vtDirA) > cos( 60 * DEGTORAD)) ; case MCH_SCC_ADIR_FAR : if ( bExact) return ( ! vtDirA.IsSmall() && ( vtDirI * vtDirA) < cos( 179.1 * DEGTORAD)) ; else return ( vtDirA.IsSmall() || ( vtDirI * vtDirA) < cos( 120 * DEGTORAD)) ; } } //---------------------------------------------------------------------------- bool Machine::AdjustAngleInStroke( const STROKE& Stroke, double& dAng) const { // se asse rotante vero if ( Stroke.Max - Stroke.Min > EPS_ANG_SMALL) { // eseguo gli aggiustamenti while ( dAng < Stroke.Min) dAng += ANG_FULL ; while ( dAng > Stroke.Max) dAng -= ANG_FULL ; // verifico return ( dAng >= Stroke.Min && dAng <= Stroke.Max) ; } // altrimenti asse rotante senza corsa else { // angolo di riferimento double dStrokeMid = ( Stroke.Min + Stroke.Max) / 2 ; // eseguo gli aggiustamenti while ( dAng < dStrokeMid - EPS_ANG_SMALL) dAng += ANG_FULL ; while ( dAng > dStrokeMid + EPS_ANG_SMALL) dAng -= ANG_FULL ; // verifico if ( abs( dAng - dStrokeMid) < EPS_ANG_SMALL) { dAng = dStrokeMid ; return true ; } else return false ; } } //---------------------------------------------------------------------------- bool Machine::GetNearestAngleInStroke( int nInd, double dAngRef, double& dAng) const { // se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size())) return true ; // cerco l'angolo più vicino stando nella corsa while ( dAng - dAngRef > ANG_STRAIGHT + EPS_ANG_ZERO && dAng - ANG_FULL >= m_vCalcRotAx[nInd].stroke.Min) dAng -= ANG_FULL ; while ( dAng - dAngRef < -ANG_STRAIGHT + EPS_ANG_ZERO && dAng + ANG_FULL <= m_vCalcRotAx[nInd].stroke.Max) dAng += ANG_FULL ; return ( dAng >= m_vCalcRotAx[nInd].stroke.Min && dAng <= m_vCalcRotAx[nInd].stroke.Max) ; } //---------------------------------------------------------------------------- bool Machine::LimitAngleToStroke( int nInd, double& dAng) const { // se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size())) return true ; // se angolo fuori corsa, lo porto all'estremo più vicino if ( dAng < m_vCalcRotAx[nInd].stroke.Min) dAng = m_vCalcRotAx[nInd].stroke.Min ; else if ( dAng > m_vCalcRotAx[nInd].stroke.Max) dAng = m_vCalcRotAx[nInd].stroke.Max ; return true ; } //---------------------------------------------------------------------------- bool Machine::VerifyAngleOutstroke( int nInd, double dAng) const { // se angolo fittizio (non esiste l'asse rotante corrispondente), va bene if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size())) return true ; // se angolo fuori corsa, errore if ( dAng < m_vCalcRotAx[nInd].stroke.Min) return false ; else if ( dAng > m_vCalcRotAx[nInd].stroke.Max) return false ; // va bene return true ; } //---------------------------------------------------------------------------- bool Machine::VerifyOutstroke( double dX, double dY, double dZ, const DBLVECTOR& vAng, bool bClear, int& nStat) const { // default tutto ok nStat = 0 ; if ( bClear) m_OutstrokeInfo.Clear() ; // verifica degli assi lineari DBLVECTOR vLin( 3) ; vLin[0] = dX ; vLin[1] = dY ; vLin[2] = dZ ; for ( size_t i = 0 ; i < m_vCalcLinAx.size() && i < vLin.size() ; ++ i) { if ( vLin[i] < m_vCalcLinAx[i].stroke.Min) { nStat += ( 1 << ( 2 * i)) ; m_OutstrokeInfo.sAxName = GetAxis( m_vCalcLinAx[i].nGrpId)->GetName() ; m_OutstrokeInfo.sAxToken = GetAxis( m_vCalcLinAx[i].nGrpId)->GetToken() ; m_OutstrokeInfo.bLinear = true ; m_OutstrokeInfo.dExtra = vLin[i] - m_vCalcLinAx[i].stroke.Min ; m_OutstrokeInfo.sAuxInfo = " (L" + ToString( int(i) + 1) + "-) " ; } else if( vLin[i] > m_vCalcLinAx[i].stroke.Max) { nStat += ( 2 << ( 2 * i)) ; m_OutstrokeInfo.sAxName = GetAxis( m_vCalcLinAx[i].nGrpId)->GetName() ; m_OutstrokeInfo.sAxToken = GetAxis( m_vCalcLinAx[i].nGrpId)->GetToken() ; m_OutstrokeInfo.bLinear = true ; m_OutstrokeInfo.dExtra = vLin[i] - m_vCalcLinAx[i].stroke.Max ; m_OutstrokeInfo.sAuxInfo = " (L" + ToString( int(i) + 1) + "+) " ; } } // verifica degli assi rotanti for ( size_t i = 0 ; i < m_vCalcRotAx.size() && i < vAng.size() ; ++ i) { if ( vAng[i] < m_vCalcRotAx[i].stroke.Min) { nStat += ( 64 << ( 2 * i)) ; m_OutstrokeInfo.sAxName = GetAxis( m_vCalcRotAx[i].nGrpId)->GetName() ; m_OutstrokeInfo.sAxToken = GetAxis( m_vCalcRotAx[i].nGrpId)->GetToken() ; m_OutstrokeInfo.bLinear = false ; m_OutstrokeInfo.dExtra = vAng[i] - m_vCalcRotAx[i].stroke.Min ; m_OutstrokeInfo.sAuxInfo = " (R" + ToString( int(i) + 1) + "-) " ; } else if( vAng[i] > m_vCalcRotAx[i].stroke.Max) { nStat += ( 128 << ( 2 * i)) ; m_OutstrokeInfo.sAxName = GetAxis( m_vCalcRotAx[i].nGrpId)->GetName() ; m_OutstrokeInfo.sAxToken = GetAxis( m_vCalcRotAx[i].nGrpId)->GetToken() ; m_OutstrokeInfo.bLinear = false ; m_OutstrokeInfo.dExtra = vAng[i] - m_vCalcRotAx[i].stroke.Max ; m_OutstrokeInfo.sAuxInfo = " (R" + ToString( int(i) + 1) + "+) " ; } } // verifica delle aree protette if ( nStat == 0) return const_cast( this)->VerifyProtectedAreas( dX, dY, dZ, vAng, nStat) ; return true ; } //---------------------------------------------------------------------------- bool Machine::ExistProtectedAreas(void) const { return LuaExistsFunction( ON_VERIFY_PROTECTEDAREAS) ; } //---------------------------------------------------------------------------- bool Machine::VerifyProtectedAreas( double dX, double dY, double dZ, const DBLVECTOR& vAng, int& nStat) { // se non esiste funzione gestione aree protette, non devo fare alcunchè if ( ! LuaExistsFunction( ON_VERIFY_PROTECTEDAREAS)) return true ; // default bool bOk = true ; int nErr = 99 ; // salvo eventuale variabile EMC_VAR già presente bool bOldEMC = LuaChangeNameGlobVar( EMC_VAR, EMC_VAR_BACKUP) ; // definisco variabili bOk = bOk && LuaCreateGlobTable( EMC_VAR) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_L1, dX) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_L2, dY) ; bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_L3, dZ) ; if ( vAng.size() >= 1) bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_R1, vAng[0]) ; if ( vAng.size() >= 2) bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_R2, vAng[1]) ; if ( vAng.size() >= 3) bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_R3, vAng[2]) ; if ( vAng.size() >= 4) bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_R4, vAng[3]) ; // chiamo funzione bOk = bOk && LuaCallFunction( ON_VERIFY_PROTECTEDAREAS) ; // recupero il risultato bOk = bOk && LuaGetGlobVar( EMC_VAR + EVAR_ERROR, nErr) ; if ( nErr != 0) { bOk = false ; string sOut = " Error in " + ON_VERIFY_PROTECTEDAREAS + " (" + ToString( nErr) + ")" ; LOG_INFO( GetEMkLogger(), sOut.c_str()) } int nMyStat = 0 ; bOk = bOk && LuaGetGlobVar( EMC_VAR + EVAR_STAT, nMyStat) ; if ( nMyStat != 0) { nStat += ( nMyStat << 14) ; m_OutstrokeInfo.sAxName = AXIS_NAME_PROTECTEDAREAS ; m_OutstrokeInfo.sAxToken = "" ; m_OutstrokeInfo.bLinear = false ; m_OutstrokeInfo.dExtra = 0 ; bOk = bOk && LuaGetGlobVar( EMC_VAR + EVAR_AUXINFO, m_OutstrokeInfo.sAuxInfo) ; } // reset variabili bOk = LuaResetGlobVar( EMC_VAR) && bOk ; // ripristino eventuale variabile EMC_VAR già presente if ( bOldEMC) LuaChangeNameGlobVar( EMC_VAR_BACKUP, EMC_VAR) ; // esco return bOk ; } //---------------------------------------------------------------------------- bool Machine::VerifyOutstroke( const string& sAxName, double dVal) const { // recupero l'asse Axis* pAx = GetAxis( GetAxisId( sAxName)) ; if ( pAx == nullptr) { m_OutstrokeInfo.sAxName = sAxName ; m_OutstrokeInfo.sAxToken = sAxName ; m_OutstrokeInfo.bLinear = false ; m_OutstrokeInfo.dExtra = 0 ; m_OutstrokeInfo.sAuxInfo = " (Unknown) " ; return false ; } // verifica con l'estremo inferiore if ( dVal < pAx->GetStroke().Min) { m_OutstrokeInfo.sAxName = pAx->GetName() ; m_OutstrokeInfo.sAxToken = pAx->GetToken() ; m_OutstrokeInfo.bLinear = ( pAx->GetType() == MCH_AT_LINEAR) ; m_OutstrokeInfo.dExtra = dVal - pAx->GetStroke().Min ; m_OutstrokeInfo.sAuxInfo = " (Ax-) " ; return false ; } // verifica con l'estremo superiore if ( dVal > pAx->GetStroke().Max) { m_OutstrokeInfo.sAxName = pAx->GetName() ; m_OutstrokeInfo.sAxToken = pAx->GetToken() ; m_OutstrokeInfo.bLinear = ( pAx->GetType() == MCH_AT_LINEAR) ; m_OutstrokeInfo.dExtra = dVal - pAx->GetStroke().Max ; m_OutstrokeInfo.sAuxInfo = " (Ax+) " ; return false ; } // va bene return true ; } //---------------------------------------------------------------------------- string Machine::GetOutstrokeInfo( bool bMM) const { // se non c'è extracorsa, ritorno stringa vuota if ( m_OutstrokeInfo.sAxName.empty()) return "" ; // creo stringa con info opportune if ( m_OutstrokeInfo.sAxName == AXIS_NAME_PROTECTEDAREAS) return m_OutstrokeInfo.sAuxInfo ; else if ( bMM || ! m_OutstrokeInfo.bLinear) return m_OutstrokeInfo.sAxToken + "=" + ToString( m_OutstrokeInfo.dExtra, 1) + m_OutstrokeInfo.sAuxInfo ; else return m_OutstrokeInfo.sAxToken + "=" + ToString( m_OutstrokeInfo.dExtra / ONEINCH, 2) + m_OutstrokeInfo.sAuxInfo ; } //---------------------------------------------------------------------------- int Machine::GetCurrLinAxes( void) const { return int( m_vCalcLinAx.size()) ; } //---------------------------------------------------------------------------- int Machine::GetCurrRotAxes( void) const { return int( m_vCalcRotAx.size()) ; } //---------------------------------------------------------------------------- bool Machine::GetCurrAxisName( int nInd, string& sAxName) const { int nLinAxes = int( m_vCalcLinAx.size()) ; int nRotAxes = int( m_vCalcRotAx.size()) ; if ( nInd >= 0 && nInd < nLinAxes) { Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ; if ( pAx == nullptr) return false ; sAxName = pAx->GetName() ; return true ; } else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) { Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ; if ( pAx == nullptr) return false ; sAxName = pAx->GetName() ; return true ; } return false ; } //---------------------------------------------------------------------------- bool Machine::GetAllCurrAxesName( STRVECTOR& vAxName) const { vAxName.clear() ; bool bOk = true ; // ciclo sugli assi lineari correnti for ( auto& CalcLinAx : m_vCalcLinAx) { Axis* pAx = GetAxis( CalcLinAx.nGrpId) ; if ( pAx != nullptr) vAxName.emplace_back( pAx->GetName()) ; else bOk = false ; } // ciclo sugli assi rotanti correnti for ( auto& CalcRotAx : m_vCalcRotAx) { Axis* pAx = GetAxis( CalcRotAx.nGrpId) ; if ( pAx != nullptr) vAxName.emplace_back( pAx->GetName()) ; else bOk = false ; } return bOk ; } //---------------------------------------------------------------------------- bool Machine::GetCurrAxisToken( int nInd, string& sAxToken) const { int nLinAxes = int( m_vCalcLinAx.size()) ; int nRotAxes = int( m_vCalcRotAx.size()) ; if ( nInd >= 0 && nInd < nLinAxes) { Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ; if ( pAx == nullptr) return false ; sAxToken = pAx->GetToken() ; return true ; } else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) { Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ; if ( pAx == nullptr) return false ; sAxToken = pAx->GetToken() ; return true ; } return false ; } //---------------------------------------------------------------------------- bool Machine::GetAllCurrAxesToken( STRVECTOR& vAxToken) const { vAxToken.clear() ; bool bOk = true ; // ciclo sugli assi lineari correnti for ( auto& CalcLinAx : m_vCalcLinAx) { Axis* pAx = GetAxis( CalcLinAx.nGrpId) ; if ( pAx != nullptr) vAxToken.emplace_back( pAx->GetToken()) ; else bOk = false ; } // ciclo sugli assi rotanti correnti for ( auto& CalcRotAx : m_vCalcRotAx) { Axis* pAx = GetAxis( CalcRotAx.nGrpId) ; if ( pAx != nullptr) vAxToken.emplace_back( pAx->GetToken()) ; else bOk = false ; } return bOk ; } //---------------------------------------------------------------------------- bool Machine::GetCurrAxisMin( int nInd, double& dMin) const { int nLinAxes = int( m_vCalcLinAx.size()) ; int nRotAxes = int( m_vCalcRotAx.size()) ; if ( nInd >= 0 && nInd < nLinAxes) { Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ; if ( pAx == nullptr) return false ; dMin = pAx->GetStroke().Min ; return true ; } else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) { Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ; if ( pAx == nullptr) return false ; dMin = pAx->GetStroke().Min ; return true ; } return false ; } //---------------------------------------------------------------------------- bool Machine::GetCurrAxisMax( int nInd, double& dMax) const { int nLinAxes = int( m_vCalcLinAx.size()) ; int nRotAxes = int( m_vCalcRotAx.size()) ; if ( nInd >= 0 && nInd < nLinAxes) { Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ; if ( pAx == nullptr) return false ; dMax = pAx->GetStroke().Max ; return true ; } else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) { Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ; if ( pAx == nullptr) return false ; dMax = pAx->GetStroke().Max ; return true ; } return false ; } //---------------------------------------------------------------------------- bool Machine::GetCurrAxisHomePos( int nInd, double& dHome) const { int nLinAxes = int( m_vCalcLinAx.size()) ; int nRotAxes = int( m_vCalcRotAx.size()) ; if ( nInd >= 0 && nInd < nLinAxes) { Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ; if ( pAx == nullptr) return false ; dHome = pAx->GetHomeVal() ; return true ; } else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) { Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ; if ( pAx == nullptr) return false ; dHome = pAx->GetHomeVal() ; return true ; } return false ; } //---------------------------------------------------------------------------- bool Machine::GetAllCurrAxesHomePos( DBLVECTOR& vAxHomeVal) const { vAxHomeVal.clear() ; bool bOk = true ; // ciclo sugi assi lineari correnti for ( auto& CalcLinAx : m_vCalcLinAx) { Axis* pAx = GetAxis( CalcLinAx.nGrpId) ; if ( pAx != nullptr) vAxHomeVal.emplace_back( pAx->GetHomeVal()) ; else bOk = false ; } // ciclo sugi assi rotanti correnti for ( auto& CalcRotAx : m_vCalcRotAx) { Axis* pAx = GetAxis( CalcRotAx.nGrpId) ; if ( pAx != nullptr) vAxHomeVal.emplace_back( pAx->GetHomeVal()) ; else bOk = false ; } return bOk ; }