diff --git a/MachMgr.h b/MachMgr.h index c765090..8b5d271 100644 --- a/MachMgr.h +++ b/MachMgr.h @@ -184,6 +184,8 @@ class MachMgr : public IMachMgr int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) ; virtual bool GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB, int& nStat, double& dX, double& dY, double& dZ) ; + virtual bool GetCalcTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB, + bool bBottom, Point3d& ptTip) ; virtual bool GetNearestAngleInStroke( int nId, double dAngRef, double& dAng) ; virtual bool VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat) ; // Machine Move diff --git a/MachMgrMachines.cpp b/MachMgrMachines.cpp index a5654e4..db8d0df 100644 --- a/MachMgrMachines.cpp +++ b/MachMgrMachines.cpp @@ -309,6 +309,18 @@ MachMgr::GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB, return m_vMachines[m_nCurrMch].pMachine->GetPositions( ptP, dAngA, dAngB, nStat, dX, dY, dZ) ; } +//---------------------------------------------------------------------------- +bool +MachMgr::GetCalcTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB, + bool bBottom, Point3d& ptTip) +{ + if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) || + m_vMachines[m_nCurrMch].pMachine == nullptr) + return false ; + return m_vMachines[m_nCurrMch].pMachine->GetTipFromPositions( dX, dY, dZ, dAngA, dAngB, bBottom, ptTip) ; + +} + //---------------------------------------------------------------------------- bool MachMgr::GetNearestAngleInStroke( int nId, double dAngRef, double& dAng) diff --git a/Machine.h b/Machine.h index e462fad..d757b68 100644 --- a/Machine.h +++ b/Machine.h @@ -64,6 +64,8 @@ class Machine int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) ; bool GetPositions( const Point3d& ptP, double dA, double dB, int& nStat, double& dX, double& dY, double& dZ) ; + bool GetTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB, + bool bBottom, Point3d& ptTip) ; bool GetNearestAngleInStroke( int nId, double dAngRef, double& dAng) ; bool VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat) ; bool LuaCallFunction( const std::string& sFun) ; @@ -110,6 +112,7 @@ class Machine bool CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit) ; bool CalculateKinematicChain( void) ; bool AddKinematicAxis( bool bOnHead, int nId) ; + bool GetDirection( const Vector3d& vtDir, double dAngA, double dAngB, Vector3d& vtNew) ; bool VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh) ; bool AdjustAngleInStroke( int nId, double& dAng) ; bool LuaInit( const std::string& sMachineName) ; @@ -139,6 +142,7 @@ class Machine Vector3d m_vtCalcDir ; // direzione utensile a riposo per calcoli Vector3d m_vtCalcADir ; // direzione ausiliaria a riposo per calcoli double m_dCalcTLen ; // lunghezza utensile corrente per calcoli + double m_dCalcTRad ; // raggio utensile corrente per calcoli KINAXISVECTOR m_vCalcLinAx ; // vettore assi lineari attivi per calcoli KINAXISVECTOR m_vCalcRotAx ; // vettore assi rotanti attivi per calcoli diff --git a/MachineCalc.cpp b/MachineCalc.cpp index a09fefb..6a45a11 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -78,6 +78,7 @@ Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit) m_dCalcRot1W = 1 ; m_nCalcSolCh = MCH_SCC_NONE ; m_dCalcTLen = 0 ; + m_dCalcTRad = 0 ; // recupero il gruppo della testa int nHeadId = GetGroup( sHead) ; // recupero i dati della testa @@ -98,7 +99,10 @@ Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit) if ( nToolId == GDB_ID_NULL || m_pGeomDB->GetGdbType( nToolId) != GDB_TY_GROUP) return false ; double dTLen = 0 ; - if ( ! m_pMchMgr->TdbSetCurrTool( sTool) || ! m_pMchMgr->TdbGetCurrToolParam( TPA_LEN, dTLen)) + double dTDiam = 0 ; + if ( ! m_pMchMgr->TdbSetCurrTool( sTool) || + ! m_pMchMgr->TdbGetCurrToolParam( TPA_LEN, dTLen) || + ! m_pMchMgr->TdbGetCurrToolParam( TPA_DIAM, dTDiam)) return false ; // assegno tutti i dati m_nCalcHeadId = nHeadId ; @@ -110,6 +114,7 @@ Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit) m_vtCalcDir = pExit->GetTDir() ; m_vtCalcADir = pHead->GetADir() ; m_dCalcTLen = dTLen ; + m_dCalcTRad = dTDiam / 2 ; // determino la catena cinematica return CalculateKinematicChain() ; } @@ -433,7 +438,7 @@ Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB, int& nStat, double& dX, double& dY, double& dZ) { // la posizione deve essere espressa rispetto allo ZERO MACCHINA - // per ora gestisco solo gli assi di testa + // per ora gestisco solo gli assi rotanti di testa // posizione e direzione fresa su testa a riposo Point3d ptPosH = m_ptCalcPos ; @@ -478,6 +483,68 @@ Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB, return true ; } +//---------------------------------------------------------------------------- +bool +Machine::GetDirection( const Vector3d& vtDir, double dAngA, double dAngB, Vector3d& vtNew) +{ + // per ora gestisco solo gli assi rotanti di testa + + // direzione a riposo + vtNew = vtDir ; + + // se c'č secondo asse rotante di testa + 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 dati a riposo + vtNew.Rotate( vtAx2, dAngB) ; + } + + // se c'č primo asse rotante di testa + 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 dati a riposo + vtNew.Rotate( vtAx1, dAngA) ; + } + return true ; +} + +//---------------------------------------------------------------------------- +bool +Machine::GetTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB, + bool bBottom, Point3d& ptTip) +{ + // la posizione deve essere espressa rispetto allo ZERO MACCHINA + // per ora gestisco solo gli assi rotanti di testa + + // Calcoli recuperi della testa e lunghezza utensile per orientamento + int nStat ; + double dRecX, dRecY, dRecZ ; + if ( ! GetPositions( ORIG, dAngA, dAngB, nStat, dRecX, dRecY, dRecZ)) + return false ; + ptTip.Set( dX - dRecX, dY - dRecY, dZ - dRecZ) ; + + // Se richiesto punto sotto del tip utensile + if ( bBottom) { + // calcolo la direzione fresa + Vector3d vtDirT ; + if ( ! GetDirection( m_vtCalcDir, dAngA, dAngB, vtDirT)) + return false ; + // se direzione fresa non esattamente verticale, esiste un punto pių basso del'utensile + if ( ! vtDirT.IsZplus() && ! vtDirT.IsZminus()) { + // calcolo la direzione perpendicolare pių verticale possibile + Vector3d vtCorr = FromUprightOrtho( vtDirT) ; + // correggo il tip + ptTip -= vtCorr * m_dCalcTRad ; + } + } + + return true ; +} + //---------------------------------------------------------------------------- bool Machine::VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh)