diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index 1a61810..0db982c 100644 Binary files a/EgtMachKernel.rc and b/EgtMachKernel.rc differ diff --git a/MachMgr.h b/MachMgr.h index 2bba79b..a5801ed 100644 --- a/MachMgr.h +++ b/MachMgr.h @@ -461,7 +461,7 @@ class MachMgr : public IMachMgr bool GetCurrAxisMax( int nInd, double& dHome) const ; bool GetCurrAxisMin( int nInd, double& dHome) const ; const Frame3d& GetCurrLinAxesFrame( void) const ; - bool GetCurrIsCenter( void) const ; + bool GetCurrIsMcent( void) const ; bool GetCurrIsRobot( void) const ; bool ApplyRotAxisBlock( void) ; void ClearRotAxisBlock( void) diff --git a/MachMgrMachines.cpp b/MachMgrMachines.cpp index 547a13b..3e96000 100644 --- a/MachMgrMachines.cpp +++ b/MachMgrMachines.cpp @@ -741,12 +741,12 @@ MachMgr::GetCurrLinAxesFrame( void) const //---------------------------------------------------------------------------- bool -MachMgr::GetCurrIsCenter( void) const +MachMgr::GetCurrIsMcent( void) const { Machine* pMch = GetCurrMachine() ; if ( pMch == nullptr) return false ; - return ( pMch->GetCurrKinematicChainType() == KIN_CHAIN_CENTER) ; + return ( pMch->GetCurrKinematicChainType() == KIN_CHAIN_MCENT) ; } //---------------------------------------------------------------------------- diff --git a/MachineCalc.cpp b/MachineCalc.cpp index 990152b..07f57a6 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -729,7 +729,7 @@ Machine::CalculateKinematicChain( void) } } // dichiaro tipo centro di lavoro - m_nCalcChainType = KIN_CHAIN_CENTER ; + m_nCalcChainType = KIN_CHAIN_MCENT ; return true ; } @@ -1348,7 +1348,7 @@ Machine::GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& return false ; // se centro di lavoro - if ( m_nCalcChainType == KIN_CHAIN_CENTER) { + if ( m_nCalcChainType == KIN_CHAIN_MCENT) { // 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) { @@ -1401,7 +1401,7 @@ Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& return false ; // se centro di lavoro - if ( m_nCalcChainType == KIN_CHAIN_CENTER) { + if ( m_nCalcChainType == KIN_CHAIN_MCENT) { // 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) { diff --git a/MachineStruConst.h b/MachineStruConst.h index 133748b..3e5808c 100644 --- a/MachineStruConst.h +++ b/MachineStruConst.h @@ -104,7 +104,7 @@ enum MchSelType { MCH_SLT_FIXEDEXITS = 0, //---------------------------------------------------------------------------- // Tipo della catena cinematica enum KinChainType { KIN_CHAIN_NONE = 0, - KIN_CHAIN_CENTER = 1, + KIN_CHAIN_MCENT = 1, KIN_CHAIN_ROBOT = 2} ; //---------------------------------------------------------------------------- diff --git a/Operation.cpp b/Operation.cpp index a1dc95a..86bc624 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -1916,11 +1916,11 @@ Operation::CalculateAxesValues( const string& sHint, bool bRotContOnNext, bool b // calcolo il valore degli assi macchina di tutti i movimenti bool bOk = true ; - if ( m_pMchMgr->GetCurrIsCenter()) { + if ( m_pMchMgr->GetCurrIsMcent()) { int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ; while ( nClPathId != GDB_ID_NULL) { - if ( ! CalculateClPathCenterAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, - bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec)) + if ( ! CalculateClPathMcentAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, + bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec)) bOk = false ; nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ; } @@ -1940,25 +1940,25 @@ Operation::CalculateAxesValues( const string& sHint, bool bRotContOnNext, bool b //---------------------------------------------------------------------------- bool -Operation::CalculateClPathCenterAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, - bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, - const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) +Operation::CalculateClPathMcentAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, + bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) { DBLVECTOR vAxRotPrecOri = vAxRotPrec ; // cancello eventuali punti aggiunti EraseAddedPoints( nClPathId) ; // calcolo int nOutStrC = 0 ; - if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, - bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, nOutStrC)) { + if ( ! MyCalculateClPathMcentAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, + bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, nOutStrC)) { // se attivata scelta angolo iniziale vicino ad home ed extra corsa C, provo a rifarlo disattivando if ( dAngDeltaMinForHome < ANG_FULL && nOutStrC != 0) { // cancello eventuali punti aggiunti EraseAddedPoints( nClPathId) ; // ricalcolo m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ; - if ( CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, - bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC)) + if ( MyCalculateClPathMcentAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, + bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC)) return true ; } // se extracorsa dell'asse C, provo a precaricarlo al contrario @@ -1969,8 +1969,8 @@ Operation::CalculateClPathCenterAxesValues( int nClPathId, int nLinAxes, int nRo vAxRotPrec = vAxRotPrecOri ; vAxRotPrec[0] = vAxRotPrecOri[0] + ( nOutStrC > 0 ? - ANG_FULL : ANG_FULL) ; m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ; - if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, - bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC)) + if ( ! MyCalculateClPathMcentAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, + bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC)) return false ; } else @@ -1999,9 +1999,9 @@ Operation::EraseAddedPoints( int nClPathId) //---------------------------------------------------------------------------- bool -Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, - bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, - const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC) +Operation::MyCalculateClPathMcentAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, + bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC) { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; @@ -2039,8 +2039,8 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, Vector3d vtAux = pCamData->GetAuxDir() ; Vector3d vtCorr = pCamData->GetCorrDir() ; DBLVECTOR vAxRot ; - if ( ! CalculateRotAxesValues( bFirst, vtDir, vtAux, dRot1W, - bMaxDeltaR2OnFirst, bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, vAxRot)) { + if ( ! CalculateMcentRotAxesValues( bFirst, vtDir, vtAux, dRot1W, + bMaxDeltaR2OnFirst, bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, vAxRot)) { pCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ; LOG_ERROR( GetEMkLogger(), "Error : tool direction unreachable") return false ; @@ -2112,10 +2112,10 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, bMidAdded = true ; else { bool bAxesError ; - if ( ! VerifyLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec, - ptP, vtDir, vtAux, vtCorr, vAxVal, - 1, nEntId, dRot1W, nMoveType, bToolShowPrec && pCamData->GetToolShow(), - bMidAdded, bAxesError)) { + if ( ! VerifyMcentLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec, + ptP, vtDir, vtAux, vtCorr, vAxVal, + 1, nEntId, dRot1W, nMoveType, bToolShowPrec && pCamData->GetToolShow(), + bMidAdded, bAxesError)) { string sOut = "Error : VerifyLineMidPoint of CalculateClPathAxesValues failed (EntId=" + ToString( nEntId) + ")" ; LOG_ERROR( GetEMkLogger(), sOut.c_str()) return false ; @@ -2277,9 +2277,9 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, //---------------------------------------------------------------------------- bool -Operation::CalculateRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux, - double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, - const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) +Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux, + double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) { // massima variazione secondo asse rotante per continuità const double dMaxDeltaR2 = 30 ; @@ -2427,9 +2427,9 @@ Operation::CalculateRotAxesValues( bool bFirst, const Vector3d& vtTool, const Ve //---------------------------------------------------------------------------- bool -Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec, - const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal, - int nCnt, int nEntId, double dRot1W, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError) +Operation::VerifyMcentLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec, + const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal, + int nCnt, int nEntId, double dRot1W, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError) { // impostazioni bAdded = false ; @@ -2470,13 +2470,20 @@ Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, double dDist ; if ( ! DistPointLine( ptMoveMid, ptPrec, ptP).GetDist( dDist)) return false ; - // LOG_INFO( GetEMkLogger(), ( "Midpoint dist =" + ToString( dDist, 3)).c_str()) double dLinTol = GetApproxLinTol() ; if ( nMoveType == 0) dLinTol = max( 10 * dLinTol, LIN_TOL_RAW) ; + else + dLinTol = max( dLinTol, 2 * EPS_SMALL) ; + bool bInTol = ( dDist <= dLinTol) ; + if ( ExeGetDebugLevel() >= 5) { + string sOut = "Midpoint dist = " + ToString( dDist, -4) + + ( nMoveType == 0 ? " (G0" : " (G1") + " Cnt=" + ToString( nCnt) + ") " + ( bInTol ? "Ok" : "Split") ; + LOG_INFO( GetEMkLogger(), sOut.c_str()) + } // Se posizione in tolleranza, non devo fare alcunché - if ( dDist <= dLinTol) + if ( bInTol) return true ; // Va inserito il punto medio @@ -2510,7 +2517,7 @@ Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, if ( IsNull( pGP)) return false ; // assegno le coordinate del punto - pGP->Set( ptP) ; + pGP->Set( ptMid) ; // inserisco l'oggetto nel DB geometrico int nMidEntId = m_pGeomDB->InsertGeoObj( GDB_ID_NULL, nEntId, GDB_BEFORE, Release( pGP)) ; if ( nMidEntId == GDB_ID_NULL) @@ -2539,7 +2546,7 @@ Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, DBLVECTOR vAxRotPrec( int( vAxVal.size() - 3)) ; for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i) vAxRotPrec[i-3] = vAxVal[i] ; - if ( ! CalculateRotAxesValues(false, vtDirMid, vtAuxMid, dRot1W, false, true, INFINITO, vAxRotHome, vAxRotPrec, vAxRotMid)) { + if ( ! CalculateMcentRotAxesValues( false, vtDirMid, vtAuxMid, dRot1W, false, true, INFINITO, vAxRotHome, vAxRotPrec, vAxRotMid)) { pMidCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ; return false ; } @@ -2574,17 +2581,17 @@ Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, // devo verificare le due nuove parti ++ nCnt ; bool bAdded1, bAxError1, bAdded2, bAxError2 ; - if ( ! VerifyLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec, - ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid, - nCnt, nMidEntId, dRot1W, nMoveType, bToolShow, bAdded1, bAxError1)) + if ( ! VerifyMcentLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec, + ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid, + nCnt, nMidEntId, dRot1W, nMoveType, bToolShow, bAdded1, bAxError1)) return false ; if ( bAxError1) { bAxError = true ; return true ; } - if ( ! VerifyLineMidPoint( ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid, - ptP, vtDir, vtAux, vtCorr, vAxVal, - nCnt, nEntId, dRot1W, nMoveType, bToolShow, bAdded2, bAxError2)) + if ( ! VerifyMcentLineMidPoint( ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid, + ptP, vtDir, vtAux, vtCorr, vAxVal, + nCnt, nEntId, dRot1W, nMoveType, bToolShow, bAdded2, bAxError2)) return false ; if ( bAxError2) { bAxError = true ; @@ -2602,6 +2609,8 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; + // cancello eventuali punti aggiunti + EraseAddedPoints( nClPathId) ; // eventuale direzione Aux preferenziale bool bUseRefAux = true ; Vector3d vtRefAux = Y_AX ; @@ -2614,6 +2623,13 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot case MCH_SCC_ADIR_ZM : vtRefAux = -Z_AX ; break ; default : bUseRefAux = false ; break ; } + // valori del precedente + Point3d ptPrec ; + Vector3d vtDirPrec ; + Vector3d vtAuxPrec ; + Vector3d vtCorrPrec ; + int nFlag2Prec = 0 ; + bool bToolShowPrec = true ; // predispongo variabile per valori assi DBLVECTOR vAxVal ; vAxVal.reserve( 8) ; @@ -2636,69 +2652,30 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot // ricavo posizione con eventuali modifiche dipendenti dalla lavorazione Point3d ptP = pCamData->GetEndPoint() ; AdjustEndPointForAxesCalc( pCamData, ptP) ; - // calcolo gli assi del robot - Vector3d vtTool = pCamData->GetToolDir() ; + // recupero direzioni + Vector3d vtDir = pCamData->GetToolDir() ; Vector3d vtAux ; if ( ! bUseRefAux) { Frame3d frTool ; - frTool.Set( ORIG, vtTool) ; + frTool.Set( ORIG, vtDir) ; frTool.Rotate( frTool.Orig(), frTool.VersZ(), 0, 1) ; vtAux = frTool.VersX() ; } else { - vtAux = OrthoCompo( vtRefAux, vtTool) ; + vtAux = OrthoCompo( vtRefAux, vtDir) ; vtAux.Normalize() ; } pCamData->SetAuxDir( vtAux) ; - DBLVECTOR vAng1, vAng2 ; - bool bRaOk = m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptP, vtTool, vtAux, vAng1, vAng2) ; - if ( ! bRaOk) { + Vector3d vtCorr = pCamData->GetCorrDir() ; + // calcolo gli assi del robot + if ( ! CalculateRobotAxesValues( bFirst, ptP, vtDir, vtAux, + bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, vAxVal)) { pCamData->SetAxes( CamData::AS_ERR, vAxVal) ; return false ; } - // se inizio - if ( bFirst) { - // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; - if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; - } - for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) { - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; - if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; - } - bFirst = false ; - } - // altrimenti movimenti successivi - else { - // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - if ( bRotContOnNext) - vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; - else - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; - } - for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) { - if ( bRotContOnNext) - vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ; - else - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; - } - } - // Scelgo la soluzione più vicina - // minimizzo rotazione polso - if ( vAng2.empty() || abs( vAng1[3] - vAxRotPrec[3]) < abs( vAng2[3] - vAxRotPrec[3]) + 10 * EPS_ANG_SMALL) - vAxVal = vAng1 ; - else - vAxVal = vAng2 ; - // controllo continuità di R6 dalla posizione precedente - m_pMchMgr->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ; // verifico di essere nelle corse degli assi int nStat ; bool bOsOk = m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAxVal, false, nStat) ; - // salvo il risultato if ( ! bOsOk || nStat != 0) { bOk = false ; pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ; @@ -2706,13 +2683,256 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot else { pCamData->SetAxes( CamData::AS_OK, vAxVal) ; } + // tipo di movimento + int nMoveType = pCamData->GetMoveType() ; + // se non è l'inizio, verifico il punto medio + if ( ! bFirst) { + // Verifica ricorsiva del punto medio (se non già fatta) + bool bMidAdded = false ; + if ( nFlag2Prec == -1 || pCamData->GetFlag2() == -1) + bMidAdded = true ; + else { + bool bAxesError ; + if ( ! VerifyRobotLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxRotPrec, + ptP, vtDir, vtAux, vtCorr, vAxVal, + 1, nEntId, nMoveType, bToolShowPrec && pCamData->GetToolShow(), bMidAdded, bAxesError)) { + string sOut = "Error : VerifyLineMidPoint of CalculateClPathAxesValues failed (EntId=" + ToString( nEntId) + ")" ; + LOG_ERROR( GetEMkLogger(), sOut.c_str()) + return false ; + } + if ( bAxesError) { + bOk = false ; + continue ; + } + } + } // aggiorno valori precedenti + bFirst = false ; + ptPrec = ptP ; + vtDirPrec = vtDir ; + vtAuxPrec = vtAux ; + vtCorrPrec = vtCorr ; vAxRotPrec = vAxVal ; + nFlag2Prec = pCamData->GetFlag2() ; + bToolShowPrec = pCamData->GetToolShow() ; } return bOk ; } +//---------------------------------------------------------------------------- +bool +Operation::CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vector3d& vtTool, const Vector3d& vtAux, + bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxVal) +{ + // calcolo angoli dei giunti + DBLVECTOR vAng1, vAng2 ; + bool bRaOk = m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptP, vtTool, vtAux, vAng1, vAng2) ; + if ( ! bRaOk) + return false ; + // se inizio + if ( bFirst) { + // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + } + for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) { + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; + if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; + } + bFirst = false ; + } + // altrimenti movimenti successivi + else { + // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + if ( bRotContOnNext) + vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; + else + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + } + for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) { + if ( bRotContOnNext) + vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ; + else + m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; + } + } + // Scelgo la soluzione più vicina + // minimizzo rotazione polso + if ( vAng2.empty() || abs( vAng1[3] - vAxRotPrec[3]) < abs( vAng2[3] - vAxRotPrec[3]) + 10 * EPS_ANG_SMALL) + vAxVal = vAng1 ; + else + vAxVal = vAng2 ; + // controllo continuità di R6 dalla posizione precedente + m_pMchMgr->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ; + return true ; +} + +//---------------------------------------------------------------------------- +bool +Operation::VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec, + const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal, + int nCnt, int nEntId, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError) +{ + // impostazioni + bAdded = false ; + bAxError = false ; + Machine* pMachine = m_pMchMgr->GetCurrMachine() ; + + // se disposizione non vanno fatti controlli + if ( GetType() == OPER_DISP) + return true ; + + // se superato il limite di ricursioni, non devo fare alcunché + const int MAX_RECURSION = 8 ; + if ( nCnt > MAX_RECURSION) + return true ; + + // calcolo variazione assi + double dDeltaAng = 0 ; + for ( int i = 0 ; i < int( vAxVal.size()) ; ++i) + dDeltaAng += abs( vAxVal[i] - vAxPrec[i]) ; + + // Se non cambia la direzione utensile, non devo fare alcunché + const double MAX_DELTA_ANG = 0.01 ; + if ( dDeltaAng <= MAX_DELTA_ANG) + return true ; + + // Determino i valori degli assi al punto medio + DBLVECTOR vAxMid( vAxVal.size()) ; + for ( int i = 0 ; i < int( vAxVal.size()) ; ++ i) + vAxMid[i] = ( vAxPrec[i] + vAxVal[i]) / 2 ; + // ricavo il punto medio del movimento + Point3d ptMoveMid ; + if ( ! pMachine->GetTipFromPositions( 0, 0, 0, vAxMid, false, false, true, ptMoveMid)) + return false ; + // verifico le differenze + double dDist ; + if ( ! DistPointLine( ptMoveMid, ptPrec, ptP).GetDist( dDist)) + return false ; + double dLinTol = GetApproxLinTol() ; + if ( nMoveType == 0) + dLinTol = max( 10 * dLinTol, LIN_TOL_RAW) ; + else + dLinTol = max( dLinTol, 2 * EPS_SMALL) ; + bool bInTol = ( dDist <= dLinTol) ; + if ( ExeGetDebugLevel() >= 5) { + string sOut = "Midpoint dist = " + ToString( dDist, -4) + + ( nMoveType == 0 ? " (G0" : " (G1") + " Cnt=" + ToString( nCnt) + ") " + ( bInTol ? "Ok" : "Split") ; + LOG_INFO( GetEMkLogger(), sOut.c_str()) + } + + // Se posizione in tolleranza, non devo fare alcunché + if ( bInTol) + return true ; + + // Va inserito il punto medio + Point3d ptMid = Media( ptPrec, ptP) ; + Vector3d vtDirMid = Media( vtDirPrec, vtDir) ; + if ( ! vtDirMid.Normalize()) { + if ( ! pMachine->GetBackToolDirFromAngles( vAxMid, vtDirMid)) + return ( nMoveType == 0) ; + } + Vector3d vtAuxMid = Media( vtAuxPrec, vtAux) ; + if ( ! vtAuxPrec.IsSmall() && ! vtAux.IsSmall() && ! vtAuxMid.Normalize()) { + if ( ! pMachine->GetBackAuxDirFromAngles( vAxMid, vtAuxMid)) + return ( nMoveType == 0) ; + } + Vector3d vtCorrMid = Media( vtCorrPrec, vtCorr) ; + if ( ! vtCorrPrec.IsSmall() && ! vtCorr.IsSmall() && ! vtCorrMid.Normalize()) { + // assegno il versore correzione precedente + vtCorrMid = vtCorrPrec ; + // se possibile, cerco di migliorarlo considerando i rif. prec. e sul medio da direzioni fresa (Z) e ausiliaria (X) + Frame3d refPrec, refMid ; + if ( refPrec.Set( ORIG, vtDirPrec, vtAuxPrec) && + refMid.Set( ORIG, vtDirMid, vtAuxMid)) { + // versore correzione in locale al precente coincide con quello in locale al corrente + vtCorrMid.ToLoc( refPrec) ; + vtCorrMid.ToGlob( refMid) ; + } + } + // inserisco nuova entità punto (per evitare problemi di lunghezza linea) + // creo oggetto punto per DB geometrico + PtrOwner pGP( CreateGeoPoint3d()) ; + if ( IsNull( pGP)) + return false ; + // assegno le coordinate del punto + pGP->Set( ptMid) ; + // inserisco l'oggetto nel DB geometrico + int nMidEntId = m_pGeomDB->InsertGeoObj( GDB_ID_NULL, nEntId, GDB_BEFORE, Release( pGP)) ; + if ( nMidEntId == GDB_ID_NULL) + return false ; + // creo oggetto dati Cam + const CamData* pEntCam = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ; + if ( pEntCam == nullptr) + return false ; + PtrOwner pCam( pEntCam->Clone()) ; + if ( IsNull( pCam)) + return false ; + // associo questo oggetto a quello geometrico + m_pGeomDB->SetUserObj( nMidEntId, Release( pCam)) ; + // assegno valori + CamData* pMidCamData = GetCamData( m_pGeomDB->GetUserObj( nMidEntId)) ; + if ( pMidCamData == nullptr) + return false ; + pMidCamData->SetEndPoint( ptMid) ; + pMidCamData->SetToolDir( vtDirMid) ; + pMidCamData->SetAuxDir( vtAuxMid) ; + pMidCamData->SetCorrDir( vtCorrMid) ; + pMidCamData->SetFlag2( -1) ; + pMidCamData->SetToolShow( bToolShow && nCnt < 4) ; + // calcolo gli assi per il punto medio + DBLVECTOR vAxRotHome( vAxVal.size()) ; + DBLVECTOR vAxRotPrec( vAxVal.size()) ; + for ( int i = 0 ; i < int( vAxVal.size()) ; ++ i) + vAxRotPrec[i] = vAxVal[i] ; + if ( ! CalculateRobotAxesValues( false, ptMid, vtDirMid, vtAuxMid, + true, INFINITO, vAxRotHome, vAxRotPrec, vAxMid)) { + pMidCamData->SetAxes( CamData::AS_ERR, vAxVal) ; + return false ; + } + // verifico di essere nelle corse degli assi + int nStat ; + bool bOsOk = m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAxMid, false, nStat) ; + if ( ! bOsOk || nStat != 0) { + pMidCamData->SetAxes( CamData::AS_OUTSTROKE, vAxMid) ; + bAxError = true ; + return true ; + } + // assegno i valori degli assi + pMidCamData->SetAxes( CamData::AS_OK, vAxMid) ; + // calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo + Vector3d vtBackAux ; pMachine->GetBackAuxDirFromAngles( vAxMid, vtBackAux) ; + pMidCamData->SetBackAuxDir( vtBackAux) ; + + // devo verificare le due nuove parti + ++ nCnt ; + bool bAdded1, bAxError1, bAdded2, bAxError2 ; + if ( ! VerifyRobotLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec, + ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid, + nCnt, nMidEntId, nMoveType, bToolShow, bAdded1, bAxError1)) + return false ; + if ( bAxError1) { + bAxError = true ; + return true ; + } + if ( ! VerifyRobotLineMidPoint( ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid, + ptP, vtDir, vtAux, vtCorr, vAxVal, + nCnt, nEntId, nMoveType, bToolShow, bAdded2, bAxError2)) + return false ; + if ( bAxError2) { + bAxError = true ; + return true ; + } + + return true ; +} + //---------------------------------------------------------------------------- bool Operation::AdjustStartEndMovements( bool bVerifyPreviousLink) @@ -3232,7 +3452,7 @@ Operation::AdjustOneStartEndMovement( int nClPathId, int nPrevClPathId, Operatio // se provengo da Z massima if ( bMaxZ) { // se centro di lavoro - if ( m_pMchMgr->GetCurrIsCenter()) { + if ( m_pMchMgr->GetCurrIsMcent()) { // copio l'entità if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL) return false ; @@ -3292,7 +3512,7 @@ Operation::AdjustOneStartEndMovement( int nClPathId, int nPrevClPathId, Operatio // verifico se la testa interferisce con i pezzi o i bloccaggi sulla tavola else { // se centro di lavoro - if ( m_pMchMgr->GetCurrIsCenter()) { + if ( m_pMchMgr->GetCurrIsMcent()) { // Recupero box complessivo dei grezzi attivi BBox3d b3Raws ; GetCurrRawsGlobBox( b3Raws) ; @@ -3760,7 +3980,7 @@ Operation::AddRise( DBLVECTOR& vAxVal, double dDelta, int nClPathId, int nToMinM int nFlag2 = 0 ; bool bOk = false ; // se centro di lavoro - if ( m_pMchMgr->GetCurrIsCenter()) { + if ( m_pMchMgr->GetCurrIsMcent()) { // devono esserci almeno i tre assi lineari if ( vAxVal.size() < 3) return false ; @@ -4545,7 +4765,7 @@ Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEn } // Verifica - int nLinkAxisOrder = ( m_pMchMgr->GetCurrIsCenter() ? pMch->GetLinkAxesMoveOrder() : LKAMO_INTERP) ; + int nLinkAxisOrder = ( m_pMchMgr->GetCurrIsMcent() ? pMch->GetLinkAxesMoveOrder() : LKAMO_INTERP) ; // se ordine dipende da variazione angolare, verifico quale ordine effettivo if ( nLinkAxisOrder == LKAMO_HEAD_BEFORE_IF_SAME_ANG || nLinkAxisOrder == LKAMO_HEAD_AFTER_IF_SAME_ANG) { // cerco massima variazione degli assi rotanti diff --git a/Operation.h b/Operation.h index 69191fa..987b72d 100644 --- a/Operation.h +++ b/Operation.h @@ -211,22 +211,28 @@ class Operation : public IUserObj const CamData* GetClPathInitialCamData( int nClPathId, bool bSkipClimb) const ; const CamData* GetFinalCamData( bool bSkipRise) const ; const CamData* GetClPathFinalCamData( int nClPathId, bool bSkipRise) const ; - bool CalculateClPathCenterAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, - bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, - const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) ; + bool CalculateClPathMcentAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, + bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) ; bool EraseAddedPoints( int nClPathId) ; - bool CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, - bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, - const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC) ; - bool CalculateRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux, - double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, - const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) ; - bool VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec, - const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal, - int nCnt, int nEntId, double dRot1W, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError) ; + bool MyCalculateClPathMcentAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, + bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC) ; + bool CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux, + double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) ; + bool VerifyMcentLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec, + const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal, + int nCnt, int nEntId, double dRot1W, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError) ; bool CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) ; + bool CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vector3d& vtTool, const Vector3d& vtAux, + bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) ; + bool VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec, + const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal, + int nCnt, int nEntId, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError) ; bool GetAxisMidForTestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, int nAxisOrder, Machine* pMch, DBLVECTOR& vAxMid) const ; bool OneMoveTestCollisionAvoid( const STRVECTOR& vAxName, const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd,