Egalware/EgtMachKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

EgtMachKernel :

Browse Source 
- nel calcolo degli assi macchina su percorsi ora si infittiscono opportunamente i punti quando necessario a 5 assi.
This commit is contained in:
Dario Sassi
2024-05-22 08:20:56 +02:00
parent 9c38fcd015
commit 171c473e5b
5 changed files with 386 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GenMachining.cpp
+11
View File
@@ -964,3 +964,14 @@ GenMachining::VerifyGeometry( SelData Id, int& nSubs, int& nType)
else
return false ;
}
//----------------------------------------------------------------------------
double
GenMachining::GetApproxLinTol( void) const
{
double dLinTol ;
if ( GetValInNotes( m_Params.m_sUserNotes, "LinTol", dLinTol))
return dLinTol ;
else
return Operation::GetApproxLinTol() ;
}
GenMachining.h
+1
View File
@@ -67,6 +67,7 @@ class GenMachining : public Machining
private :
bool VerifyGeometry( SelData Id, int& nSubs, int& nType) ;
double GetApproxLinTol( void) const override ;
private :
double GetSpeed() const
Machine.h
+1 -1
View File
@@ -147,7 +147,7 @@ class Machine
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 bOverall, bool bBottom, Point3d& ptTip) const ;
bool GetToolDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const ;
bool GetAuxDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const ;
bool GetPartDirFromAngles( const Vector3d& vtPart, const DBLVECTOR& vAng, Vector3d& vtDir) const ;
Operation.cpp
+367 -188
View File
@@ -26,6 +26,7 @@
#include "/EgtDev/Include/EGkCurveArc.h"
#include "/EgtDev/Include/EGkArcSpecial.h"
#include "/EgtDev/Include/EGkCurveComposite.h"
#include "/EgtDev/Include/EGkDistPointLine.h"
#include "/EgtDev/Include/EGkDistPointCurve.h"
#include "/EgtDev/Include/EGkIntersCurves.h"
#include "/EgtDev/Include/EGkIntersLineSurfTm.h"
@@ -1857,13 +1858,16 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
{
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
return false ;
// massima variazione secondo asse rotante per continuità
double dMaxDeltaR2 = 30 ;
Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
// predispongo variabile per valori assi
DBLVECTOR vAxVal ;
vAxVal.reserve( 8) ;
// variabili per coordinate lineari precedenti
double dXprec, dYprec, dZprec ;
// valori del precedente
Point3d ptPrec ;
Vector3d vtDirPrec ;
Vector3d vtAuxPrec ;
Vector3d vtCorrPrec ;
DBLVECTOR vAxPrec ;
// ciclo su tutte le entità del percorso CL
nOutStrC = 0 ;
bool bOk = true ;
@@ -1881,154 +1885,24 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
LOG_ERROR( GetEMkLogger(), "Error : first move in ClPath is an arc")
return false ;
}
// calcolo degli assi rotanti della macchina
int nRStat ;
DBLVECTOR vAng1, vAng2 ;
bool bROk = m_pMchMgr->GetCalcAngles( pCamData->GetToolDir(), pCamData->GetAuxDir(), nRStat, vAng1, vAng2) ;
if ( ! bROk || nRStat == 0) {
// calcolo gli assi rotanti della macchina
Vector3d vtDir = pCamData->GetToolDir() ;
Vector3d vtAux = pCamData->GetAuxDir() ;
Vector3d vtCorr = pCamData->GetCorrDir() ;
DBLVECTOR vAxRot ;
if ( ! CalculateRotAxesValues( 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 ;
}
if ( abs( nRStat) == 1) {
// se primo movimento
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]) ;
}
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa
if ( nRStat < 0 && vAng1.size() >= 1) {
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ;
break ;
}
}
}
// per movimenti successivi
else {
// scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse
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]) ;
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
}
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente
if ( nRStat < 0 && vAng1.size() >= 1) {
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
break ;
}
}
}
}
if ( abs( nRStat) == 2) {
bool bAng1 = true ;
bool bAng2 = true ;
// se primo movimento
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]) ;
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
}
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa
if ( nRStat < 0 && vAng1.size() >= 1) {
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ;
vAng2[i] = vAng1[i] ;
break ;
}
}
}
else {
// scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse
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]) ;
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
if ( bRotContOnNext)
vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ;
else
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
}
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente
if ( nRStat < 0 && vAng1.size() >= 1) {
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
vAng2[i] = vAng1[i] ;
break ;
}
}
// verifico che le soluzioni siano nelle corse
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng1[i]))
bAng1 = false ;
if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng2[i]))
bAng2 = false ;
}
}
// scelgo la soluzione più vicina alla precedente
double dDelta1 = 0 ;
double dDelta2 = 0 ;
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// calcolo i delta asse con eventuale peso
bool bFirst = ( i == 0) ;
dDelta1 += abs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ;
dDelta2 += abs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ;
}
if ( bAng2 && ( dDelta2 < dDelta1 - EPS_ANG_SMALL || ! bAng1))
swap( vAng1, vAng2) ;
// se imposto limite su variazione di secondo asse rotante
if ( ( bMaxDeltaR2OnFirst || ! bFirst) && dMaxDeltaR2 > EPS_ANG_SMALL && vAng1.size() >= 2) {
double dR2Diff1 = vAng1[1] - vAxRotPrec[1] ;
double dR2Diff2 = vAng2[1] - vAxRotPrec[1] ;
if ( abs( dR2Diff1) > abs( dR2Diff2) && abs( dR2Diff1) > dMaxDeltaR2)
swap( vAng1, vAng2) ;
}
}
// ricavo posizione con eventuali modifiche dipendenti dalla lavorazione
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, vAng1, nLStat, dX, dY, dZ) ;
bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAxRot, nLStat, dX, dY, dZ) ;
if ( ! bLOk || nLStat != 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
@@ -2039,11 +1913,11 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
vAxVal.emplace_back( dX) ;
vAxVal.emplace_back( dY) ;
vAxVal.emplace_back( dZ) ;
for ( auto dAng : vAng1)
for ( auto dAng : vAxRot)
vAxVal.emplace_back( dAng) ;
// verifico i limiti di corsa degli assi
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( dX, dY, dZ, vAng1, false, nStat) ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( dX, dY, dZ, vAxRot, false, nStat) ;
if ( ! bOsOk || nStat != 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
@@ -2057,47 +1931,65 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
LOG_INFO( GetEMkLogger(), sInfo.c_str())
// calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo
Vector3d vtBackAux ;
m_pMchMgr->GetCurrMachine()->GetBackAuxDirFromAngles( vAng1, vtBackAux) ;
pMachine->GetBackAuxDirFromAngles( vAxRot, vtBackAux) ;
pCamData->SetBackAuxDir( vtBackAux) ;
// imposto stato
bFirst = false ;
// memorizzo i valori degli assi lineari come nuovi precedenti
dXprec = dX ;
dYprec = dY ;
dZprec = dZ ;
// memorizzo i precedenti
ptPrec = ptP ;
vtDirPrec = vtDir ;
vtAuxPrec = vtAux ;
vtCorrPrec = vtCorr ;
vAxPrec = vAxVal ;
// memorizzo i valori degli angoli come nuovi precedenti
vAxRotPrec = vAng1 ;
vAxRotPrec = vAxRot ;
continue ;
}
// salvo i valori degli assi
pCamData->SetAxes( CamData::AS_OK, vAxVal) ;
// calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo
Vector3d vtBackAux ;
m_pMchMgr->GetCurrMachine()->GetBackAuxDirFromAngles( vAng1, vtBackAux) ;
pMachine->GetBackAuxDirFromAngles( vAxRot, vtBackAux) ;
pCamData->SetBackAuxDir( vtBackAux) ;
// se esiste precedente, corrente è linea ed esistono aree protette, devo verificare anche i punti intermedi
if ( ! bFirst && pCamData->IsLine() && m_pMchMgr->ExistProtectedAreas()) {
// verifico i limiti di corsa dei punti lungo la linea
const int NUM_VERIF_STEP = 16 ;
Point3d ptPrec( dXprec, dYprec, dZprec) ;
Point3d ptP( dX, dY, dZ) ;
for ( int i = 1 ; i < NUM_VERIF_STEP ; ++ i) {
double dCoeff = double( i) / NUM_VERIF_STEP ;
Point3d ptCurr = Media( ptPrec, ptP, dCoeff) ;
DBLVECTOR vAng( vAng1.size()) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i)
vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAng1[i] * dCoeff ;
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng, false, nStat) ;
if ( ! bOsOk || nStat != 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
string sInfo = "Outstroke : " + m_pMchMgr->GetOutstrokeInfo() ;
LOG_INFO( GetEMkLogger(), sInfo.c_str())
// se punto finale di linea
if ( ! bFirst && pCamData->IsLine()) {
// Verifica ricorsiva del punto medio
bool bMidAdded ;
bool bAxesError ;
if ( ! VerifyLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec,
ptP, vtDir, vtAux, vtCorr, vAxVal,
0, nEntId, dRot1W, bMidAdded, bAxesError)) {
LOG_ERROR( GetEMkLogger(), "Error : VerifyLineMidPoint of CalculateClPathAxesValues failed")
return false ;
}
if ( bAxesError) {
bOk = false ;
continue ;
}
// Se esistono aree protette, devo comunque verificare i punti intermedi
if ( ! bMidAdded && m_pMchMgr->ExistProtectedAreas()) {
// verifico i limiti di corsa dei punti lungo la linea
const int NUM_VERIF_STEP = 16 ;
Point3d ptPrec( vAxPrec[0], vAxPrec[1], vAxPrec[2]) ;
Point3d ptP( dX, dY, dZ) ;
for ( int i = 1 ; i < NUM_VERIF_STEP ; ++ i) {
double dCoeff = double( i) / NUM_VERIF_STEP ;
Point3d ptCurr = Media( ptPrec, ptP, dCoeff) ;
DBLVECTOR vAng( vAxRot.size()) ;
for ( size_t i = 0 ; i < vAxRot.size() ; ++ i)
vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAxRot[i] * dCoeff ;
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng, false, nStat) ;
if ( ! bOsOk || nStat != 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
string sInfo = "Outstroke : " + m_pMchMgr->GetOutstrokeInfo() ;
LOG_INFO( GetEMkLogger(), sInfo.c_str())
}
}
}
}
// se arco devo calcolarne il centro in assi macchina
// se punto finale di arco -> devo calcolarne il centro in assi macchina
else if ( pCamData->IsArc()) {
// devo lavorare con arco schiacciato nel suo piano
Point3d ptCen = pCamData->GetCenter() ;
@@ -2109,9 +2001,9 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
Point3d ptMid = ptP - dDeltaN * vtN ;
ptMid.Rotate( ptCen, vtN, - dAngCen / 2) ;
// determino i valori degli assi al punto medio
DBLVECTOR vAngMid( vAng1.size()) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i)
vAngMid[i] = ( vAxRotPrec[i] + vAng1[i]) / 2 ;
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) ;
@@ -2123,9 +2015,9 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
// ricavo punto finale nel piano dell'arco
Point3d ptEnd = ptP - dDeltaN * vtN ;
// determino i valori degli assi al punto finale
DBLVECTOR vAngEnd( vAng1.size()) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i)
vAngEnd[i] = vAng1[i] ;
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) ;
@@ -2135,7 +2027,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
continue ;
}
// punti espressi in assi macchina
Point3d ptP1M( dXprec, dYprec, dZprec) ;
Point3d ptP1M( vAxPrec[0], vAxPrec[1], vAxPrec[2]) ;
Point3d ptP2M( dXmid, dYmid, dZmid) ;
Point3d ptP3M( dXend, dYend, dZend) ;
// se coincidono
@@ -2178,14 +2070,14 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
Point3d ptCen = pCamData->GetAxesCen() ;
double dAngCenStep = pCamData->GetAxesAngCen() / NUM_VERIF_STEP ;
Vector3d vtN = pCamData->GetAxesNormDir() ;
Vector3d vtCurr = Point3d( dXprec, dYprec, dZprec) - ptCen ;
Vector3d vtCurr = Point3d( vAxPrec[0], vAxPrec[1], vAxPrec[2]) - ptCen ;
for ( int i = 1 ; i < NUM_VERIF_STEP ; ++ i) {
vtCurr.Rotate( vtN, dAngCenStep) ;
double dCoeff = double( i) / NUM_VERIF_STEP ;
Point3d ptCurr = ptCen + vtCurr + vtN * dDeltaN * dCoeff ;
DBLVECTOR vAng( vAng1.size()) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i)
vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAng1[i] * dCoeff ;
DBLVECTOR vAng( vAxRot.size()) ;
for ( size_t i = 0 ; i < vAxRot.size() ; ++ i)
vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAxRot[i] * dCoeff ;
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng, false, nStat) ;
if ( ! bOsOk || nStat != 0) {
@@ -2210,17 +2102,304 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
}
}
bFirst = false ;
// memorizzo i valori degli assi lineari come nuovi precedenti
dXprec = dX ;
dYprec = dY ;
dZprec = dZ ;
// memorizzo i valori degli angoli come nuovi precedenti
vAxRotPrec = vAng1 ;
// memorizzo i precedenti
ptPrec = ptP ;
vtDirPrec = vtDir ;
vtAuxPrec = vtAux ;
vtCorrPrec = vtCorr ;
vAxPrec = vAxVal ;
vAxRotPrec = vAxRot ;
}
return bOk ;
}
//----------------------------------------------------------------------------
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)
{
// massima variazione secondo asse rotante per continuità
const double dMaxDeltaR2 = 30 ;
// calcolo degli assi rotanti della macchina
int nRStat ;
DBLVECTOR vAng1, vAng2 ;
bool bROk = m_pMchMgr->GetCalcAngles( vtTool, vtAux, nRStat, vAng1, vAng2) ;
if ( ! bROk || nRStat == 0)
return false ;
if ( abs( nRStat) == 1) {
// se primo movimento
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]) ;
}
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa
if ( nRStat < 0 && vAng1.size() >= 1) {
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ;
break ;
}
}
}
// per movimenti successivi
else {
// scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse
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]) ;
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
}
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente
if ( nRStat < 0 && vAng1.size() >= 1) {
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
break ;
}
}
}
}
if ( abs( nRStat) == 2) {
bool bAng1 = true ;
bool bAng2 = true ;
// se primo movimento
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]) ;
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
}
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa
if ( nRStat < 0 && vAng1.size() >= 1) {
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ;
vAng2[i] = vAng1[i] ;
break ;
}
}
}
else {
// scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse
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]) ;
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
if ( bRotContOnNext)
vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ;
else
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
}
// se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente
if ( nRStat < 0 && vAng1.size() >= 1) {
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
vAng2[i] = vAng1[i] ;
break ;
}
}
// verifico che le soluzioni siano nelle corse
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng1[i]))
bAng1 = false ;
if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng2[i]))
bAng2 = false ;
}
}
// scelgo la soluzione più vicina alla precedente
double dDelta1 = 0 ;
double dDelta2 = 0 ;
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
continue ;
// calcolo i delta asse con eventuale peso
bool bFirst = ( i == 0) ;
dDelta1 += abs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ;
dDelta2 += abs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ;
}
if ( bAng2 && ( dDelta2 < dDelta1 - EPS_ANG_SMALL || ! bAng1))
swap( vAng1, vAng2) ;
// se imposto limite su variazione di secondo asse rotante
if ( ( bMaxDeltaR2OnFirst || ! bFirst) && dMaxDeltaR2 > EPS_ANG_SMALL && vAng1.size() >= 2) {
double dR2Diff1 = vAng1[1] - vAxRotPrec[1] ;
double dR2Diff2 = vAng2[1] - vAxRotPrec[1] ;
if ( abs( dR2Diff1) > abs( dR2Diff2) && abs( dR2Diff1) > dMaxDeltaR2)
swap( vAng1, vAng2) ;
}
}
vAxRot = vAng1 ;
return true ;
}
//----------------------------------------------------------------------------
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, bool& bAdded, bool& bAxError)
{
// impostazioni
bAdded = false ;
bAxError = false ;
Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
// se superato il limite di ricursioni, non devo fare alcunché
const int MAX_RECURSION = 6 ;
if ( nCnt > MAX_RECURSION)
return true ;
// calcolo variazione angolare
double dDeltaAng = 0 ;
for ( int i = 3 ; 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 ;
DBLVECTOR vAxRotMid( int( vAxVal.size() - 3)) ;
for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i)
vAxRotMid[i-3] = vAxMid[i] ;
// ricavo il punto medio del movimento
Point3d ptMoveMid ;
if ( ! pMachine->GetTipFromPositions( vAxMid[0], vAxMid[1], vAxMid[2], vAxRotMid, false, false, ptMoveMid))
return false ;
// verifico le differenze
double dDist ;
if ( ! DistPointLine( ptMoveMid, ptPrec, ptP).GetDist( dDist))
return false ;
// LOG_INFO( GetEMkLogger(), ( "Midpoint dist =" + ToString( dDist, 3)).c_str())
double dLinTol = GetApproxLinTol() ;
// Se posizione in tolleranza, non devo fare alcunché
if ( dDist <= dLinTol)
return true ;
// Va inserito il punto medio
Point3d ptMid = Media( ptPrec, ptP) ;
Vector3d vtDirMid = Media( vtDirPrec, vtDir) ;
if ( ! vtDirMid.Normalize())
return false ;
Vector3d vtAuxMid = Media( vtAuxPrec, vtAux) ;
if ( ! vtAuxPrec.IsSmall() && ! vtAux.IsSmall() && ! vtAuxMid.Normalize())
return false ;
Vector3d vtCorrMid = Media( vtCorrPrec, vtCorr) ;
if ( ! vtCorrPrec.IsSmall() && ! vtCorr.IsSmall() && ! vtCorrMid.Normalize())
return false ;
// inserisco nuova entità e modifico vecchia
int nMidEntId = m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_BEFORE) ;
if ( nMidEntId == GDB_ID_NULL)
return false ;
ICurve* pCurve = GetCurve( m_pGeomDB->GetGeoObj( nEntId)) ;
if ( pCurve == nullptr || ! pCurve->ModifyStart( ptMid))
return false ;
ICurve* pMidCurve = GetCurve( m_pGeomDB->GetGeoObj( nMidEntId)) ;
if ( pMidCurve == nullptr || ! pMidCurve->ModifyEnd( ptMid))
return false ;
CamData* pMidCamData = GetCamData( m_pGeomDB->GetUserObj( nMidEntId)) ;
if ( pMidCamData == nullptr)
return false ;
pMidCamData->SetEndPoint( ptMid) ;
pMidCamData->SetToolDir( vtDirMid) ;
pMidCamData->SetAuxDir( vtAuxMid) ;
pMidCamData->SetFlag( 401) ;
// calcolo gli assi rotanti per il punto medio
DBLVECTOR vAxRotHome( int( vAxVal.size() - 3)) ;
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)) {
pMidCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ;
return false ;
}
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) {
pMidCamData->SetAxes( CamData::AS_ERR, vAxMid) ;
bAxError = true ;
return true ;
}
// verifico i limiti di corsa degli assi
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( dMidX, dMidY, dMidZ, vAxRotMid, false, nStat) ;
if ( ! bOsOk || nStat != 0) {
pMidCamData->SetAxes( CamData::AS_OUTSTROKE, vAxMid) ;
bAxError = true ;
return true ;
}
// assegno i valori degli assi
vAxMid[0] = dMidX ;
vAxMid[1] = dMidY ;
vAxMid[2] = dMidZ ;
pMidCamData->SetAxes( CamData::AS_OK, vAxMid) ;
// calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo
Vector3d vtBackAux ; pMachine->GetBackAuxDirFromAngles( vAxRotMid, vtBackAux) ;
pMidCamData->SetBackAuxDir( vtBackAux) ;
// 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, 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, bAdded2, bAxError2))
return false ;
if ( bAxError2) {
bAxError = true ;
return true ;
}
return true ;
}
//----------------------------------------------------------------------------
bool
Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
Operation.h
+6
View File
@@ -150,6 +150,12 @@ class Operation : public IUserObj
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, 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) ;