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

EgtMachKernel 2.4d2 :

Browse Source 
- modifiche per gestire macchina con asse rotante tipo ralla che porta due assi lineari ad esso perpendicolare (terzo lineare diretto come ralla).
This commit is contained in:
DarioS
2022-04-19 11:24:41 +02:00
parent 33090bc9e0
commit 6bb714e581
6 changed files with 151 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+135 -66
View File
@@ -174,6 +174,12 @@ Machine::GetCurrTableIsTilting( bool& bTilting) const
// verifico esistenza tavola
if ( m_nCalcTabId == GDB_ID_NULL)
return false ;
// verifico se presente flag che lo forza come tale
bool bTiltingLike ;
if ( m_pGeomDB->GetInfo( m_nCalcTabId, MCH_TILTINGLIKE, bTiltingLike) && bTiltingLike) {
bTilting = true ;
return true ;
}
// recupero gli eventuali assi rotanti della tavola
int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ;
if ( nTParId == GDB_ID_NULL)
@@ -443,6 +449,11 @@ Machine::CalculateKinematicChain( void)
if ( m_pGeomDB == nullptr)
return false ;
// azzero tutti gli assi della catena cinematica
m_nTabLinAxes = 0 ;
m_nTabRotAxes = 0 ;
m_nHeadLinAxes = 0 ;
m_nHeadRotAxes = 0 ;
m_nHeadSpecRotAxis = -1 ;
m_vCalcLinAx.clear() ;
m_vCalcRotAx.clear() ;
// recupero gli assi di tavola
@@ -451,6 +462,8 @@ Machine::CalculateKinematicChain( void)
int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ;
if ( nTParId == GDB_ID_NULL)
return false ;
m_nTabLinAxes = 0 ;
m_nTabRotAxes = 0 ;
while ( IsAxisGroup( nTParId)) {
if ( ! AddKinematicAxis( false, nTParId))
return false ;
@@ -462,12 +475,22 @@ Machine::CalculateKinematicChain( void)
int nHParId = m_pGeomDB->GetParentId( m_nCalcHeadId) ;
if ( nHParId == GDB_ID_NULL)
return false ;
m_nHeadLinAxes = 0 ;
m_nHeadRotAxes = 0 ;
while ( IsAxisGroup( nHParId)) {
if ( ! AddKinematicAxis( true, nHParId))
return false ;
nHParId = m_pGeomDB->GetParentId( nHParId) ;
}
// verifiche sugli assi lineari :
// aggiusto gli indici di ordine sulla sua catena cinematica (1-based)
for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) {
if ( m_vCalcLinAx[i].bHead)
m_vCalcLinAx[i].nInd += m_nHeadLinAxes + m_nHeadRotAxes + 1 ;
else
m_vCalcLinAx[i].nInd *= -1 ;
}
// devono essere 3
if ( m_vCalcLinAx.size() != 3)
return false ;
@@ -490,12 +513,21 @@ Machine::CalculateKinematicChain( void)
return false ;
}
}
// verifiche sugli assi rotanti :
bool bOk = false ;
// aggiusto gli indici di ordine sulla sua catena cinematica (1-based)
for ( int i = 0 ; i < int( m_vCalcRotAx.size()) ; ++ i) {
if ( m_vCalcRotAx[i].bHead)
m_vCalcRotAx[i].nInd += m_nHeadLinAxes + m_nHeadRotAxes + 1 ;
else
m_vCalcRotAx[i].nInd *= -1 ;
}
// se 0 o 1 va bene
if ( m_vCalcRotAx.size() <= 1)
return true ;
bOk = true ;
// se 2 va bene
if ( m_vCalcRotAx.size() == 2) {
else if ( m_vCalcRotAx.size() == 2) {
// se entrambi di testa devo invertirne l'ordine
if ( m_vCalcRotAx[0].bHead && m_vCalcRotAx[1].bHead)
swap( m_vCalcRotAx[0], m_vCalcRotAx[1]) ;
@@ -507,10 +539,10 @@ Machine::CalculateKinematicChain( void)
m_vCalcRotAx[1].stroke.Max = min( m_vCalcRotAx[1].stroke.Max, pHead->GetRot2Stroke().Max) ;
}
}
return true ;
bOk = true ;
}
// se 3 va bene ( uno dovrà poi avere valore assegnato)
if ( m_vCalcRotAx.size() == 3) {
else if ( m_vCalcRotAx.size() == 3) {
int n2ndHeadRotAx = - 1 ;
// se tutti e tre di testa, devo invertire il primo con il terzo
if ( m_vCalcRotAx[0].bHead && m_vCalcRotAx[1].bHead && m_vCalcRotAx[2].bHead) {
@@ -530,10 +562,48 @@ Machine::CalculateKinematicChain( void)
m_vCalcRotAx[n2ndHeadRotAx].stroke.Max = min( m_vCalcRotAx[n2ndHeadRotAx].stroke.Max, pHead->GetRot2Stroke().Max) ;
}
}
return true ;
bOk = true ;
}
if ( ! bOk)
return false ;
// verifico esistenza eventuale asse rotante speciale di testa
if ( m_nHeadRotAxes > 0 && m_nHeadLinAxes > 0) {
// indice di posizione primo asse di testa
int nHeadFirst = 1 ;
// ricerco sui rotanti
for ( int i = 0 ; i < int( m_vCalcRotAx.size()) ; ++ i) {
// se asse di testa
if ( m_vCalcRotAx[i].bHead && m_vCalcRotAx[i].nInd <= nHeadFirst) {
// non sono ammessi due assi di questo tipo
if ( m_nHeadSpecRotAxis != -1)
return false ;
// la tavola non deve avere più di un asse lineare
if ( m_nTabLinAxes > 1)
return false ;
// se ha un asse lineare deve essere allineato con il rotante speciale
else if ( m_nTabLinAxes == 1) {
// ne recupero la direzione
Vector3d vtTabLinDir ;
for ( int j = 0 ; j < int( m_vCalcLinAx.size()) ; ++ j) {
if ( ! m_vCalcLinAx[i].bHead) {
vtTabLinDir = m_vCalcLinAx[i].vtDir ;
break ;
}
}
// la confronto con quella dell'asse rotante speciale
if ( ! AreSameOrOppositeVectorApprox( vtTabLinDir, m_vCalcRotAx[i].vtDir))
return false ;
}
// ne salvo l'indice
m_nHeadSpecRotAxis = i ;
// incremento indice di posizione posibile primo asse lineare di testa
++ nHeadFirst ;
}
}
}
return true ;
// altrimenti non ancora gestito, quindi errore
// altrimenti non ancora gestito, quindi errore
LOG_ERROR( GetEMkLogger(), "Rotary Axes not manageable")
return false ;
}
@@ -553,11 +623,20 @@ Machine::AddKinematicAxis( bool bOnHead, int nId)
KinAxis kAx ;
kAx.nGrpId = nId ;
kAx.bLinear = ( pAx->GetType() != MCH_AT_ROTARY) ;
kAx.bHead = bOnHead ; // posizione su catena cinematica
kAx.bHead = bOnHead ; // catena cinematica di appartenenza (testa o tavola)
kAx.ptPos = pAx->GetPos() ;
kAx.vtDir = pAx->GetDir() ;
kAx.stroke = pAx->GetStroke() ;
kAx.dHomeVal = pAx->GetHomeVal() ;
// ne determino l'indice di posizione nella sua catena cinematica (assegno valore negato perchè provvisorio)
if ( kAx.bHead) {
( kAx.bLinear ? ++ m_nHeadLinAxes : ++ m_nHeadRotAxes) ;
kAx.nInd = - ( m_nHeadLinAxes + m_nHeadRotAxes) ;
}
else {
( kAx.bLinear ? ++ m_nTabLinAxes : ++ m_nTabRotAxes) ;
kAx.nInd = - ( m_nTabLinAxes + m_nTabRotAxes) ;
}
// se lineare di tavola, devo invertirlo
if ( kAx.bLinear && ! kAx.bHead)
kAx.vtDir.Invert() ;
@@ -1025,26 +1104,33 @@ Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng,
// aggiorno punto di lavoro mediante ciclo diretto sugli assi di tavola
Point3d ptW = ptP ;
// annullo la posizione home degli assi lineari
for ( size_t i = 0 ; i < m_vCalcLinAx.size() ; ++ i) {
for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) {
// se asse di tavola
if ( ! m_vCalcLinAx[i].bHead)
ptW.Translate( - m_vCalcLinAx[i].dHomeVal * ( - m_vCalcLinAx[i].vtDir)) ;
}
// effettuo rotazione diminuita della posizione home degli assi rotanti
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
for ( int i = 0 ; i < int( m_vCalcRotAx.size()) ; ++ i) {
// se asse di tavola
if ( ! m_vCalcRotAx[i].bHead)
ptW.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i] - m_vCalcRotAx[i].dHomeVal) ;
}
// effettuo rotazione inversa per asse rotante di testa speciale
if ( m_nHeadSpecRotAxis != -1) {
if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size()))
return false ;
int i = m_nHeadSpecRotAxis ;
ptW.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, -vAng[i]) ;
}
// aggiorno posizione e direzione fresa su testa a riposo mediante ciclo inverso sugli assi di testa
Point3d ptPosH = m_ptCalcPos ;
Vector3d vtDirH = m_vtCalcDir ;
for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) {
// se asse di testa
if ( m_vCalcRotAx[i-1].bHead) {
ptPosH.Rotate( m_vCalcRotAx[i-1].ptPos, m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
vtDirH.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) {
// se asse di testa non speciale
if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis) {
ptPosH.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
vtDirH.Rotate( m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
}
@@ -1067,46 +1153,6 @@ Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng,
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetHeadOffsetDelta( const DBLVECTOR& vAng,
double& dRecX, double& dRecY, double& dRecZ) const
{
// ovviamente tutto è espresso nel riferimento ZERO MACCHINA
// verifico che siano stati assegnati gli angoli necessari, altrimenti errore
if ( vAng.size() < m_vCalcRotAx.size())
return false ;
// aggiorno posizione e direzione fresa su testa a riposo mediante ciclo inverso sugli assi di testa
Point3d ptPosH = m_ptCalcPos ;
Vector3d vtDirH = m_vtCalcDir ;
for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) {
// se asse di testa
if ( m_vCalcRotAx[i-1].bHead) {
ptPosH.Rotate( m_vCalcRotAx[i-1].ptPos, m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
vtDirH.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
}
}
// assegno l'offset testa
Vector3d vtDtHe = ORIG - m_ptCalcPos ;
// calcolo il recupero degli assi : è l'opposto dello spostamento della posizione
Vector3d vtDtAx = m_ptCalcPos - ptPosH ;
// calcolo il recupero di lunghezza utensile
Vector3d vtDtTL = vtDirH * m_dCalcTLen ;
// calcolo le posizioni degli assi lineari
dRecX = vtDtHe.x + vtDtAx.x + vtDtTL.x ;
dRecY = vtDtHe.y + vtDtAx.y + vtDtTL.y ;
dRecZ = vtDtHe.z + vtDtAx.z + vtDtTL.z ;
// tutto ok
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& vtNew) const
@@ -1166,17 +1212,25 @@ Machine::GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR&
// aggiorno posizione testa a riposo mediante ciclo inverso sugli assi rotanti di testa
ptNose = m_ptCalcPos ;
for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) {
// se asse di testa
if ( m_vCalcRotAx[i-1].bHead)
ptNose.Rotate( m_vCalcRotAx[i-1].ptPos, m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) {
// se asse di testa non speciale
if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis)
ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
// aggiorno posizione testa con assi lineari di testa
DBLVECTOR vMov( {dX, dY, dZ}) ;
for ( size_t i = 1 ; i <= m_vCalcLinAx.size() ; ++ i) {
if ( m_vCalcLinAx[i-1].bHead)
ptNose += m_vCalcLinAx[i-1].vtDir * vMov[i-1] ;
for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) {
if ( m_vCalcLinAx[i].bHead)
ptNose += m_vCalcLinAx[i].vtDir * vMov[i] ;
}
// eseguo rotazione eventuale asse rotante speciale di testa
if ( m_nHeadSpecRotAxis != -1) {
if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size()))
return false ;
int i = m_nHeadSpecRotAxis ;
ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
return true ;
}
@@ -1188,11 +1242,26 @@ Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR&
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa)
// Calcoli recuperi della testa e lunghezza utensile per orientamento
double dRecX, dRecY, dRecZ ;
if ( ! GetHeadOffsetDelta( vAng, dRecX, dRecY, dRecZ))
return false ;
ptTip.Set( dX - dRecX, dY - dRecY, dZ - dRecZ) ;
// 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) {
// se asse di testa non speciale
if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis)
ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
// aggiorno posizione tip utensile con assi lineari di testa
DBLVECTOR vMov( {dX, dY, dZ}) ;
for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) {
if ( m_vCalcLinAx[i].bHead)
ptTip += m_vCalcLinAx[i].vtDir * vMov[i] ;
}
// eseguo rotazione eventuale asse rotante speciale di testa
if ( m_nHeadSpecRotAxis != -1) {
if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size()))
return false ;
int i = m_nHeadSpecRotAxis ;
ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
// Se richiesto ingombro totale o punto sotto del tip utensile
if ( bOverall || bBottom) {