diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index e6d808f..5156166 100644 Binary files a/EgtMachKernel.rc and b/EgtMachKernel.rc differ diff --git a/MachineCalc.cpp b/MachineCalc.cpp index c64028d..167f998 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -490,10 +490,17 @@ Machine::CalculateKinematicChain( void) nHParId = m_pGeomDB->GetParentId( nHParId) ; } + // se non ci sono assi, né lineari né rotanti, sicuramente errore + if ( m_nTabLinAxes == 0 && m_nHeadLinAxes == 0 && + m_nTabRotAxes == 0 && m_nHeadRotAxes == 0) { + LOG_ERROR( GetEMkLogger(), "Errors in Axes : none have been found") + return false ; + } + // se nessun asse lineare deve essere un robot (in futuro va permesso un lineare di tavola) if ( m_nTabLinAxes == 0 && m_nHeadLinAxes == 0) { // verifico ci siano 6 assi rotanti tutti di testa (in futuro va permesso un rotante di tavola) - if ( m_nTabRotAxes != 0 && m_nHeadRotAxes != 6) { + if ( m_nTabRotAxes != 0 || m_nHeadRotAxes != 6) { LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : number or type") return false ; } @@ -501,19 +508,20 @@ Machine::CalculateKinematicChain( void) swap( m_vCalcRotAx[0], m_vCalcRotAx[5]) ; swap( m_vCalcRotAx[1], m_vCalcRotAx[4]) ; swap( m_vCalcRotAx[2], m_vCalcRotAx[3]) ; - // direzione assi rotanti deve essere Z Y Y Z Y Z + // direzione assi rotanti deve essere Z Y Y X Y X if ( ! m_vCalcRotAx[0].vtDir.IsZ() || ! m_vCalcRotAx[1].vtDir.IsY() || ! m_vCalcRotAx[2].vtDir.IsY() || - ! m_vCalcRotAx[3].vtDir.IsZ() || + ! m_vCalcRotAx[3].vtDir.IsX() || ! m_vCalcRotAx[4].vtDir.IsY() || - ! m_vCalcRotAx[5].vtDir.IsZ()) { - LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-ZYZ") + ! m_vCalcRotAx[5].vtDir.IsX()) { + LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-XYX") return false ; } // verifico che gli ultimi 3 assi formino un polso sferico (ovvero passino per uno stesso punto) - if ( ! AreSamePointXYApprox( m_vCalcRotAx[3].ptPos, m_vCalcRotAx[5].ptPos) || - abs( m_vCalcRotAx[4].ptPos.x - m_vCalcRotAx[3].ptPos.x) > EPS_SMALL) { + if ( abs( m_vCalcRotAx[3].ptPos.y - m_vCalcRotAx[5].ptPos.y) > EPS_SMALL || + abs( m_vCalcRotAx[3].ptPos.z - m_vCalcRotAx[5].ptPos.z) > EPS_SMALL || + abs( m_vCalcRotAx[4].ptPos.z - m_vCalcRotAx[3].ptPos.z) > EPS_SMALL) { LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not spherical Wrist") return false ; } @@ -2045,7 +2053,7 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto double dDistR2R3 = m_vCalcRotAx[2].ptPos.z - m_vCalcRotAx[1].ptPos.z ; Vector3d vtR3R5 = m_vCalcRotAx[4].ptPos - m_vCalcRotAx[2].ptPos ; vtR3R5.y = 0 ; double dDistR3R5 = vtR3R5.Len() ; - double dAngR3R5 = atan2( -vtR3R5.x, vtR3R5.z) * RADTODEG ; + double dAngR3R5 = atan2( vtR3R5.z, vtR3R5.x) * RADTODEG ; double dCosB = ( dDistR2R3 * dDistR2R3 + dDistR2Cen * dDistR2Cen - dDistR3R5 * dDistR3R5) / ( 2 * dDistR2R3 * dDistR2Cen) ; double dCosC = ( dDistR3R5 * dDistR3R5 + dDistR2Cen * dDistR2Cen - dDistR2R3 * dDistR2R3) / ( 2 * dDistR3R5 * dDistR2Cen) ; if ( abs( dCosB) > 1 || abs( dCosC) > 1) { @@ -2062,11 +2070,12 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto vAng1.push_back( dAng2) ; vAng2.push_back( dAng2) ; double dAngC = acos( dCosC) * RADTODEG ; - double dAng3 = dAngB + dAngC + dAngR3R5 ; + double dAng3 = dAngB + dAngC + dAngR3R5 - ANG_RIGHT ; vAng1.push_back( dAng3) ; vAng2.push_back( dAng3) ; // calcolo i primi due assi rotanti del polso - Frame3d frWrist( m_vCalcRotAx[3].ptPos) ; + Frame3d frWrist ; + frWrist.Set( m_vCalcRotAx[3].ptPos, m_vCalcRotAx[3].vtDir, m_vCalcRotAx[4].vtDir ^ m_vCalcRotAx[3].vtDir) ; frWrist.Rotate( m_vCalcRotAx[2].ptPos, m_vCalcRotAx[2].vtDir, dAng3) ; frWrist.Rotate( m_vCalcRotAx[1].ptPos, m_vCalcRotAx[1].vtDir, dAng2) ; frWrist.Rotate( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, dAng1) ; diff --git a/Operation.cpp b/Operation.cpp index 69ef9fb..31048c0 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -4175,7 +4175,7 @@ Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEn // traslo il riferimento per avere il cilindro centrato sull'asse Y in positivo frHsol.Translate( frHsol.VersZ() * ptMin.z) ; // verifica di collisione tra cilindro e solido - if ( pStm == nullptr || CDeCylClosedSurfTm( frHsol, max( vtDiag.x, vtDiag.y) / 2, vtDiag.z, dSafeDist, *pStm)) { + if ( pStm == nullptr || CDeCylClosedSurfTm( frHsol, max( vtDiag.x, vtDiag.y) / 2, vtDiag.z, *pStm, dSafeDist)) { bCollide = true ; break ; } @@ -4185,7 +4185,7 @@ Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEn // traslo il riferimento per avere il box nel primo ottante frHsol.Translate( frHsol.VersX() * ptMin.x + frHsol.VersY() * ptMin.y + frHsol.VersZ() * ptMin.z) ; // verifica di collisione tra box e solido - if ( pStm == nullptr || CDeBoxClosedSurfTm( frHsol, vtDiag, dSafeDist, *pStm)) { + if ( pStm == nullptr || CDeBoxClosedSurfTm( frHsol, vtDiag, *pStm, dSafeDist)) { bCollide = true ; break ; } diff --git a/OutputConst.h b/OutputConst.h index 468f5ef..d0e27b0 100644 --- a/OutputConst.h +++ b/OutputConst.h @@ -206,6 +206,8 @@ static const std::string ON_ESTIM_RAPID = "OnEstimRapid" ; static const std::string ON_ESTIM_LINEAR = "OnEstimLinear" ; static const std::string ON_ESTIM_ARC = "OnEstimArc" ; // Funzioni simulazione +static const std::string ON_SIMUL_INIT = "OnSimulInit" ; +static const std::string ON_SIMUL_EXIT = "OnSimulExit" ; static const std::string ON_SIMUL_START = "OnSimulStart" ; static const std::string ON_SIMUL_END = "OnSimulEnd" ; static const std::string ON_SIMUL_DISPOSITION_STARTING = "OnSimulDispositionStarting" ; diff --git a/Simulator.cpp b/Simulator.cpp index 0e5d872..dbb0cb6 100644 --- a/Simulator.cpp +++ b/Simulator.cpp @@ -61,6 +61,7 @@ Simulator::Simulator( void) m_pGeomDB = nullptr ; m_pMachine = nullptr ; m_pPerfCnt = nullptr ; + m_nStatus = SIS_CREATED ; m_dStep = MID_STEP ; m_nUiStatus = MCH_UISIM_NULL ; m_nOpId = GDB_ID_NULL ; @@ -92,6 +93,8 @@ Simulator::~Simulator( void) { // porto la macchina in posizione home GoHome() ; + // gestione evento uscita dal simulatore + OnExit() ; // rimuovo tavola variabili globali m_pMachine->LuaResetGlobVar( GLOB_VAR) ; // rimuovo performance counter @@ -113,6 +116,7 @@ Simulator::Init( MachMgr* pMchMgr) m_pGeomDB = m_pMchMgr->GetGeomDB() ; m_pMachine = m_pMchMgr->GetCurrMachine() ; m_pPerfCnt = new PerformanceCounter ; + m_nStatus = SIS_INITIALIZED ; return true ; } @@ -129,8 +133,8 @@ Simulator::Start( bool bFirst) bool bOk = true ; - // Se avvio vero - if ( bFirst) { + // Se appena entrati in simulazione + if ( m_nStatus == SIS_INITIALIZED) { // Forzo aggiornamento attrezzaggio della macchinata if ( ! m_pMchMgr->UpdateCurrSetup()) bOk = false ; @@ -167,8 +171,15 @@ Simulator::Start( bool bFirst) m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_VER, GetEMkVer()) ; m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_MACHNAME, m_pMachine->GetMachineName()) ; - // Richiamo funzione su avvio simulazione - if ( ! OnStart( bFirst)) + // Se appena entrati in simulazione + if ( m_nStatus == SIS_INITIALIZED) { + if ( ! OnInit()) + bOk = false ; + m_nStatus = SIS_READYTOSTART ; + } + + // Richiamo funzione per evento avvio simulazione + if ( ! OnProgramStart( bFirst)) bOk = false ; // Arrivo alla preparazione per la prima lavorazione @@ -193,6 +204,9 @@ Simulator::Start( bool bFirst) if ( m_pPerfCnt != nullptr) m_pPerfCnt->Start() ; + // Imposto lo stato interno + m_nStatus = ( bOk ? SIS_READYTORUN : SIS_READYTOSTART) ; + return bOk ; } @@ -248,7 +262,7 @@ bool Simulator::Move( int& nStatus) { // Verifiche - if ( m_pGeomDB == nullptr || m_pMchMgr == nullptr || m_pMachine == nullptr) { + if ( m_pGeomDB == nullptr || m_pMchMgr == nullptr || m_pMachine == nullptr || m_nStatus != SIS_READYTORUN) { nStatus = MCH_SIM_ERR ; return false ; } @@ -292,7 +306,7 @@ Simulator::Move( int& nStatus) return false ; // se non ce ne sono altre, sono alla fine if ( m_nOpId == GDB_ID_NULL) { - OnEnd() ; + OnProgramEnd() ; nStatus = MCH_SIM_END ; return false ; } @@ -1425,27 +1439,27 @@ Simulator::ExecCollisionCheck( int& nCdInd, int& nObjInd, int nMoveType) if ( m_CollObj[j].nType == MCH_SIM_COB_BOX) { Vector3d vtDiag( m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3) ; if ( pVZM != nullptr) - bOk = pVZM->AvoidBox( frObj, vtDiag, m_dSafeDist) ; + bOk = ! pVZM->CDeBox( frObj, vtDiag, m_dSafeDist) ; else - bOk = ! CDeBoxClosedSurfTm( frObj, vtDiag, m_dSafeDist, *pSTM) ; + bOk = ! CDeBoxClosedSurfTm( frObj, vtDiag, *pSTM, m_dSafeDist) ; } else if ( m_CollObj[j].nType == MCH_SIM_COB_CYL) { if ( pVZM != nullptr) - bOk = pVZM->AvoidCylinder( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist) ; + bOk = ! pVZM->CDeCylinder( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist) ; else - bOk = ! CDeCylClosedSurfTm( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist, *pSTM) ; + bOk = ! CDeCylClosedSurfTm( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, *pSTM, m_dSafeDist) ; } else if ( m_CollObj[j].nType == MCH_SIM_COB_SPHE) { if ( pVZM != nullptr) - bOk = pVZM->AvoidSphere( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist) ; + bOk = ! pVZM->CDeSphere( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist) ; else - bOk = ! CDeSpheClosedSurfTm( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist, *pSTM) ; + bOk = ! CDeSpheClosedSurfTm( frObj.Orig(), m_CollObj[j].dPar1, *pSTM, m_dSafeDist) ; } else if ( m_CollObj[j].nType == MCH_SIM_COB_CONE) { if ( pVZM != nullptr) - bOk = pVZM->AvoidConeFrustum( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3, m_dSafeDist) ; + bOk = ! pVZM->CDeConeFrustum( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3, m_dSafeDist) ; else - bOk = ! CDeConeFrustumClosedSurfTm( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3, m_dSafeDist, *pSTM) ; + bOk = ! CDeConeFrustumClosedSurfTm( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3, *pSTM, m_dSafeDist) ; } } // altrimenti poliedro @@ -1454,7 +1468,7 @@ Simulator::ExecCollisionCheck( int& nCdInd, int& nObjInd, int nMoveType) const ISurfTriMesh* pStmLoc = GetSurfTriMesh( SurfLoc) ; if ( pStmLoc != nullptr) { if ( pVZM != nullptr) - bOk = pVZM->AvoidSurfTm( *pStmLoc, m_dSafeDist) ; + bOk = ! pVZM->CDeSurfTm( *pStmLoc, m_dSafeDist) ; else bOk = ! CDeClosedSurfTmClosedSurfTm( *pStmLoc, *pSTM, m_dSafeDist) ; } @@ -1474,7 +1488,29 @@ Simulator::ExecCollisionCheck( int& nCdInd, int& nObjInd, int nMoveType) //---------------------------------------------------------------------------- bool -Simulator::OnStart( bool bFirst) +Simulator::OnInit( void) +{ + // verifico esistenza funzione + if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_INIT)) + return true ; + // chiamo la funzione di ingresso nella simulazione + return m_pMachine->LuaCallFunction( ON_SIMUL_INIT) ; +} + +//---------------------------------------------------------------------------- +bool +Simulator::OnExit( void) +{ + // verifico esistenza funzione + if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_EXIT)) + return true ; + // chiamo la funzione di uscita dalla simulazione + return m_pMachine->LuaCallFunction( ON_SIMUL_EXIT) ; +} + +//---------------------------------------------------------------------------- +bool +Simulator::OnProgramStart( bool bFirst) { // assegno flag inizio simulazione bool bOk = m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_SIM1ST, bFirst) ; @@ -1488,7 +1524,7 @@ Simulator::OnStart( bool bFirst) //---------------------------------------------------------------------------- bool -Simulator::OnEnd( void) +Simulator::OnProgramEnd( void) { // verifico esistenza funzione if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_END)) diff --git a/Simulator.h b/Simulator.h index 3c340c6..a168e26 100644 --- a/Simulator.h +++ b/Simulator.h @@ -86,8 +86,10 @@ class Simulator { return ( ! m_CollObj.empty() && ! m_CdId.empty()) ; } bool Stopped( void) { return ( m_nUiStatus == MCH_UISIM_STOP) ; } - bool OnStart( bool bFirst) ; - bool OnEnd( void) ; + bool OnInit( void) ; + bool OnExit( void) ; + bool OnProgramStart( bool bFirst) ; + bool OnProgramEnd( void) ; 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, @@ -135,12 +137,17 @@ class Simulator : sName( sN), sHead( sH), nExit( nE), dTdOffs( dT), dAdOffs( dA) {} } ; typedef std::vector VMTVECTOR ; + enum { SIS_CREATED = 0, + SIS_INITIALIZED = 1, + SIS_READYTOSTART = 2, + SIS_READYTORUN = 3} ; private : MachMgr* m_pMchMgr ; // puntatore al gestore di tutte le lavorazioni IGeomDB* m_pGeomDB ; // puntatore al DB geometrico Machine* m_pMachine ; // puntatore alla macchina PerformanceCounter* m_pPerfCnt ; // timer per calcolo FPS + int m_nStatus ; // stato interno del simulatore (creato, inizializzato, pronto al movimento) double m_dStep ; // lunghezza di riferimento per la velocità di simulazione int m_nUiStatus ; // stato simulazione a livello utente int m_nOpId ; // identificativo della operazione (lavoraz.) corrente