diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc index cbfee2e..2a174e2 100644 Binary files a/EgtMachKernel.rc and b/EgtMachKernel.rc differ diff --git a/Machine.cpp b/Machine.cpp index 629ce9a..63230e6 100644 --- a/Machine.cpp +++ b/Machine.cpp @@ -58,6 +58,7 @@ Machine::Machine( void) m_nHeadRotAxes = 0 ; m_nHeadSpecRotAxis = -1 ; m_frLinAx.Reset( false) ; + m_frRobot.Reset( false) ; m_nCalcChainType = KIN_CHAIN_NONE ; m_nMachineLook = MCH_LOOK_NONE ; } diff --git a/Machine.h b/Machine.h index 896ffdd..d1b9048 100644 --- a/Machine.h +++ b/Machine.h @@ -317,6 +317,7 @@ class Machine KINAXISVECTOR m_vCalcLinAx ; // vettore assi lineari attivi per calcoli KINAXISVECTOR m_vCalcRotAx ; // vettore assi rotanti attivi per calcoli Frame3d m_frLinAx ; // sistema di riferimento definito dagli assi lineari + Frame3d m_frRobot ; // sistema di riferimento canonico del robot int m_nCalcChainType ; // tipologia testa attiva (nulla, centro di lavoro o robot) Point3d m_ptWristCen ; // centro del polso sferico nel riferimento testa/uscita di calcolo Vector3d m_vtWristRef ; // direzione del polso sferico nel riferimento testa/uscita di calcolo diff --git a/MachineCalc.cpp b/MachineCalc.cpp index dc1d8c3..212e11e 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -493,6 +493,8 @@ Machine::CalculateKinematicChain( void) m_nHeadSpecRotAxis = -1 ; m_vCalcLinAx.clear() ; m_vCalcRotAx.clear() ; + m_frLinAx.Reset( false) ; + m_frRobot.Reset( false) ; m_nCalcChainType = KIN_CHAIN_NONE ; // recupero gli assi di tavola if ( m_nCalcTabId == GDB_ID_NULL) @@ -535,6 +537,19 @@ Machine::CalculateKinematicChain( void) swap( m_vCalcRotAx[0], m_vCalcRotAx[5]) ; swap( m_vCalcRotAx[1], m_vCalcRotAx[4]) ; swap( m_vCalcRotAx[2], m_vCalcRotAx[3]) ; + // determino il riferimento canonico del robot + if ( ! m_frRobot.Set( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, m_vCalcRotAx[3].vtDir)) { + LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : impossible canonic frame") + return false ; + } + // porto tutti gli assi cinematici e i dati testa nel riferimento canonico + for ( int i = 0 ; i < m_nHeadRotAxes ; ++ i) { + m_vCalcRotAx[i].ptPos.ToLoc( m_frRobot) ; + m_vCalcRotAx[i].vtDir.ToLoc( m_frRobot) ; + } + m_ptCalcPos.ToLoc( m_frRobot) ; + m_vtCalcDir.ToLoc( m_frRobot) ; + m_vtCalcADir.ToLoc( m_frRobot) ; // direzione assi rotanti deve essere Z Y Y X Y X if ( ! m_vCalcRotAx[0].vtDir.IsZ() || ! m_vCalcRotAx[1].vtDir.IsY() || @@ -1266,6 +1281,10 @@ Machine::GetDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& v vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ; } + // nel caso di robot devo passare dal riferimento canonico robot a quello di macchina + if ( m_nCalcChainType == KIN_CHAIN_ROBOT) + vtNew.ToGlob( m_frRobot) ; + return true ; } @@ -1396,6 +1415,10 @@ Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& else return false ; + // nel caso di robot devo passare dal riferimento canonico robot a quello di macchina + if ( m_nCalcChainType == KIN_CHAIN_ROBOT) + ptTip.ToGlob( m_frRobot) ; + // Se richiesto ingombro totale o punto sotto del tip utensile if ( bOverall || bBottom) { // calcolo la direzione fresa @@ -2050,13 +2073,17 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto // pulisco il risultato vAng1.clear() ; vAng2.clear() ; + // porto i dati nel riferimento robot + Point3d ptPL = GetToLoc( ptP, m_frRobot) ; + Vector3d vtDirTL = GetToLoc( vtDirT, m_frRobot) ; + Vector3d vtDirAL = GetToLoc( vtDirA, m_frRobot) ; // riferimento utensile Frame3d frTool ; - frTool.Set( ORIG, vtDirT, vtDirA) ; + frTool.Set( ORIG, vtDirTL, vtDirAL) ; // deduco la posizione richiesta del centro del polso - Point3d ptCen = ptP + m_ptWristCen.x * frTool.VersX() + - m_ptWristCen.y * frTool.VersY() + - ( m_dCalcTLen + m_ptWristCen.z) * frTool.VersZ() ; + Point3d ptCen = ptPL + m_ptWristCen.x * frTool.VersX() + + m_ptWristCen.y * frTool.VersY() + + ( m_dCalcTLen + m_ptWristCen.z) * frTool.VersZ() ; // deduco la direzione richiesta del centro del polso Vector3d vtCen = m_vtWristRef.x * frTool.VersX() + m_vtWristRef.y * frTool.VersY() + @@ -2131,11 +2158,11 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto for ( int i = 4; i >= 0 ; --i) vtTool.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ; double dAng6 ; bool bDet6 ; - if ( ! vtTool.GetRotation( vtDirT, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) { + if ( ! vtTool.GetRotation( vtDirTL, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) { Vector3d vtAux = m_vtCalcADir ; for ( int i = 4; i >= 0 ; --i) vtAux.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ; - if ( ! vtAux.GetRotation( vtDirA, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) { + if ( ! vtAux.GetRotation( vtDirAL, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) { LOG_ERROR( GetEMkLogger(), "Error : R6 not calculable (robot)") return false ; } diff --git a/Operation.cpp b/Operation.cpp index 3c04180..d336d83 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -42,7 +42,7 @@ using namespace std ; //---------------------------------------------------------------------------- const double MAX_DIST_RAW = 200.0 ; -const double MAX_ROBOT_G0_LEN = 300.0 ; +const double MAX_ROBOT_G0_LEN = 299.0 ; //---------------------------------------------------------------------------- bool diff --git a/Pocketing.cpp b/Pocketing.cpp index ffee199..b55c615 100644 --- a/Pocketing.cpp +++ b/Pocketing.cpp @@ -4223,8 +4223,14 @@ Pocketing::CalcCircleSpiral( const Point3d& ptCen, const Vector3d& vtN, double d return false ; } pRCrv->AddCurve( Release( pArc2)) ; - // inverto e eventualmente sistemo archi + // inverto pRCrv->Invert() ; + // se necessario, approssimo con rette + if ( bSplitArcs && ! ApproxWithLines( pRCrv)) { + m_pMchMgr->SetLastError( 2419, "Error in Pocketing : Linear Approx not computable") ; + return false ; + } + // eventuale sistemazione archi VerifyArcs( pRCrv) ; }