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

EgtMachKernel :

Browse Source 
- corretta AddRobotClimb (aggiungeva entità in posizione non corretta)
- aggiunta gestione soluzione indeterminata con tre assi rotanti liberi.
This commit is contained in:
Dario Sassi
2026-04-09 20:17:50 +02:00
parent 40f1e051a4
commit 932700d4b2
5 changed files with 86 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Operation.cpp
+47 -36
View File
@@ -2354,34 +2354,41 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con
double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot)
{
// 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) ;
// massima variazione secondo asse rotante per continuità
const double dMaxDeltaR2 = 30 ;
// imposto primo asse precedente (per caso con 3 rotanti liberi)
pMachine->SetPrevAngA( ( ! vAxRotPrec.empty() ? vAxRotPrec[0] : NAN)) ;
// calcolo degli assi rotanti della macchina
int nRStat ;
DBLVECTOR vAng1, vAng2 ;
bool bROk = m_pMchMgr->GetCalcAngles( vtTool, vtAux, nRStat, vAng1, vAng2) ;
bool bROk = pMachine->GetAngles( vtTool, vtAux, nRStat, vAng1, vAng2) ;
pMachine->SetPrevAngA( NAN) ;
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]) ;
for ( int i = 0 ; i < ssize( 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]) ;
}
// 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) {
for ( int i = 0 ; i < ssize( vAng1) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
if ( pMachine->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ;
pMachine->LimitAngleToStroke( i, vAng1[i]) ;
break ;
}
}
@@ -2389,19 +2396,19 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con
// 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) {
for ( int i = 0 ; i < ssize( vAng1) ; ++ i) {
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]) ;
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
pMachine->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) {
for ( int i = 0 ; i < ssize( vAng1) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
if ( pMachine->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
@@ -2416,23 +2423,23 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con
// 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]) ;
for ( int i = 0 ; i < ssize( vAng1) ; ++ i) {
pMachine->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]) ;
pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[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]) ;
}
// 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) {
for ( int i = 0 ; i < ssize( vAng1) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
if ( pMachine->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ;
pMachine->LimitAngleToStroke( i, vAng1[i]) ;
vAng2[i] = vAng1[i] ;
break ;
}
@@ -2440,25 +2447,25 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con
}
else {
// scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
for ( int i = 0 ; i < ssize( vAng1) ; ++ i) {
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]) ;
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[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]) ;
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
pMachine->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) {
for ( int i = 0 ; i < ssize( vAng1) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
if ( pMachine->IsKinematicRotAxisBlocked( i))
continue ;
// assegno il precedente ed esco
vAng1[i] = vAxRotPrec[i] ;
@@ -2467,29 +2474,29 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con
}
}
// verifico che le soluzioni siano nelle corse
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng1[i]))
for ( int i = 0 ; i < ssize( vAng1) ; ++ i) {
if ( ! pMachine->VerifyAngleOutstroke( i, vAng1[i]))
bAng1 = false ;
if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng2[i]))
if ( ! pMachine->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) {
for ( int i = 0 ; i < ssize( vAng1) ; ++ i) {
// ignoro gli assi bloccati
if ( m_pMchMgr->IsKinematicRotAxisBlocked( i))
if ( pMachine->IsKinematicRotAxisBlocked( i))
continue ;
// calcolo i delta asse con eventuale peso
bool bFirst = ( i == 0) ;
dDelta1 += abs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? m_pMchMgr->GetCalcRot1W() : 1) ;
dDelta2 += abs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? m_pMchMgr->GetCalcRot1W() : 1) ;
dDelta1 += abs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? pMachine->GetCurrRot1W() : 1) ;
dDelta2 += abs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? pMachine->GetCurrRot1W() : 1) ;
}
if ( bAng2 && ( dDelta2 < dDelta1 - EPS_ANG_SMALL || ! bAng1))
swap( vAng1, vAng2) ;
// se imposto limite su variazione di secondo asse rotante
if ( ( m_pMchMgr->GetCalcMaxDeltaR2OnFirst() || ! bFirst) && dMaxDeltaR2 > EPS_ANG_SMALL && vAng1.size() >= 2) {
if ( ( pMachine->GetCurrCalcMaxDeltaR2OnFirst() || ! 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)
@@ -4943,6 +4950,10 @@ Operation::AddRobotClimb( int nEntId, double dDeltaZ)
int nNewEntId = m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_BEFORE) ;
if ( nNewEntId == GDB_ID_NULL)
return false ;
// aggiorno il CamData
pCdEnt = GetCamData( m_pGeomDB->GetUserObj( nNewEntId)) ;
if ( pCdEnt == nullptr)
return false ;
// modifico quella originale (è la prima del percorso)
m_pGeomDB->SetName( nNewEntId, MCH_CL_CLIMB) ;
m_pGeomDB->RemoveInfo( nNewEntId, KEY_CL_DOUBLE) ;
@@ -4954,7 +4965,7 @@ Operation::AddRobotClimb( int nEntId, double dDeltaZ)
double dStep = dNewDeltaZ / nStep ;
for ( int i = 1 ; i < nStep ; ++ i) {
// copio l'entità
int nNewId = m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) ;
int nNewId = m_pGeomDB->Copy( nNewEntId, GDB_ID_NULL, nNewEntId, GDB_AFTER) ;
if ( nNewId == GDB_ID_NULL)
return false ;
// calcolo la posizione sopra il punto a Zmax