From 3f1a871b5a61c417b08f9ae7fdf6be6927abe3dd Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Mon, 15 Jan 2018 09:59:08 +0000 Subject: [PATCH] EgtMachKernel : - modifiche per virtual milling (sperimentale). --- Machine.h | 2 + MachineCalc.cpp | 21 ++++++++++ OutputConst.h | 1 + Simulator.cpp | 105 +++++++++++++++++++++++++++++++++++++++++------- Simulator.h | 3 ++ 5 files changed, 118 insertions(+), 14 deletions(-) diff --git a/Machine.h b/Machine.h index dc37c27..626a197 100644 --- a/Machine.h +++ b/Machine.h @@ -114,6 +114,8 @@ class Machine int& nStat, double& dX, double& dY, double& dZ) const ; bool GetHeadOffsetDelta( const DBLVECTOR& vAng, double& dRecX, double& dRecY, double& dRecZ) const ; + bool GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, + Point3d& ptNose) const ; bool GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, bool bBottom, bool bOverall, Point3d& ptTip) const ; bool GetToolDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const ; diff --git a/MachineCalc.cpp b/MachineCalc.cpp index 442cd4b..cdd0043 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -1055,6 +1055,27 @@ Machine::GetBackDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3 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 rotazioni di testa) + + // aggiorno posizione testa a riposo mediante ciclo inverso sugli assi di testa + Point3d ptPosH = m_ptCalcPos ; + 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]) ; + } + } + + ptNose.Set( dX + ptPosH.x, dY + ptPosH.y, dZ + ptPosH.z) ; + return true ; +} + //---------------------------------------------------------------------------- bool Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, diff --git a/OutputConst.h b/OutputConst.h index 75d7a0f..ddd2bb9 100644 --- a/OutputConst.h +++ b/OutputConst.h @@ -126,6 +126,7 @@ static const std::string GVAR_ENABAXES = ".EnabAxes" ; // (bool) flag abili static const std::string GVAR_AUXAXES = ".AuxAxes" ; // (int) numero assi ausiliari per simulazione static const std::string GVAR_AN = ".AN" ; // (num) valore del N-esimo asse ausiliario per simulazione static const std::string GVAR_ANN = ".ANn" ; // (string) nome del N-esimo asse ausiliario per simulazione +static const std::string GVAR_VMILL = ".VMILL" ; // (int) identificativo Zmap attivo // Funzioni generazione static const std::string ON_START = "OnStart" ; diff --git a/Simulator.cpp b/Simulator.cpp index f9686a9..6aa539a 100644 --- a/Simulator.cpp +++ b/Simulator.cpp @@ -20,6 +20,9 @@ #include "Machining.h" #include "MachiningConst.h" #include "OutputConst.h" +#include "DllMain.h" +#include "/EgtDev/Include/EGkVolZmap.h" +#include "/EgtDev/Include/EGkStringUtils3d.h" #include "/EgtDev/Include/EMkToolConst.h" #include "/EgtDev/Include/EMkOperationConst.h" #include "/EgtDev/Include/EgtNumUtils.h" @@ -47,6 +50,7 @@ Simulator::Simulator( void) m_nAuxSInd = 0 ; m_nAuxETot = 0 ; m_nAuxEInd = 0 ; + m_nVmId = GDB_ID_NULL ; m_bEnabAxes = true ; m_AxesName.reserve( 8) ; m_AxesToken.reserve( 8) ; @@ -761,7 +765,6 @@ Simulator::ManagePathEndAux( int& nStatus) bool Simulator::ManageMove( int& nStatus) { - // Recupero posizione finale const CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( m_nEntId)) ; if ( pCamData == nullptr) { @@ -815,44 +818,73 @@ Simulator::ManageMove( int& nStatus) double dSqCoeff = (( i < 3) ? 1 : 100) ; dSqDist += dSqCoeff * ( AxesEnd[i] - m_AxesVal[i]) * ( AxesEnd[i] - m_AxesVal[i]) ; } + double dPrevCoeff = m_dCoeff ; double dDist = sqrt( dSqDist) ; if ( dDist > EPS_SMALL) { - m_dCoeff += ( nMoveType == 0 ? 4 : 1) * m_dStep / dDist ; + int nStep = int( max( dDist / ( ( nMoveType == 0 ? 4 : 1) * m_dStep), 1.)) ; + m_dCoeff += 1. / nStep ; if ( m_dCoeff > 1) m_dCoeff = 1 ; } else m_dCoeff = 1 ; + // Posizione e direzione attuali dell'utensile (per Vmill) + Point3d ptNoseI ; Vector3d vtDirI ; + bool bOkI = GetHeadCurrPosDir( ptNoseI, vtDirI) ; + // Eseguo movimento rapido o lineare if ( nMoveType != 2 && nMoveType != 3) { + // assegno posizioni finali for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i) { double dVal = m_AxesVal[i] * ( 1 - m_dCoeff) + AxesEnd[i] * m_dCoeff ; m_pMachine->SetAxisPos( m_AxesName[i], dVal) ; } + // eseguo eventuale Vmill + if ( m_nVmId != GDB_ID_NULL) { + Point3d ptNoseF ; Vector3d vtDirF ; + bool bOkF = GetHeadCurrPosDir( ptNoseF, vtDirF) ; + ExecVmillOnLine( ptNoseI, vtDirI, ptNoseF, vtDirF) ; + } } // Eseguo movimento su arco else { - // primi due assi lineari + // dati dell'arco Point3d ptCen = pCamData->GetAxesCen() ; double dAngCen = pCamData->GetAxesAngCen() ; Vector3d vtN = pCamData->GetAxesNormDir() ; - Vector3d vtRot = Point3d( m_AxesVal[0], m_AxesVal[1], m_AxesVal[2]) - ptCen ; - vtRot.Rotate( vtN, m_dCoeff * dAngCen) ; double dDeltaN = ( Point3d( AxesEnd[0], AxesEnd[1], AxesEnd[2]) - ptCen) * vtN ; - m_pMachine->SetAxisPos( m_AxesName[0], ptCen.x + vtRot.x + m_dCoeff * dDeltaN * vtN.x) ; - m_pMachine->SetAxisPos( m_AxesName[1], ptCen.y + vtRot.y + m_dCoeff * dDeltaN * vtN.y) ; - m_pMachine->SetAxisPos( m_AxesName[2], ptCen.z + vtRot.z + m_dCoeff * dDeltaN * vtN.z) ; - // altri assi - for ( size_t i = 3 ; i < m_AxesName.size() ; ++ i) { - double dVal = m_AxesVal[i] * ( 1 - m_dCoeff) + AxesEnd[i] * m_dCoeff ; - m_pMachine->SetAxisPos( m_AxesName[i], dVal) ; + double dDiffAng = ( m_dCoeff - dPrevCoeff) * dAngCen ; + Vector3d vtRot = Point3d( m_AxesVal[0], m_AxesVal[1], m_AxesVal[2]) - ptCen ; + vtRot.Rotate( vtN, dPrevCoeff * dAngCen) ; + // approssimo movimento con 1 o pių step a seconda ci sia Vmill + int nStep = 1 ; + if ( m_nVmId != GDB_ID_NULL) + nStep = int( max( abs( dDiffAng) / 15.0, 1.)) ; + for ( int i = 1 ; i <= nStep ; ++ i) { + double dCurrCoeff = dPrevCoeff + ( m_dCoeff - dPrevCoeff) / nStep * i ; + vtRot.Rotate( vtN, dDiffAng / nStep) ; + m_pMachine->SetAxisPos( m_AxesName[0], ptCen.x + vtRot.x + dCurrCoeff * dDeltaN * vtN.x) ; + m_pMachine->SetAxisPos( m_AxesName[1], ptCen.y + vtRot.y + dCurrCoeff * dDeltaN * vtN.y) ; + m_pMachine->SetAxisPos( m_AxesName[2], ptCen.z + vtRot.z + dCurrCoeff * dDeltaN * vtN.z) ; + for ( size_t j = 3 ; j < m_AxesName.size() ; ++ j) { + double dVal = m_AxesVal[j] * ( 1 - dCurrCoeff) + AxesEnd[j] * dCurrCoeff ; + m_pMachine->SetAxisPos( m_AxesName[j], dVal) ; + } + // eseguo eventuale Vmill + if ( m_nVmId != GDB_ID_NULL) { + Point3d ptNoseF ; Vector3d vtDirF ; + bool bOkF = GetHeadCurrPosDir( ptNoseF, vtDirF) ; + ExecVmillOnLine( ptNoseI, vtDirI, ptNoseF, vtDirF) ; + ptNoseI = ptNoseF ; + vtDirI = vtDirF ; + } } } } else { // Calcolo distanza di movimento - double dSqDist = 0 ; + double dSqDist = 0 ; for ( size_t i = 0 ; i < m_AuxAxesName.size() ; ++ i) { // coefficiente moltiplicativo per differenziare assi lineari e rotanti double dSqCoeff = ( m_AuxAxesLinear[i] ? 1 : 100) ; @@ -860,7 +892,8 @@ Simulator::ManageMove( int& nStatus) } double dDist = sqrt( dSqDist) ; if ( dDist > EPS_SMALL) { - m_dCoeff += ( nMoveType == 0 ? 4 : 1) * m_dStep / dDist ; + int nStep = int( max( dDist / ( ( nMoveType == 0 ? 4 : 1) * m_dStep), 1.)) ; + m_dCoeff += 1. / nStep ; if ( m_dCoeff > 1) m_dCoeff = 1 ; } @@ -894,6 +927,46 @@ Simulator::ManageMove( int& nStatus) return true ; } +//---------------------------------------------------------------------------- +bool +Simulator::GetHeadCurrPosDir( Point3d& ptH, Vector3d& vtH) +{ + // ci devono essere almeno i tre assi lineari + if ( m_AxesName.size() < 3) + return false ; + // recupero le posizioni degli assi lineari + DBLVECTOR vLinAx( 3) ; + for ( size_t i = 0 ; i < 3 ; ++ i) + m_pMachine->GetAxisPos( m_AxesName[i], vLinAx[i]) ; + // recupero le posizioni degli eventuali assi rotanti + DBLVECTOR vRotAx( m_AxesName.size() - 3) ; + for ( size_t i = 3 ; i < m_AxesName.size() ; ++ i) + m_pMachine->GetAxisPos( m_AxesName[i], vRotAx[i-3]) ; + // determino posizione e orientamento della testa + m_pMachine->GetNoseFromPositions( vLinAx[0], vLinAx[1], vLinAx[2], vRotAx, ptH) ; + m_pMachine->GetToolDirFromAngles( vRotAx, vtH) ; + return true ; +} + +//---------------------------------------------------------------------------- +bool +Simulator::ExecVmillOnLine( const Point3d& ptHi, const Vector3d& vtHi, const Point3d& ptHf, const Vector3d& vtHf) +{ + // Recupero Zmap + IVolZmap* pVZM = GetVolZmap( m_pGeomDB->GetGeoObj( m_nVmId)) ; + if ( pVZM == nullptr) + return false ; + // Porto gli estremi nel riferimento dello Zmap + Frame3d frVZM ; + m_pGeomDB->GetGlobFrame( m_nVmId, frVZM) ; + Point3d ptHiL = ptHi ; ptHiL.ToLoc( frVZM) ; + Vector3d vtHiL = vtHi ; vtHiL.ToLoc( frVZM) ; + Point3d ptHfL = ptHf ; ptHfL.ToLoc( frVZM) ; + Vector3d vtHfL = vtHf ; vtHfL.ToLoc( frVZM) ; + // Eseguo + return pVZM->MillingStep( ptHiL, vtHiL, ptHfL, vtHfL) ; +} + //---------------------------------------------------------------------------- bool Simulator::OnDispositionStarting( int nOpId, int nOpInd, int nPhase, @@ -957,6 +1030,10 @@ Simulator::OnDispositionEnd( void) return true ; // chiamo la funzione di fine disposizione bool bOk = m_pMachine->LuaCallFunction( ON_SIMUL_DISPOSITION_END) ; + // recupero i dati di ritorno + int nVmId = GDB_ID_NULL ; + m_pMachine->LuaGetGlobVar( GLOB_VAR + GVAR_VMILL, nVmId) ; + m_nVmId = nVmId ; // forzo aggiornamento posizione assi (possono essere stati mossi nello script) UpdateAxesPos() ; return bOk ; diff --git a/Simulator.h b/Simulator.h index 0499166..ecdc4a6 100644 --- a/Simulator.h +++ b/Simulator.h @@ -53,6 +53,8 @@ class Simulator bool ManagePathStartAux( int& nStatus) ; bool ManagePathEndAux( int& nStatus) ; bool ManageMove( int& nStatus) ; + bool GetHeadCurrPosDir( Point3d& ptH, Vector3d& vtH) ; + bool ExecVmillOnLine( const Point3d& ptHi, const Vector3d& vtHi, const Point3d& ptHf, const Vector3d& vtHf) ; bool OnDispositionStarting( int nOpId, int nOpInd, int nPhase, const std::string& sTable, const Point3d& ptOri1, bool bEmpty, bool bSomeByHand) ; bool OnDispositionStart( int nOpId, int nOpInd, int nPhase, @@ -88,6 +90,7 @@ class Simulator int m_nAuxETot ; // numero totale movimenti ausiliari di fine percorso int m_nAuxEInd ; // indice del movimento ausiliario di fine percorso corrente std::string m_sTool ; // nome dell'utensile corrente + int m_nVmId ; // identificativo dell'oggetto Zmap per Virtual Milling bool m_bEnabAxes ; // flag abilitazione movimento assi attivi STRVECTOR m_AxesName ; // nomi degli assi macchina attivi STRVECTOR m_AxesToken ; // token degli assi macchina attivi