diff --git a/MachMgr.h b/MachMgr.h index eef5b99..804ed9d 100644 --- a/MachMgr.h +++ b/MachMgr.h @@ -354,9 +354,9 @@ class MachMgr : public IMachMgr bool GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const override ; bool GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB, - int& nStat, double& dX, double& dY, double& dZ) const override ; + double& dX, double& dY, double& dZ) const override ; bool GetCalcPositions( const Point3d& ptP, const DBLVECTOR& vAng, - int& nStat, double& dX, double& dY, double& dZ) const override ; + double& dX, double& dY, double& dZ) const override ; bool GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vector3d& vtDirA, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const override ; bool GetCalcTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB, diff --git a/MachMgrMachines.cpp b/MachMgrMachines.cpp index e079bcb..4346a07 100644 --- a/MachMgrMachines.cpp +++ b/MachMgrMachines.cpp @@ -780,20 +780,20 @@ MachMgr::GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, //---------------------------------------------------------------------------- bool MachMgr::GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB, - int& nStat, double& dX, double& dY, double& dZ) const + double& dX, double& dY, double& dZ) const { DBLVECTOR vAng( 2) ; vAng[0] = dAngA ; vAng[1] = dAngB ; Machine* pMch = GetCurrMachine() ; - return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, nStat, dX, dY, dZ) : false) ; + return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, dX, dY, dZ) : false) ; } //---------------------------------------------------------------------------- bool MachMgr::GetCalcPositions( const Point3d& ptP, const DBLVECTOR& vAng, - int& nStat, double& dX, double& dY, double& dZ) const + double& dX, double& dY, double& dZ) const { Machine* pMch = GetCurrMachine() ; - return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, nStat, dX, dY, dZ) : false) ; + return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, dX, dY, dZ) : false) ; } //---------------------------------------------------------------------------- diff --git a/Machine.h b/Machine.h index 187f431..a233441 100644 --- a/Machine.h +++ b/Machine.h @@ -171,7 +171,7 @@ class Machine bool GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const ; bool GetPositions( const Point3d& ptP, const DBLVECTOR& vAng, - int& nStat, double& dX, double& dY, double& dZ) const ; + double& dX, double& dY, double& dZ) const ; bool GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vector3d& vtDirA, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const ; bool GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, diff --git a/MachineCalc.cpp b/MachineCalc.cpp index 16761f0..4c82e60 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -1294,7 +1294,7 @@ Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, //---------------------------------------------------------------------------- bool Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng, - int& nStat, double& dX, double& dY, double& dZ) const + double& dX, double& dY, double& dZ) const { // la posizione deve essere espressa rispetto allo ZERO MACCHINA // il punto è dato rispetto alla posizione home della tavola @@ -1355,7 +1355,6 @@ Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng, dZ = ptPos.z ; // tutto ok - nStat = 0 ; return true ; } @@ -1701,8 +1700,19 @@ Machine::AdjustAngleInStroke( const STROKE& Stroke, double& dAng) const dAng += ANG_FULL ; while ( dAng > Stroke.Max) dAng -= ANG_FULL ; - // verifico - return ( dAng >= Stroke.Min && dAng <= Stroke.Max) ; + // se nei limiti, tutto bene + if ( dAng >= Stroke.Min && dAng <= Stroke.Max) + return true ; + // altrimenti, determino il valore con il minimo outstroke + if ( dAng < Stroke.Min) { + if ( abs( dAng - Stroke.Min) > abs( dAng + ANG_FULL - Stroke.Max)) + dAng += ANG_FULL ; + } + else { + if ( abs( dAng - Stroke.Max) > abs( dAng - ANG_FULL - Stroke.Min)) + dAng -= ANG_FULL ; + } + return false ; } // altrimenti asse rotante senza corsa else { @@ -2265,7 +2275,14 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto 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) { - LOG_ERROR( GetEMkLogger(), "Error : position unreachable (robot)") + double dDeltaOut = dDistR2Cen - ( dDistR2R3 + dDistR3R5) ; + double dDeltaIn = dDistR2Cen - abs( dDistR2R3 - dDistR3R5) ; + string sErr = "Error : position for the robot unreachable" ; + if ( dDeltaOut > 0) + sErr += " (Out=" + ToString( dDeltaOut, 3) + ")" ; + else if ( dDeltaIn < 0) + sErr += " (In=" + ToString( dDeltaIn, 3) + ")" ; + LOG_ERROR( GetEMkLogger(), sErr.c_str()) return false ; } double dAngB = acos( dCosB) * RADTODEG ; @@ -2325,21 +2342,28 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto vAng2.push_back( dAng6 + ( dAng6 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT)) ; // verifiche dei limiti di corsa - for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) { - if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng2[i])) { - vAng2.clear() ; - break ; - } - } + bool bAng1 = true, bAng2 = true ; for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng1[i])) { + if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng1[i])) + bAng1 = false ; + if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng2[i])) + bAng2 = false ; + } + if ( bAng2) { + if ( ! bAng1) { vAng1 = vAng2 ; - break ; + vAng2.clear() ; + swap( bAng1, bAng2) ; } } - if ( vAng1.empty()) { - LOG_ERROR( GetEMkLogger(), "Error : angles out stroke (robot)") - return false ; + else { + vAng2.clear() ; + } + if ( ! bAng1) { + int nStat ; + VerifyOutstroke( 0, 0, 0, vAng1, true, nStat) ; + LOG_ERROR( GetEMkLogger(), GetOutstrokeInfo().c_str()) + return false ; } return true ; diff --git a/Operation.cpp b/Operation.cpp index 27a577d..ef77197 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -2005,6 +2005,8 @@ Operation::MyCalculateClPathMcentAxesValues( int nClPathId, double dAngDeltaMinF if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; Machine* pMachine = m_pMchMgr->GetCurrMachine() ; + if ( pMachine == nullptr) + return false ; // predispongo variabile per valori assi DBLVECTOR vAxVal ; vAxVal.reserve( 8) ; @@ -2048,10 +2050,9 @@ Operation::MyCalculateClPathMcentAxesValues( int nClPathId, double dAngDeltaMinF Point3d ptP = pCamData->GetEndPoint() ; AdjustEndPointForAxesCalc( pCamData, ptP) ; // calcolo gli assi lineari della macchina - int nLStat ; double dX, dY, dZ ; - bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAxRot, nLStat, dX, dY, dZ) ; - if ( ! bLOk || nLStat != 0) { + bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAxRot, dX, dY, dZ) ; + if ( ! bLOk) { bOk = false ; pCamData->SetAxes( CamData::AS_ERR, vAxVal) ; continue ; @@ -2160,10 +2161,9 @@ Operation::MyCalculateClPathMcentAxesValues( int nClPathId, double dAngDeltaMinF DBLVECTOR vAngMid( vAxRot.size()) ; for ( size_t i = 0 ; i < vAxRot.size() ; ++ i) vAngMid[i] = ( vAxRotPrec[i] + vAxRot[i]) / 2 ; - int nLmidStat ; double dXmid, dYmid, dZmid ; - bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, vAngMid, nLmidStat, dXmid, dYmid, dZmid) ; - if ( ! bLmidOk || nLmidStat != 0) { + bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, vAngMid, dXmid, dYmid, dZmid) ; + if ( ! bLmidOk) { bOk = false ; pCamData->SetAxes( CamData::AS_ERR, vAxVal) ; continue ; @@ -2174,10 +2174,9 @@ Operation::MyCalculateClPathMcentAxesValues( int nClPathId, double dAngDeltaMinF DBLVECTOR vAngEnd( vAxRot.size()) ; for ( size_t i = 0 ; i < vAxRot.size() ; ++ i) vAngEnd[i] = vAxRot[i] ; - int nLendStat ; double dXend, dYend, dZend ; - bool bLendOk = m_pMchMgr->GetCalcPositions( ptEnd, vAngEnd, nLendStat, dXend, dYend, dZend) ; - if ( ! bLendOk || nLendStat != 0) { + bool bLendOk = m_pMchMgr->GetCalcPositions( ptEnd, vAngEnd, dXend, dYend, dZend) ; + if ( ! bLendOk) { bOk = false ; pCamData->SetAxes( CamData::AS_ERR, vAxVal) ; continue ; @@ -2553,10 +2552,9 @@ Operation::VerifyMcentLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDir for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i) vAxMid[i] = vAxRotMid[i-3] ; // determino il nuovo valore degli assi lineari sul punto medio - int nMidLStat ; double dMidX, dMidY, dMidZ ; - bool bMidLOk = m_pMchMgr->GetCalcPositions( ptMid, vAxRotMid, nMidLStat, dMidX, dMidY, dMidZ) ; - if ( ! bMidLOk || nMidLStat != 0) { + bool bMidLOk = m_pMchMgr->GetCalcPositions( ptMid, vAxRotMid, dMidX, dMidY, dMidZ) ; + if ( ! bMidLOk) { pMidCamData->SetAxes( CamData::AS_ERR, vAxMid) ; bAxError = true ; return true ; @@ -2684,6 +2682,8 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, double dAngDeltaMinFor } else { pCamData->SetAxes( CamData::AS_OK, vAxVal) ; + // l'asse ausiliario di testa riportato sul grezzo coincide con quello già impostato + pCamData->SetBackAuxDir( vtAux) ; } // tipo di movimento int nMoveType = pCamData->GetMoveType() ; @@ -2726,25 +2726,29 @@ Operation::CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vect double dAngDeltaMinForHome, const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxVal) { + // macchina + Machine* pMachine = m_pMchMgr->GetCurrMachine() ; + if ( pMachine == nullptr) + return false ; // continuità su assi rotanti nei movimenti successivi al primo per tutte le lavorazioni, tranne disposizioni bool bRotContOnNext = ( GetType() != OPER_DISP) ; // calcolo angoli dei giunti DBLVECTOR vAng1, vAng2 ; - bool bRaOk = m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptP, vtTool, vtAux, vAng1, vAng2) ; + bool bRaOk = pMachine->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]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; } for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) { - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; } bFirst = false ; } @@ -2755,13 +2759,37 @@ Operation::CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vect if ( bRotContOnNext) vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; else - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + pMachine->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]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; + } + // verifico che le soluzioni siano nelle corse + bool bAng1 = true, bAng2 = true ; + for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + if ( ! pMachine->VerifyAngleOutstroke( i, vAng1[i])) + bAng1 = false ; + if ( ! pMachine->VerifyAngleOutstroke( i, vAng2[i])) + bAng2 = false ; + } + if ( bAng2) { + if ( ! bAng1) { + vAng1 = vAng2 ; + vAng2.clear() ; + swap( bAng1, bAng2) ; + } + } + else { + vAng2.clear() ; + } + if ( ! bAng1) { + int nStat ; + pMachine->VerifyOutstroke( 0, 0, 0, vAng1, true, nStat) ; + LOG_ERROR( GetEMkLogger(), pMachine->GetOutstrokeInfo().c_str()) + return false ; } } // Scelgo la soluzione più vicina @@ -2779,7 +2807,7 @@ Operation::CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vect else vAxVal = vAng2 ; // controllo continuità di R6 dalla posizione precedente - m_pMchMgr->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ; + pMachine->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ; return true ; } @@ -2789,10 +2817,14 @@ Operation::VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDir 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) { + // macchina + Machine* pMachine = m_pMchMgr->GetCurrMachine() ; + if ( pMachine == nullptr) + return false ; + // impostazioni bAdded = false ; bAxError = false ; - Machine* pMachine = m_pMchMgr->GetCurrMachine() ; // se disposizione non vanno fatti controlli if ( GetType() == OPER_DISP) @@ -2909,7 +2941,7 @@ Operation::VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDir } // verifico di essere nelle corse degli assi int nStat ; - bool bOsOk = m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAxMid, false, nStat) ; + bool bOsOk = pMachine->VerifyOutstroke( 0, 0, 0, vAxMid, false, nStat) ; if ( ! bOsOk || nStat != 0) { pMidCamData->SetAxes( CamData::AS_OUTSTROKE, vAxMid) ; bAxError = true ; @@ -2917,9 +2949,8 @@ Operation::VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDir } // 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) ; + // l'asse ausiliario di testa riportato sul grezzo coincide con quello già impostato + pMidCamData->SetBackAuxDir( vtAuxMid) ; // devo verificare le due nuove parti ++ nCnt ;