diff --git a/Disposition.cpp b/Disposition.cpp index a27e4e6..776fd19 100644 --- a/Disposition.cpp +++ b/Disposition.cpp @@ -343,7 +343,18 @@ Disposition::Apply(void) FixData.nId = nId ; } } - // aggiornamento movimento pezzi + // annullo movimento grezzi + int nRawId = m_pMchMgr->GetFirstRawPart() ; + while ( nRawId != GDB_ID_NULL) { + if ( m_pMchMgr->VerifyRawPartPhase( nRawId, m_nPhase)) { + // recupero riferimento del grezzo + Frame3d* pfrRaw = m_pGeomDB->GetGroupFrame( nRawId) ; + // lo trasformo nel riferimento globale + pfrRaw->Reset() ; + } + nRawId = m_pMchMgr->GetNextRawPart( nRawId) ; + } + // aggiornamento movimento grezzi for ( const auto& vMvrData : m_vMvrData) { switch ( vMvrData.nType) { case MoveRawData::COR : diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index 37a67d7..c8796f9 100644 Binary files a/EgtMachKernel.rc and b/EgtMachKernel.rc differ diff --git a/Generator.cpp b/Generator.cpp index 263b1dd..9fe87df 100644 --- a/Generator.cpp +++ b/Generator.cpp @@ -29,7 +29,7 @@ using namespace std ; //---------------------------------------------------------------------------- static const string ERR_EXT = ".err" ; -static const int MAX_AXES = 5 ; +static const int MAX_AXES = 7 ; //---------------------------------------------------------------------------- Generator::Generator( void) @@ -708,6 +708,8 @@ Generator::GetGlobVarAxisValue( int nAx) case 3 : return ( GLOB_VAR + GVAR_L3) ; case 4 : return ( GLOB_VAR + GVAR_R1) ; case 5 : return ( GLOB_VAR + GVAR_R2) ; + case 6 : return ( GLOB_VAR + GVAR_R3) ; + case 7 : return ( GLOB_VAR + GVAR_R4) ; default : return "" ; } } @@ -722,6 +724,8 @@ Generator::GetGlobVarAxisPrev( int nAx) case 3 : return ( GLOB_VAR + GVAR_L3P) ; case 4 : return ( GLOB_VAR + GVAR_R1P) ; case 5 : return ( GLOB_VAR + GVAR_R2P) ; + case 6 : return ( GLOB_VAR + GVAR_R3P) ; + case 7 : return ( GLOB_VAR + GVAR_R4P) ; default : return "" ; } } @@ -736,6 +740,8 @@ Generator::GetGlobVarAxisToken( int nAx) case 3 : return ( GLOB_VAR + GVAR_L3T) ; case 4 : return ( GLOB_VAR + GVAR_R1T) ; case 5 : return ( GLOB_VAR + GVAR_R2T) ; + case 6 : return ( GLOB_VAR + GVAR_R3T) ; + case 7 : return ( GLOB_VAR + GVAR_R4T) ; default : return "" ; } } diff --git a/GeoCalc.cpp b/GeoCalc.cpp index 3237097..bfb6a12 100644 --- a/GeoCalc.cpp +++ b/GeoCalc.cpp @@ -32,8 +32,10 @@ GetRotationComponent( const Vector3d& vtDir1, double dComp, const Vector3d& vtDi // se Z e W sono allineati (o quasi) Vector3d vtU = vtPvZW ; if ( ! vtU.Normalize()) { + // determino se equiversi o controversi + bool bEquiv = ( vtDir2 * vtRotAx) > 0 ; // se le componenti concordano, angolo indeterminato - if ( fabs( dT0w - dComp) < 0.5 * SIN_EPS_ANG_SMALL) { + if ( fabs( dT0w - ( bEquiv ? dComp : - dComp)) < 0.5 * SIN_EPS_ANG_SMALL) { bDet = false ; return 1 ; } diff --git a/MachMgr.h b/MachMgr.h index 62f3233..6a998ab 100644 --- a/MachMgr.h +++ b/MachMgr.h @@ -74,7 +74,7 @@ class MachMgr : public IMachMgr int GetCurrMachGroup( void) const override ; // Phases int AddPhase( void) override ; - bool SetCurrPhase( int nPhase) override ; + bool SetCurrPhase( int nPhase, bool bForced = false) override ; int GetCurrPhase( void) const override ; bool RemoveLastPhase( void) override ; int GetPhaseCount( void) const override ; @@ -222,8 +222,14 @@ class MachMgr : public IMachMgr bool GetCalcTool( std::string& sTool) override ; bool GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) override ; + bool BlockKinematicRotAxis( const std::string& sAxis, double dVal) ; + bool FreeKinematicRotAxis( const std::string& sAxis) ; + bool GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, + int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) ; bool GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB, int& nStat, double& dX, double& dY, double& dZ) override ; + bool GetCalcPositions( const Point3d& ptP, DBLVECTOR& vAng, + int& nStat, double& dX, double& dY, double& dZ) ; bool GetCalcTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB, bool bBottom, Point3d& ptTip) override ; bool GetCalcToolDirFromAngles( double dAngA, double dAngB, Vector3d& vtDir) override ; @@ -273,6 +279,7 @@ class MachMgr : public IMachMgr int GetCurrOperId( void) const { return ( ExistsCurrMachGroup() ? m_cCurrMGrp.OperGroupId : GDB_ID_NULL) ; } // RawParts + bool GetRawPartPhases( int nRawId, INTVECTOR& vPhase) const ; bool GetRawPartCenter( int nRawId, Point3d& ptCen) ; // Machines Machine* GetCurrMachine( void) const ; @@ -284,6 +291,7 @@ class MachMgr : public IMachMgr bool GetAllCurrAxesName( STRVECTOR& vAxName) ; bool GetAllCalcAxesHomePos( DBLVECTOR& vAxHomeVal) ; double GetCalcRot1W( void) ; + bool IsKinematicRotAxisBlocked( int nInd) ; // operations bool GetOperationNewName( std::string& sName) const ; const ToolData* GetMachiningToolData( void) const ; @@ -308,7 +316,7 @@ class MachMgr : public IMachMgr int GetMachine( const std::string& sMachineName) const ; int GetCurrMachineId( void) const ; // Phases - bool PrepareCurrPhase( int nPhase, bool bDoDisp) ; + bool PrepareCurrPhase( int nPhase, bool bDoDisp, bool bForced) ; int CalcPhaseCount( void) const ; // RawParts int AddRawPart( int nCrvId, double dOverMat, double dZmin, double dHeight, Color cCol) ; diff --git a/MachMgrBasic.cpp b/MachMgrBasic.cpp index 19a613b..e86cdca 100644 --- a/MachMgrBasic.cpp +++ b/MachMgrBasic.cpp @@ -64,6 +64,11 @@ MachMgr::~MachMgr( void) void MachMgr::Clear( void) { + // cancello simulatore, se necessario + if ( m_pSimul != nullptr) { + delete m_pSimul ; + m_pSimul = nullptr ; + } // pulisco le macchine while ( ! m_vMachines.empty()) { if ( m_vMachines.back().pMsMgr != nullptr) @@ -74,11 +79,6 @@ MachMgr::Clear( void) delete m_vMachines.back().pMachine ; m_vMachines.pop_back() ; } - // cancello simulatore, se necessario - if ( m_pSimul != nullptr) { - delete m_pSimul ; - m_pSimul = nullptr ; - } } //---------------------------------------------------------------------------- diff --git a/MachMgrMachGroups.cpp b/MachMgrMachGroups.cpp index 12eb1c7..6cedcc8 100644 --- a/MachMgrMachGroups.cpp +++ b/MachMgrMachGroups.cpp @@ -303,6 +303,8 @@ MachMgr::PrepareCurrMachGroup( int nId) return false ; // se esiste gruppo corrente, lo disabilito if ( m_nCurrMGrpId != GDB_ID_NULL) { + // termino eventuale simulazione attiva + SimStop() ; // riporto i pezzi nei grezzi precedenti nella loro posizione standard SwapParts( false) ; // nascondo precedente gruppo corrente e la relativa macchina @@ -334,6 +336,8 @@ MachMgr::ResetCurrMachGroup( void) // se non c'è gruppo corrente, posso uscire subito if ( m_nCurrMGrpId == GDB_ID_NULL) return true ; + // termino eventuale simulazione attiva + SimStop() ; // riporto i pezzi nei grezzi nella loro posizione standard SwapParts( false) ; // visualizzo pezzi rimasti sotto la radice diff --git a/MachMgrMachines.cpp b/MachMgrMachines.cpp index 1f04ae1..dff6b78 100644 --- a/MachMgrMachines.cpp +++ b/MachMgrMachines.cpp @@ -302,6 +302,39 @@ MachMgr::GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, return ( ( pMch != nullptr) ? pMch->GetAngles( vtDirT, vtDirA, nStat, dAngA1, dAngB1, dAngA2, dAngB2) : false) ; } +//---------------------------------------------------------------------------- +bool +MachMgr::BlockKinematicRotAxis( const string& sAxis, double dVal) +{ + Machine* pMch = GetCurrMachine() ; + return ( ( pMch != nullptr) ? pMch->BlockKinematicRotAxis( sAxis, dVal) : false) ; +} + +//---------------------------------------------------------------------------- +bool +MachMgr::FreeKinematicRotAxis( const string& sAxis) +{ + Machine* pMch = GetCurrMachine() ; + return ( ( pMch != nullptr) ? pMch->FreeKinematicRotAxis( sAxis) : false) ; +} + +//---------------------------------------------------------------------------- +bool +MachMgr::IsKinematicRotAxisBlocked( int nInd) +{ + Machine* pMch = GetCurrMachine() ; + return ( ( pMch != nullptr) ? pMch->IsKinematicRotAxisBlocked( nInd) : false) ; +} + +//---------------------------------------------------------------------------- +bool +MachMgr::GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, + int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) +{ + Machine* pMch = GetCurrMachine() ; + return ( ( pMch != nullptr) ? pMch->GetAngles( vtDirT, vtDirA, nStat, vAng1, vAng2) : false) ; +} + //---------------------------------------------------------------------------- bool MachMgr::GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB, @@ -311,6 +344,16 @@ MachMgr::GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB, return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, dAngA, dAngB, nStat, dX, dY, dZ) : false) ; } +//---------------------------------------------------------------------------- +bool +MachMgr::GetCalcPositions( const Point3d& ptP, DBLVECTOR& vAng, + int& nStat, double& dX, double& dY, double& dZ) +{ + Machine* pMch = GetCurrMachine() ; + return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, nStat, dX, dY, dZ) : false) ; +} + + //---------------------------------------------------------------------------- bool MachMgr::GetCalcTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB, diff --git a/MachMgrPhases.cpp b/MachMgrPhases.cpp index 8d38996..20458c0 100644 --- a/MachMgrPhases.cpp +++ b/MachMgrPhases.cpp @@ -39,7 +39,7 @@ MachMgr::AddPhase( void) // incremento il numero totale delle fasi ++ m_nPhasesCount ; // imposto la nuova fase corrente - PrepareCurrPhase( m_nPhasesCount, false) ; + PrepareCurrPhase( m_nPhasesCount, false, true) ; // inserisco la prima operazione di fase, sempre una disposizione string sName = "Disp" + ToString( m_nCurrPhase, 2) ; m_nCurrDispId = AddDisposition( sName) ; @@ -56,34 +56,34 @@ MachMgr::AddPhase( void) //---------------------------------------------------------------------------- bool -MachMgr::SetCurrPhase( int nPhase) +MachMgr::SetCurrPhase( int nPhase, bool bForced) { - return PrepareCurrPhase( nPhase, true) ; + return PrepareCurrPhase( nPhase, true, bForced) ; } //---------------------------------------------------------------------------- bool -MachMgr::PrepareCurrPhase( int nPhase, bool bDoDisp) +MachMgr::PrepareCurrPhase( int nPhase, bool bDoDisp, bool bForced) { // verifico esistenza fase if ( nPhase <= 0 || nPhase > m_nPhasesCount) return false ; - // se coincide con la vecchia fase, non devo fare alcunché - if ( nPhase == m_nCurrPhase) + // se non forzato e coincide con la vecchia fase, non devo fare alcunché + if ( ! bForced && nPhase == m_nCurrPhase) return true ; // se c'è lavorazione corrente e non appartiene a questa fase, reset const Machining* pMch = GetMachining( m_pGeomDB->GetUserObj( GetCurrMachining())) ; if ( pMch != nullptr && pMch->GetPhase() != nPhase) ResetCurrMachining() ; + // riporto tutti i grezzi linkati nel gruppo dei grezzi + Machine* pMach = GetCurrMachine() ; + if ( pMach != nullptr) + pMach->UnlinkAllRawPartsFromGroups() ; // tolgo i pezzi dai grezzi che non appartengono alla fase int nRawId = GetFirstRawPart() ; while ( nRawId != GDB_ID_NULL) { - // recupero le fasi in cui è presente il grezzo (se manca è fase 1) - INTVECTOR vPhase ; - if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty()) - vPhase.emplace_back( 1) ; - // se non appartiene - if ( find( vPhase.begin(), vPhase.end(), nPhase) == vPhase.end()) + // se non appartiene alla fase + if ( ! VerifyRawPartPhase( nRawId, nPhase)) SwapRawPartParts( nRawId, false) ; // passo al prossimo nRawId = GetNextRawPart( nRawId) ; @@ -91,12 +91,8 @@ MachMgr::PrepareCurrPhase( int nPhase, bool bDoDisp) // inserisco i pezzi nei grezzi che appartengono alla fase nRawId = GetFirstRawPart() ; while ( nRawId != GDB_ID_NULL) { - // recupero le fasi in cui è presente il grezzo (se manca è fase 1) - INTVECTOR vPhase ; - if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty()) - vPhase.emplace_back( 1) ; - // se appartiene - if ( find( vPhase.begin(), vPhase.end(), nPhase) != vPhase.end()) + // se appartiene alla fase + if ( VerifyRawPartPhase( nRawId, nPhase)) SwapRawPartParts( nRawId, true) ; // passo al prossimo nRawId = GetNextRawPart( nRawId) ; @@ -104,12 +100,8 @@ MachMgr::PrepareCurrPhase( int nPhase, bool bDoDisp) // aggiorno stato grezzi nRawId = GetFirstRawPart() ; while ( nRawId != GDB_ID_NULL) { - // recupero le fasi in cui è presente il grezzo - INTVECTOR vPhase ; - if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty()) - vPhase.emplace_back( 1) ; - // verifico - bool bOn = ( find( vPhase.begin(), vPhase.end(), nPhase) != vPhase.end()) ; + // verifico se appartiene alla fase + bool bOn = VerifyRawPartPhase( nRawId, nPhase) ; m_pGeomDB->SetStatus( nRawId, ( bOn ? GDB_ST_ON : GDB_ST_OFF)) ; // passo al prossimo nRawId = GetNextRawPart( nRawId) ; @@ -185,8 +177,7 @@ MachMgr::RemoveLastPhase( void) int nNextRawId = GetNextRawPart( nRawId) ; // recupero le fasi in cui è presente il grezzo INTVECTOR vPhase ; - if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty()) - vPhase.emplace_back( 1) ; + GetRawPartPhases( nRawId, vPhase) ; // se appartiene alla fase auto iIter = find( vPhase.begin(), vPhase.end(), m_nPhasesCount) ; if ( iIter != vPhase.end()) { diff --git a/MachMgrRawParts.cpp b/MachMgrRawParts.cpp index 6fa6463..ea93d4e 100644 --- a/MachMgrRawParts.cpp +++ b/MachMgrRawParts.cpp @@ -519,15 +519,27 @@ MachMgr::ModifyRawPartHeight( int nRawId, double dHeight) //---------------------------------------------------------------------------- bool -MachMgr::KeepRawPart( int nRawId) +MachMgr::GetRawPartPhases( int nRawId, INTVECTOR& vPhase) const { + // pulisco parametro di ritorno + vPhase.clear() ; // verifica validità grezzo if ( ! VerifyRawPart( nRawId)) return false ; // recupero le fasi in cui è presente il grezzo (se manca è fase 1) - INTVECTOR vPhase ; if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty()) vPhase.emplace_back( 1) ; + return true ; +} + +//---------------------------------------------------------------------------- +bool +MachMgr::KeepRawPart( int nRawId) +{ + // verifico validità e recupero fasi in cui è presente + INTVECTOR vPhase ; + if ( ! GetRawPartPhases( nRawId, vPhase)) + return false ; // se fase corrente già presente, non devo fare alcunché if ( find( vPhase.begin(), vPhase.end(), m_nCurrPhase) != vPhase.end()) return true ; @@ -545,13 +557,10 @@ MachMgr::KeepRawPart( int nRawId) bool MachMgr::VerifyRawPartPhase( int nRawId, int nPhase) const { - // verifico validità grezzo - if ( ! VerifyRawPart( nRawId)) - return false ; - // verifico sia attivo nella fase indicata + // verifico validità e recupero fasi in cui è presente INTVECTOR vPhase ; - if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty()) - vPhase.emplace_back( 1) ; + if ( ! GetRawPartPhases( nRawId, vPhase)) + return false ; return ( find( vPhase.begin(), vPhase.end(), nPhase) != vPhase.end()) ; } @@ -559,13 +568,10 @@ MachMgr::VerifyRawPartPhase( int nRawId, int nPhase) const bool MachMgr::RemoveRawPartFromCurrPhase( int nRawId) { - // verifica validità grezzo - if ( ! VerifyRawPart( nRawId)) - return false ; - // recupero le fasi in cui è presente il grezzo (se manca è fase 1) + // verifico validità e recupero fasi in cui è presente INTVECTOR vPhase ; - if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty()) - vPhase.emplace_back( 1) ; + if ( ! GetRawPartPhases( nRawId, vPhase)) + return false ; // se non appartiene alla fase corrente, non devo fare alcunché auto iIter = find( vPhase.begin(), vPhase.end(), m_nPhasesCount) ; if ( iIter == vPhase.end()) @@ -726,7 +732,7 @@ MachMgr::GetRawPartCenter( int nRawId, Point3d& ptCen) // provo a crearlo SetRawPartCenter( nRawId) ; nGPntId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_CENTER) ; - // se c'è ancora errore + // se non riesco, errore if ( nGPntId == GDB_ID_NULL) return false ; } diff --git a/MachMgrSimulation.cpp b/MachMgrSimulation.cpp index 965fe0d..cb1c270 100644 --- a/MachMgrSimulation.cpp +++ b/MachMgrSimulation.cpp @@ -112,8 +112,7 @@ MachMgr::SimStop( void) // verifico simulatore if ( m_pSimul == nullptr) return true ; - // lo fermo e cancello - m_pSimul->Stop() ; + // lo cancello (il distruttore provvede ovviamente a fermarlo) delete m_pSimul ; m_pSimul = nullptr ; return true ; diff --git a/Machine.cpp b/Machine.cpp index 8aee18d..8fc6ad1 100644 --- a/Machine.cpp +++ b/Machine.cpp @@ -211,7 +211,7 @@ Machine::LoadMachineTable( const string& sName, const string& sParent, int nType if ( ! AdjustAuxGeometry( vsAux, nLay)) return false ; // aggiusto la posizione della tavola - if ( ! AdjustTablePos( nLay, ptRef1)) + if ( ! AdjustTable( nLay, ptRef1)) return false ; // recupero l'area valida int nAreaId = m_pGeomDB->GetFirstNameInGroup( nLay, MCH_TAREA + "1") ; @@ -230,7 +230,7 @@ Machine::LoadMachineTable( const string& sName, const string& sParent, int nType //---------------------------------------------------------------------------- bool -Machine::AdjustTablePos( int nLay, const Point3d& ptRef1) +Machine::AdjustTable( int nLay, const Point3d& ptRef1) { // riferimento globale del gruppo tavola Frame3d frTable ; @@ -261,8 +261,8 @@ Machine::AdjustTablePos( int nLay, const Point3d& ptRef1) //---------------------------------------------------------------------------- bool Machine::LoadMachineAxis( const string& sName, const string& sParent, int nType, - const Point3d& ptPos, const Vector3d& vtDir, - const STROKE& Stroke, double dHome, const string& sGeo, const STRVECTOR& vsAux) + const Point3d& ptPos, const Vector3d& vtDir, const STROKE& Stroke, double dHome, + bool bAdjustAux, const string& sGeo, const STRVECTOR& vsAux) { // verifico sia di tipo ammesso if ( nType != MCH_AT_LINEAR && nType != MCH_AT_ROTARY) { @@ -305,7 +305,7 @@ Machine::LoadMachineAxis( const string& sName, const string& sParent, int nType, pAxis->Set( sName, nType, ptPos, vtDir, Stroke, dHome) ; m_pGeomDB->SetUserObj( nLay, pAxis) ; // verifico il vettore rappresentativo dell'asse - if ( ! AdjustAxisVector( nLay, sPart, sName, nType, ptPos, vtDir)) + if ( ! AdjustAxis( nLay, sPart, sName, nType, ptPos, vtDir, bAdjustAux)) return false ; // lo inserisco nel dizionario dei gruppi della macchina return m_mapGroups.emplace( sName, nLay).second ; @@ -313,8 +313,8 @@ Machine::LoadMachineAxis( const string& sName, const string& sParent, int nType, //---------------------------------------------------------------------------- bool -Machine::AdjustAxisVector( int nLay, const string& sPart, const string& sName, - int nType, const Point3d& ptPos, const Vector3d& vtDir) +Machine::AdjustAxis( int nLay, const string& sPart, const string& sName, + int nType, const Point3d& ptPos, const Vector3d& vtDir, bool bAdjustAux) { // verifico presenza vettore asse int nId = m_pGeomDB->GetFirstNameInGroup( nLay, sPart) ; @@ -355,10 +355,9 @@ Machine::AdjustAxisVector( int nLay, const string& sPart, const string& sName, LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } - return true ; } - // altrimenti asse rotante, devo verificarne direzione e coassialità + // altrimenti asse rotante, devo verificarne direzione e coassialità (questa solo se non abilitato movimento) else { Vector3d vtDirN = vtDir ; if ( ! vtDirN.Normalize() || ! AreSameVectorApprox( vtAxis, vtDirN)) { @@ -367,10 +366,10 @@ Machine::AdjustAxisVector( int nLay, const string& sPart, const string& sName, return false ; } Vector3d vtDelta = ptPos - ptBase ; - vtDelta -= ( vtDelta * vtDirN) * vtDirN ; - if ( ! vtDelta.IsSmall()) { - if ( vtDelta.Len() > m_dAxisMaxAdjust) { - string sOut = " Wrong Axis Base move = (" + ToString( vtDelta) + ")" ; + Vector3d vtDeltaPerp = vtDelta - ( vtDelta * vtDirN) * vtDirN ; + if ( ! vtDeltaPerp.IsSmall()) { + if ( vtDeltaPerp.Len() > m_dAxisMaxAdjust && ! bAdjustAux) { + string sOut = " Wrong Axis Base move = (" + ToString( vtDeltaPerp) + ")" ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) ; return false ; } @@ -380,8 +379,18 @@ Machine::AdjustAxisVector( int nLay, const string& sPart, const string& sName, m_pGeomDB->TranslateGlob( nId, vtDelta) ; } } - return true ; } + // 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) + m_pGeomDB->TranslateGlob( nIdAux, vtDelta) ; + nIdAux = m_pGeomDB->GetNext( nIdAux) ; + } + } + return true ; } //---------------------------------------------------------------------------- @@ -874,7 +883,7 @@ Machine::LinkRawPartToGroup( int nRawPartId, const std::string& sGroupName) if ( m_pGeomDB->GetParentId( nRawPartId) != nRawGrpId) return false ; // recupero il gruppo di macchina indicato - int nGrpId = m_pMchLua->GetGroup( sGroupName) ; + int nGrpId = GetGroup( sGroupName) ; if ( nGrpId == GDB_ID_NULL) return false ; // aggancio il grezzo al gruppo @@ -893,7 +902,7 @@ Machine::UnlinkRawPartFromGroup( int nRawPartId) if ( m_pGeomDB == nullptr || m_pMchMgr == nullptr) return false ; // recupero il gruppo dei grezzi dalla macchina - int nRawGrpId = m_pMchLua->m_pMchMgr->GetCurrRawGroupId() ; + int nRawGrpId = m_pMchMgr->GetCurrRawGroupId() ; if ( nRawGrpId == GDB_ID_NULL) return false ; // verifico che il grezzo indicato sia nell'elenco dei linkati @@ -901,7 +910,7 @@ Machine::UnlinkRawPartFromGroup( int nRawPartId) if ( iIter == m_vLinkedRawParts.end()) return false ; // riporto il grezzo nel gruppo dei grezzi - if ( ! m_pMchLua->m_pGeomDB->RelocateGlob( nRawPartId, nRawGrpId)) + if ( ! m_pGeomDB->RelocateGlob( nRawPartId, nRawGrpId)) return false ; // tolgo il grezzo dall'elenco dei linkati m_vLinkedRawParts.erase( iIter) ; diff --git a/Machine.h b/Machine.h index c336ca8..9f42c7a 100644 --- a/Machine.h +++ b/Machine.h @@ -68,14 +68,23 @@ class Machine bool GetCurrTool( std::string& sTool) const ; double GetCurrRot1W( void) const ; std::string GetKinematicAxis( int nInd) const ; + bool BlockKinematicRotAxis( const std::string& sName, double dVal) ; + bool BlockKinematicRotAxis( int nId, double dVal) ; + bool FreeKinematicRotAxis( const std::string& sName) ; + bool FreeKinematicRotAxis( int nId) ; + bool IsKinematicRotAxisBlocked( int nInd) ; int GetCurrLinAxes( void) const ; int GetCurrRotAxes( void) const ; bool GetAllCurrAxesName( STRVECTOR& vAxName) const ; bool GetAllCurrAxesHomePos( DBLVECTOR& vAxHomeVal) const ; bool GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const ; + bool GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, + int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const ; bool GetPositions( const Point3d& ptP, double dA, double dB, int& nStat, double& dX, double& dY, double& dZ) const ; + bool GetPositions( const Point3d& ptP, DBLVECTOR vAng, + int& nStat, double& dX, double& dY, double& dZ) const ; bool GetTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB, bool bBottom, Point3d& ptTip) const ; bool GetToolDirFromAngles( double dAngA, double dAngB, Vector3d& vtDir) const ; @@ -105,12 +114,12 @@ class Machine bool AdjustAuxGeometry( const STRVECTOR& vsAux, int nLay) ; bool LoadMachineTable( const std::string& sName, const std::string& sParent, int nType, const Point3d& ptRef1, const std::string& sGeo, const STRVECTOR& vsAux) ; - bool AdjustTablePos( int nLay, const Point3d& ptRef1) ; + bool AdjustTable( int nLay, const Point3d& ptRef1) ; bool LoadMachineAxis( const std::string& sName, const std::string& sParent, int nType, - const Point3d& ptPos, const Vector3d& vtDir, - const STROKE& Stroke, double dHome, const std::string& sGeo, const STRVECTOR& vsAux) ; - bool AdjustAxisVector( int nLay, const std::string& sPart, const std::string& sName, - int nType, const Point3d& ptPos, const Vector3d& vtDir) ; + const Point3d& ptPos, const Vector3d& vtDir, const STROKE& Stroke, double dHome, + bool bAdjustAux, const std::string& sGeo, const STRVECTOR& vsAux) ; + bool AdjustAxis( int nLay, const std::string& sPart, const std::string& sName, + int nType, const Point3d& ptPos, const Vector3d& vtDir, bool bAdjustAux) ; bool ModifyMachineAxisStroke( const std::string& sName, const STROKE& Stroke) ; bool ModifyMachineAxisHome( const std::string& sName, double dHome) ; bool LoadMachineStdHead( const std::string& sName, const std::string& sParent, const std::string& sHSet, @@ -148,9 +157,13 @@ class Machine bool CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit) ; bool CalculateKinematicChain( void) ; bool AddKinematicAxis( bool bOnHead, int nId) ; + bool 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 ; bool GetDirection( const Vector3d& vtDir, double dAngA, double dAngB, Vector3d& vtNew) const ; bool VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh) const ; - bool AdjustAngleInStroke( int nId, double& dAng) const ; + bool AdjustAngleInStroke( const STROKE& Stroke, double& dAng) const ; bool LuaInit( const std::string& sMachineName) ; bool LuaExit( void) ; bool LuaLoadMachine( const std::string& sFile) ; diff --git a/MachineCalc.cpp b/MachineCalc.cpp index 03e5387..ef9ac3f 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -277,6 +277,22 @@ Machine::CalculateKinematicChain( void) } return true ; } + // se 3 va bene ( uno dovrà poi avere valore assegnato) + if ( m_vCalcRotAx.size() == 3) { + // se ultimi due di testa, devo invertirne l'ordine + if ( m_vCalcRotAx[1].bHead && m_vCalcRotAx[2].bHead) + swap( m_vCalcRotAx[1], m_vCalcRotAx[2]) ; + // impongo limiti di corsa sul secondo asse rotante di testa + if ( m_vCalcRotAx[2].bHead) { + Head* pHead = GetHead( m_nCalcHeadId) ; + if ( pHead != nullptr) { + m_vCalcRotAx[2].stroke.Min = max( m_vCalcRotAx[2].stroke.Min, pHead->GetRot2Stroke().Min) ; + m_vCalcRotAx[2].stroke.Max = min( m_vCalcRotAx[2].stroke.Max, pHead->GetRot2Stroke().Max) ; + } + } + return true ; + } + // altrimenti non ancora gestito, quindi errore return false ; } @@ -332,18 +348,193 @@ Machine::GetKinematicAxis( int nInd) const 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) +{ + int nRotAxTot = int( m_vCalcRotAx.size()) ; + if ( nInd < 0 || nInd >= nRotAxTot) + return false ; + return m_vCalcRotAx[nInd].bFixed ; +} + //---------------------------------------------------------------------------- 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 ; - // se nessun asse rotante, non c'è alcunchè da calcolare - if ( m_vCalcRotAx.size() == 0) { - nStat = 1 ; - return true ; - } + // direzione fresa normalizzata Vector3d vtDirTn = vtDirT ; if ( ! vtDirTn.Normalize()) @@ -352,50 +543,61 @@ Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, Vector3d vtDirAn = vtDirA ; vtDirAn.Normalize() ; // direzione fresa su testa a riposo - Vector3d vtDirH = m_vtCalcDir ; + Vector3d vtDirHn = vtDirH ; // direzione ausiliaria su testa a riposo - Vector3d vtDirI = m_vtCalcADir ; + 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 = m_vCalcRotAx[0].vtDir ; // se asse di tavola, ne inverto la direzione - if ( ! m_vCalcRotAx[0].bHead) + 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 ( m_vCalcRotAx.size() == 2) { + if ( nNumRotAx == 2) { // direzione secondo asse rotante - Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ; + Vector3d vtAx2 = RotAx2.vtDir ; // se asse di tavola, ne inverto la direzione - if ( ! m_vCalcRotAx[1].bHead) + if ( ! RotAx2.bHead) vtAx2.Invert() ; // calcolo secondo angolo di rotazione - nStat = GetRotationComponent( vtDirH, dCompTSuAxR1, vtAx1, vtAx2, dAngB1, dAngB2, bDet) ; + nStat = GetRotationComponent( vtDirHn, dCompTSuAxR1, 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 = vtDirH ; + vtDirH1 = vtDirHn ; vtDirH1.Rotate( vtAx2, dAngB1) ; - vtDirI1 = vtDirI ; + vtDirI1 = vtDirIn ; vtDirI1.Rotate( vtAx2, dAngB1) ; } if ( nStat == 2) { - vtDirH2 = vtDirH ; + vtDirH2 = vtDirHn ; vtDirH2.Rotate( vtAx2, dAngB2) ; - vtDirI2 = vtDirI ; + vtDirI2 = vtDirIn ; vtDirI2.Rotate( vtAx2, dAngB2) ; } } // altrimenti verifico se compatibili else { // componente versore utensile su direzione primo asse - double dCompHSuAxR1 = vtDirH * vtAx1 ; + 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) ; @@ -405,8 +607,8 @@ Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, const double SIN_ANG_ERROR = sin( 0.002 * DEGTORAD) ; if ( fabs( dCompTOrtAxR1 * dCompHSuAxR1 - dCompHOrtAxR1 * dCompTSuAxR1) < SIN_ANG_ERROR) { nStat = 1 ; - vtDirH1 = vtDirH ; - vtDirI1 = vtDirI ; + vtDirH1 = vtDirHn ; + vtDirI1 = vtDirIn ; // reset secondo angolo dAngB1 = 0 ; } @@ -469,12 +671,14 @@ Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, // verifiche dei limiti di corsa if ( nStat >= 2) { // se non riesco ad aggiustare, elimino - if ( ! AdjustAngleInStroke( 1, dAngA2) || ! AdjustAngleInStroke( 2, dAngB2)) + if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA2) || + ( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, dAngB2))) -- nStat ; } if ( nStat >= 1) { // se non riesco ad aggiustare, elimino - if ( ! AdjustAngleInStroke( 1, dAngA1) || ! AdjustAngleInStroke( 2, dAngB1)) { + if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA1) || + ( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, dAngB1))) { -- nStat ; // riloco eventuale soluzione rimasta if ( nStat >= 1) { @@ -497,7 +701,28 @@ Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB, int& nStat, double& dX, double& dY, double& dZ) const { // la posizione deve essere espressa rispetto allo ZERO MACCHINA - // per ora gestisco solo gli assi rotanti di testa + + // posizione punto di lavoro con pezzo immobile + Point3d ptW = ptP ; + + // se c'è primo asse rotante di tavola + if ( m_vCalcRotAx.size() >= 1 && ! m_vCalcRotAx[0].bHead) { + // posizione e direzione primo asse rotante + Point3d ptAx1 = m_vCalcRotAx[0].ptPos ; + Vector3d vtAx1 = m_vCalcRotAx[0].vtDir ; + // ruoto punto di lavoro + ptW.Rotate( ptAx1, vtAx1, dAngA) ; + } + + // se c'è secondo asse rotante di tavola + if ( m_vCalcRotAx.size() >= 2 && ! m_vCalcRotAx[1].bHead) { + // posizione e direzione primo asse rotante + Point3d ptAx2 = m_vCalcRotAx[1].ptPos ; + Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ; + // ruoto punto di lavoro + ptW.Rotate( ptAx2, vtAx2, dAngB) ; + } + // posizione e direzione fresa su testa a riposo Point3d ptPosH = m_ptCalcPos ; @@ -533,9 +758,54 @@ Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB, Vector3d vtDtTL = vtDirH * m_dCalcTLen ; // calcolo le posizioni degli assi lineari - dX = ptP.x + vtDtHe.x + vtDtAx.x + vtDtTL.x ; - dY = ptP.y + vtDtHe.y + vtDtAx.y + vtDtTL.y ; - dZ = ptP.z + vtDtHe.z + vtDtAx.z + vtDtTL.z ; + dX = ptW.x + vtDtHe.x + vtDtAx.x + vtDtTL.x ; + dY = ptW.y + vtDtHe.y + vtDtAx.y + vtDtTL.y ; + dZ = ptW.z + vtDtHe.z + vtDtAx.z + vtDtTL.z ; + + // tutto ok + nStat = 0 ; + return true ; +} + +//---------------------------------------------------------------------------- +bool +Machine::GetPositions( const Point3d& ptP, DBLVECTOR vAng, + int& nStat, double& dX, double& dY, double& dZ) const +{ + // la posizione deve essere espressa rispetto allo ZERO MACCHINA + + // aggiorno punto di lavoro mediante ciclo diretto sugli assi di tavola + Point3d ptW = ptP ; + for ( size_t i = 0 ; i < 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]) ; + } + + // 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 + dX = ptW.x + vtDtHe.x + vtDtAx.x + vtDtTL.x ; + dY = ptW.y + vtDtHe.y + vtDtAx.y + vtDtTL.y ; + dZ = ptW.z + vtDtHe.z + vtDtAx.z + vtDtTL.z ; // tutto ok nStat = 0 ; @@ -639,18 +909,14 @@ Machine::VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh) //---------------------------------------------------------------------------- bool -Machine::AdjustAngleInStroke( int nId, double& dAng) const +Machine::AdjustAngleInStroke( const STROKE& Stroke, double& dAng) const { - // se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare - if ( nId <= 0 || nId > int( m_vCalcRotAx.size())) - return true ; // eseguo gli aggiustamenti - while ( dAng < m_vCalcRotAx[nId-1].stroke.Min) + while ( dAng < Stroke.Min) dAng += ANG_FULL ; - while ( dAng > m_vCalcRotAx[nId-1].stroke.Max) + while ( dAng > Stroke.Max) dAng -= ANG_FULL ; - return ( dAng >= m_vCalcRotAx[nId-1].stroke.Min && - dAng <= m_vCalcRotAx[nId-1].stroke.Max) ; + return ( dAng >= Stroke.Min && dAng <= Stroke.Max) ; } //---------------------------------------------------------------------------- diff --git a/MachineLua.cpp b/MachineLua.cpp index c2ac471..23553e8 100644 --- a/MachineLua.cpp +++ b/MachineLua.cpp @@ -40,6 +40,7 @@ static const string FLD_POS = "Pos" ; static const string FLD_DIR = "Dir" ; static const string FLD_STROKE = "Stroke" ; static const string FLD_HOME = "Home" ; +static const string FLD_ADJUSTAUX = "AdjustAux" ; static const string FLD_HSET = "HSet" ; static const string FLD_TDIR = "TDir" ; static const string FLD_ADIR = "ADir" ; @@ -384,6 +385,9 @@ Machine::LuaEmtAxis( lua_State* L) // lettura eventuale campo 'Home' dalla tabella (default 0) double dHome = 0 ; LuaGetTabFieldParam( L, 1, FLD_HOME, dHome) ; + // lettura eventuale campo 'AdjustAux' dalla tabella (defualt false) + bool bAdjustAux = false ; + LuaGetTabFieldParam( L, 1, FLD_ADJUSTAUX, bAdjustAux) ; // lettura campo 'Geo' dalla tabella string sGeo ; LuaCheckTabFieldParam( L, 1, FLD_GEO, sGeo) @@ -401,7 +405,8 @@ Machine::LuaEmtAxis( lua_State* L) return luaL_error( L, " Unknown Machine") ; // carico i dati dell'asse - if ( ! m_pMchLua->LoadMachineAxis( sName, sParent, nType, ptPos, vtDir, Stroke, dHome, sGeo, vsAux)) + if ( ! m_pMchLua->LoadMachineAxis( sName, sParent, nType, ptPos, vtDir, + Stroke, dHome, bAdjustAux, sGeo, vsAux)) return luaL_error( L, " Load Machine Axis failed") ; return 0 ; diff --git a/MachineStruConst.h b/MachineStruConst.h index 0f19460..0ec4a5e 100644 --- a/MachineStruConst.h +++ b/MachineStruConst.h @@ -14,6 +14,7 @@ #pragma once #include "/EgtDev/Include/EGkPoint3d.h" +#include "/EgtDev/Include/EGkGdbConst.h" #include #include @@ -46,7 +47,12 @@ struct KinAxis { Point3d ptPos ; Vector3d vtDir ; STROKE stroke ; -}; + bool bFixed ; + double dFixVal ; + KinAxis( void) + : nGrpId( GDB_ID_NULL), bLinear( true), bHead( true), ptPos(), vtDir(), bFixed( false), dFixVal( 0) + { stroke.Min = 0 ; stroke.Max = 0 ; } +} ; typedef std::vector KINAXISVECTOR ; //---------------------------------------------------------------------------- diff --git a/Operation.cpp b/Operation.cpp index dfeb917..a79a139 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -454,24 +454,19 @@ Operation::CalculateAxesValues( void) int nRotAxes = m_pMchMgr->GetCurrRotAxes() ; // assegno gli angoli iniziali - double dAngAprec = 0 ; - double dAngBprec = 0 ; + DBLVECTOR vAxRotPrec( nRotAxes, 0.) ; DBLVECTOR vAxVal ; // se utensile non cambiato, uso gli angoli finali della lavorazione precedente if ( ! sPrevTool.empty() && GetToolName() == sPrevTool && pPrevOp->GetFinalAxesValues( vAxVal)) { - if ( nRotAxes >= 1) - dAngAprec = vAxVal[nLinAxes - 1 + 1] ; - if ( nRotAxes >= 2) - dAngBprec = vAxVal[nLinAxes - 1 + 2] ; + for ( int i = 0 ; i < nRotAxes ; ++ i) + vAxRotPrec[i] = vAxVal[nLinAxes + i] ; } // altrimenti uso gli angoli home else { m_pMchMgr->GetAllCalcAxesHomePos( vAxVal) ; - if ( nRotAxes >= 1) - dAngAprec = vAxVal[nLinAxes - 1 + 1] ; - if ( nRotAxes >= 2) - dAngBprec = vAxVal[nLinAxes - 1 + 2] ; + for ( int i = 0 ; i < nRotAxes ; ++ i) + vAxRotPrec[i] = vAxVal[nLinAxes + i] ; } // recupero peso primo asse rotante di testa @@ -486,15 +481,14 @@ Operation::CalculateAxesValues( void) bool bOk = true ; int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ; while ( nClPathId != GDB_ID_NULL) { - double dAngAprecOri = dAngAprec ; - double dAngBprecOri = dAngBprec ; + DBLVECTOR vAxRotPrecOri = vAxRotPrec ; int nOutStrC = 0 ; - if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, dAngAprec, dAngBprec, nOutStrC)) { + if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, vAxRotPrec, nOutStrC)) { // se extracorsa dell'asse C, provo a precaricarlo al contrario if ( nOutStrC != 0) { - dAngAprec = dAngAprecOri + ( nOutStrC > 0 ? - ANG_FULL : ANG_FULL) ; - dAngBprec = dAngBprecOri ; - if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, dAngAprec, dAngBprec, nOutStrC)) + vAxRotPrec[0] = vAxRotPrecOri[0] + ( nOutStrC > 0 ? - ANG_FULL : ANG_FULL) ; + vAxRotPrec[1] = vAxRotPrecOri[1] ; + if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, vAxRotPrec, nOutStrC)) bOk = false ; } else @@ -509,7 +503,7 @@ Operation::CalculateAxesValues( void) //---------------------------------------------------------------------------- bool Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, - double& dAngAprec, double& dAngBprec, int& nOutStrC) + DBLVECTOR& vAxRotPrec, int& nOutStrC) { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; @@ -538,8 +532,9 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, } // calcolo degli assi rotanti della macchina int nRStat ; - double dAngA1, dAngB1, dAngA2, dAngB2 ; - bool bROk = m_pMchMgr->GetCalcAngles( pCamData->GetToolDir(), pCamData->GetAuxDir(), nRStat, dAngA1, dAngB1, dAngA2, dAngB2) ; + //m_pMchMgr->BlockKinematicRotAxis( "C", 90) ; + DBLVECTOR vAng1, vAng2 ; + bool bROk = m_pMchMgr->GetCalcAngles( pCamData->GetToolDir(), pCamData->GetAuxDir(), nRStat, vAng1, vAng2) ; if ( ! bROk || nRStat == 0) { bOk = false ; pCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ; @@ -549,41 +544,46 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, // se primo movimento if ( bFirst) { // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri - m_pMchMgr->GetNearestAngleInStroke( 1, dAngAprec, dAngA1) ; - m_pMchMgr->GetNearestAngleInStroke( 2, dAngBprec, dAngB1) ; + for ( size_t i = 0 ; i < vAng1.size() ; ++ i) + m_pMchMgr->GetNearestAngleInStroke( int( i), vAxRotPrec[i], vAng1[i]) ; } // per movimenti successivi else { // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse - dAngA1 = AngleNearAngle( dAngA1, dAngAprec) ; - dAngB1 = AngleNearAngle( dAngB1, dAngBprec) ; + for ( size_t i = 0 ; i < vAng1.size() ; ++ i) + vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; } } if ( nRStat == 2) { // se primo movimento if ( bFirst) { // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri - m_pMchMgr->GetNearestAngleInStroke( 1, dAngAprec, dAngA1) ; - m_pMchMgr->GetNearestAngleInStroke( 2, dAngBprec, dAngB1) ; - m_pMchMgr->GetNearestAngleInStroke( 1, dAngAprec, dAngA2) ; - m_pMchMgr->GetNearestAngleInStroke( 2, dAngBprec, dAngB2) ; + for ( size_t i = 0 ; i < vAng1.size() ; ++ i) { + m_pMchMgr->GetNearestAngleInStroke( int( i), vAxRotPrec[i], vAng1[i]) ; + m_pMchMgr->GetNearestAngleInStroke( int( i), vAxRotPrec[i], vAng2[i]) ; + } } else { // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse - dAngA1 = AngleNearAngle( dAngA1, dAngAprec) ; - dAngB1 = AngleNearAngle( dAngB1, dAngBprec) ; - dAngA2 = AngleNearAngle( dAngA2, dAngAprec) ; - dAngB2 = AngleNearAngle( dAngB2, dAngBprec) ; + for ( size_t i = 0 ; i < vAng1.size() ; ++ i) { + vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; + vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ; + } } // scelgo la soluzione più vicina ai precedenti - double dDeltaA1 = dRot1W * fabs( dAngA1 - dAngAprec) ; - double dDeltaB1 = fabs( dAngB1 - dAngBprec) ; - double dDeltaA2 = dRot1W * fabs( dAngA2 - dAngAprec) ; - double dDeltaB2 = fabs( dAngB2 - dAngBprec) ; - if ( dDeltaA2 + dDeltaB2 < dDeltaA1 + dDeltaB1 - 1.) { - dAngA1 = dAngA2 ; - dAngB1 = dAngB2 ; + double dDelta1 = 0 ; + double dDelta2 = 0 ; + bool bFirst = true ; + for ( size_t i = 0 ; i < vAng1.size() ; ++ i) { + // ignoro gli assi bloccati + if ( ! m_pMchMgr->IsKinematicRotAxisBlocked( int( i))) + continue ; + // calcolo i delta asse con eventuale peso + dDelta1 += fabs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ; + dDelta2 += fabs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ; } + if ( dDelta2 < dDelta1 - 1.) + vAng1 = vAng2 ; } // ricavo posizione con eventuali modifiche dipendenti dalla lavorazione Point3d ptP = pCamData->GetEndPoint() ; @@ -591,7 +591,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, // calcolo gli assi lineari della macchina int nLStat ; double dX, dY, dZ ; - bool bLOk = m_pMchMgr->GetCalcPositions( ptP, dAngA1, dAngB1, nLStat, dX, dY, dZ) ; + bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAng1, nLStat, dX, dY, dZ) ; if ( ! bLOk || nLStat != 0) { bOk = false ; pCamData->SetAxes( CamData::AS_ERR, vAxVal) ; @@ -602,11 +602,13 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, vAxVal.emplace_back( dX) ; vAxVal.emplace_back( dY) ; vAxVal.emplace_back( dZ) ; - vAxVal.emplace_back( dAngA1) ; - vAxVal.emplace_back( dAngB1) ; + for ( auto dAng : vAng1) + vAxVal.emplace_back( dAng) ; // verifico i limiti di corsa degli assi int nStat ; - bool bOsOk = m_pMchMgr->VerifyOutstroke( dX, dY, dZ, dAngA1, dAngB1, nStat) ; + double dAngA = ( vAng1.size() >= 1 ? vAng1[0] : 0) ; + double dAngB = ( vAng1.size() >= 2 ? vAng1[1] : 0) ; + bool bOsOk = m_pMchMgr->VerifyOutstroke( dX, dY, dZ, dAngA, dAngB, nStat) ; if ( ! bOsOk || nStat != 0) { bOk = false ; pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ; @@ -630,11 +632,12 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, Point3d ptMid = ptP ; ptMid.Rotate( ptCen, Z_AX, - dAngCen / 2) ; // determino i valori degli assi al punto medio - double dAngAmid = ( dAngAprec + dAngA1) / 2 ; - double dAngBmid = ( dAngBprec + dAngB1) / 2 ; + DBLVECTOR vAngMid( vAng1.size()) ; + for ( size_t i = 0 ; i < vAng1.size() ; ++ i) + vAngMid[i] = ( vAxRotPrec[i] + vAng1[i]) / 2 ; int nLmidStat ; double dXmid, dYmid, dZmid ; - bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, dAngAmid, dAngBmid, nLmidStat, dXmid, dYmid, dZmid) ; + bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, vAngMid, nLmidStat, dXmid, dYmid, dZmid) ; if ( ! bLmidOk || nLmidStat != 0) { bOk = false ; pCamData->SetAxes( CamData::AS_ERR, vAxVal) ; @@ -680,10 +683,11 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, vtCurr.Rotate( Z_AX, dAngCenStep) ; Point3d ptCurr = ptCen + vtCurr ; double dCoeff = i / NUM_VERIF_STEP ; - double dAngA = dAngAprec * ( 1 - dCoeff) + dAngA1 * dCoeff ; - double dAngB = dAngBprec * ( 1 - dCoeff) + dAngB1 * dCoeff ; + DBLVECTOR vAng( vAng1.size()) ; + for ( size_t i = 0 ; i < vAng1.size() ; ++ i) + vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAng1[i] * dCoeff ; int nStat ; - bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, dAngA, dAngB, nStat) ; + bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng[0], vAng[1], nStat) ; if ( ! bOsOk || nStat != 0) { bOk = false ; pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ; @@ -700,8 +704,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, dYprec = dY ; dZprec = dZ ; // memorizzo i valori degli angoli come nuovi precedenti - dAngAprec = dAngA1 ; - dAngBprec = dAngB1 ; + vAxRotPrec = vAng1 ; } return bOk ; @@ -892,6 +895,8 @@ Operation::AddRise( DBLVECTOR& vAxVal, double dDelta) return false ; // recupero i valori degli assi vAxVal = pCam->GetAxesVal() ; + if ( vAxVal.size() < 3) + return false ; // flag per tipo di movimento in rapido int nFlag = 0 ; // se delta positivo lo uso come incremento di Z diff --git a/Operation.h b/Operation.h index 685ee98..f07bf56 100644 --- a/Operation.h +++ b/Operation.h @@ -67,7 +67,7 @@ class Operation : public IUserObj bool GetClPathFinalAxesValues( int nClPathId, DBLVECTOR& vAxVal) ; bool CalculateAxesValues( void) ; bool CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, - double& dAngAprec, double& dAngBprec, int& nOutStrC) ; + DBLVECTOR& vAxRotPrec, int& nOutStrC) ; bool AdjustStartEndMovements( void) ; bool AdjustOneStartMovement( int nClPathId, const DBLVECTOR& vAxPrev) ; bool ToolChangeNeeded( const Operation& Op1, const Operation& Op2) ; diff --git a/OutputConst.h b/OutputConst.h index b2abc58..5753640 100644 --- a/OutputConst.h +++ b/OutputConst.h @@ -59,6 +59,8 @@ static const std::string GVAR_L2 = ".L2" ; // (num) valore del static const std::string GVAR_L3 = ".L3" ; // (num) valore del terzo asse lineare static const std::string GVAR_R1 = ".R1" ; // (num) valore del primo asse rotante static const std::string GVAR_R2 = ".R2" ; // (num) valore del secondo asse rotante +static const std::string GVAR_R3 = ".R3" ; // (num) valore del terzo asse rotante +static const std::string GVAR_R4 = ".R4" ; // (num) valore del quartoo asse rotante static const std::string GVAR_C1 = ".C1" ; // (num) valore del primo asse lineare per centro arco static const std::string GVAR_C2 = ".C2" ; // (num) valore del secondo asse lineare per centro arco static const std::string GVAR_C3 = ".C3" ; // (num) valore del terzo asse lineare per centro arco @@ -69,11 +71,15 @@ static const std::string GVAR_L2P = ".L2p" ; // (num) valore pre static const std::string GVAR_L3P = ".L3p" ; // (num) valore precedente del terzo asse lineare static const std::string GVAR_R1P = ".R1p" ; // (num) valore precedente del primo asse rotante static const std::string GVAR_R2P = ".R2p" ; // (num) valore precedente del secondo asse rotante +static const std::string GVAR_R3P = ".R3p" ; // (num) valore precedente del terzo asse rotante +static const std::string GVAR_R4P = ".R4p" ; // (num) valore precedente del quarto asse rotante static const std::string GVAR_L1T = ".L1t" ; // (num) token del primo asse lineare static const std::string GVAR_L2T = ".L2t" ; // (num) token del secondo asse lineare static const std::string GVAR_L3T = ".L3t" ; // (num) token del terzo asse lineare static const std::string GVAR_R1T = ".R1t" ; // (num) token del primo asse rotante static const std::string GVAR_R2T = ".R2t" ; // (num) token del secondo asse rotante +static const std::string GVAR_R3T = ".R3t" ; // (num) token del terzo asse rotante +static const std::string GVAR_R4T = ".R4t" ; // (num) token del quarto asse rotante static const std::string GVAR_C1T = ".C1t" ; // (num) token del primo asse lineare per centro arco static const std::string GVAR_C2T = ".C2t" ; // (num) token del secondo asse lineare per centro arco static const std::string GVAR_C3T = ".C3t" ; // (num) token del terzo asse lineare per centro arco diff --git a/Simulator.cpp b/Simulator.cpp index 7b260f9..a3ab745 100644 --- a/Simulator.cpp +++ b/Simulator.cpp @@ -45,7 +45,7 @@ Simulator::Simulator( void) //---------------------------------------------------------------------------- Simulator::~Simulator( void) { - + Stop() ; } //---------------------------------------------------------------------------- @@ -215,6 +215,8 @@ Simulator::Move( int& nStatus) nStatus = MCH_SIM_ERR ; return false ; } + // recupero eventuali forzature assi da gestione inizio disposizione + UpdateAxes() ; break ; } // altrimenti disposizione passiva @@ -496,8 +498,12 @@ Simulator::UpdateAxes( void) if ( ! pMachine->GetAllCurrAxesName( m_AxesName)) return false ; // Aggiorno la posizione corrente degli assi macchina attivi - for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i) - m_pMachine->GetAxisPos( m_AxesName[i], m_AxesVal[i]) ; + m_AxesVal.clear() ; + for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i) { + double dVal ; + m_pMachine->GetAxisPos( m_AxesName[i], dVal) ; + m_AxesVal.emplace_back( dVal) ; + } return true ; }