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

EgtMachKernel :

Browse Source 
- corretto recupero testa
- migliorata gestione angoli indeterminati
- migliorato controllo massimo affondamento in fresatura.
This commit is contained in:
Dario Sassi
2016-09-21 06:22:32 +00:00
parent 4c884e8ee5
commit d01434d783
4 changed files with 49 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+42 -69
View File
@@ -792,71 +792,8 @@ bool
Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB,
int& nStat, double& dX, double& dY, double& dZ) const
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// posizione punto di lavoro con pezzo immobile
Point3d ptW = ptP ;
// se c'è primo asse rotante di tavola
if ( m_vCalcRotAx.size() >= 1 && ! m_vCalcRotAx[0].bHead) {
// posizione e direzione primo asse rotante
Point3d ptAx1 = m_vCalcRotAx[0].ptPos ;
Vector3d vtAx1 = m_vCalcRotAx[0].vtDir ;
// ruoto punto di lavoro
ptW.Rotate( ptAx1, vtAx1, dAngA) ;
}
// se c'è secondo asse rotante di tavola
if ( m_vCalcRotAx.size() >= 2 && ! m_vCalcRotAx[1].bHead) {
// posizione e direzione primo asse rotante
Point3d ptAx2 = m_vCalcRotAx[1].ptPos ;
Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
// ruoto punto di lavoro
ptW.Rotate( ptAx2, vtAx2, dAngB) ;
}
// posizione e direzione fresa su testa a riposo
Point3d ptPosH = m_ptCalcPos ;
Vector3d vtDirH = m_vtCalcDir ;
// se c'è secondo asse rotante di testa
if ( m_vCalcRotAx.size() >= 2 && m_vCalcRotAx[1].bHead) {
// posizione e direzione primo asse rotante
Point3d ptAx2 = m_vCalcRotAx[1].ptPos ;
Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
// ruoto dati a riposo
ptPosH.Rotate( ptAx2, vtAx2, dAngB) ;
vtDirH.Rotate( vtAx2, dAngB) ;
}
// se c'è primo asse rotante di testa
if ( m_vCalcRotAx.size() >= 1 && m_vCalcRotAx[0].bHead) {
// posizione e direzione primo asse rotante
Point3d ptAx1 = m_vCalcRotAx[0].ptPos ;
Vector3d vtAx1 = m_vCalcRotAx[0].vtDir ;
// ruoto dati a riposo
ptPosH.Rotate( ptAx1, vtAx1, dAngA) ;
vtDirH.Rotate( vtAx1, dAngA) ;
}
// 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
dX = ptW.x + vtDtHe.x + vtDtAx.x + vtDtTL.x ;
dY = ptW.y + vtDtHe.y + vtDtAx.y + vtDtTL.y ;
dZ = ptW.z + vtDtHe.z + vtDtAx.z + vtDtTL.z ;
// tutto ok
nStat = 0 ;
return true ;
DBLVECTOR vAng( 2) ; vAng[0] = dAngA ; vAng[1] = dAngB ;
return GetPositions( ptP, vAng, nStat, dX, dY, dZ) ;
}
//----------------------------------------------------------------------------
@@ -904,11 +841,47 @@ Machine::GetPositions( const Point3d& ptP, DBLVECTOR vAng,
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetHeadOffsetDelta( DBLVECTOR vAng,
double& dRecX, double& dRecY, double& dRecZ) const
{
// ovviamente tutto è espresso nel riferimento ZERO MACCHINA
// 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, double dAngA, double dAngB, Vector3d& vtNew) const
{
// per ora gestisco solo gli assi rotanti di testa
// è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa)
// direzione a riposo
vtNew = vtDir ;
@@ -939,12 +912,12 @@ Machine::GetTipFromPositions( double dX, double dY, double dZ, double dAngA, dou
bool bOverall, bool bBottom, Point3d& ptTip) const
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// per ora gestisco solo gli assi rotanti di testa
// è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa)
// Calcoli recuperi della testa e lunghezza utensile per orientamento
int nStat ;
double dRecX, dRecY, dRecZ ;
if ( ! GetPositions( ORIG, dAngA, dAngB, nStat, dRecX, dRecY, dRecZ))
DBLVECTOR vAng( 2) ; vAng[0] = dAngA ; vAng[1] = dAngB ;
if ( ! GetHeadOffsetDelta( vAng, dRecX, dRecY, dRecZ))
return false ;
ptTip.Set( dX - dRecX, dY - dRecY, dZ - dRecZ) ;