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

EgtMachKernel 1.6e7 :

Browse Source 
- introdotto uso ObjUser per gruppi di macchina
- prima versione prototipo delle forature
- migliorie al calolo angoli e posizioni macchina.
This commit is contained in:
Dario Sassi
2015-05-26 06:49:55 +00:00
parent fe07bb767d
commit 3076dccb4b
25 changed files with 1402 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+103 -46
View File
@@ -16,6 +16,9 @@
#include "MachMgr.h"
#include "GeoCalc.h"
#include "DllMain.h"
#include "Axis.h"
#include "Head.h"
#include "Exit.h"
#include "/EgtDev/Include/EGkGeoVector3d.h"
#include "/EgtDev/Include/EGnStringUtils.h"
#include "/EgtDev/Include/EGnFileUtils.h"
@@ -46,19 +49,16 @@ Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
m_dCalcTLen = 0 ;
// recupero il gruppo della testa
int nHeadId = GetGroup( sHead) ;
if ( nHeadId == GDB_ID_NULL || ! IsHeadGroup( nHeadId))
// recupero i dati della testa
Head* pHead = GetHead( nHeadId) ;
if ( pHead == nullptr)
return false ;
// recupero il gruppo dell'uscita
string sExit = "T" + ToString( nExit) ;
string sExit = MCH_EXIT + ToString( nExit) ;
int nExitId = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nHeadId, sExit) ;
if ( nExitId == GDB_ID_NULL || m_pMchMgr->m_pGeomDB->GetGdbType( nExitId) != GDB_TY_GROUP)
return false ;
// recupero posizione e direzione a riposo
Point3d ptPos ;
if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nExitId, "Pos", ptPos))
return false ;
Vector3d vtDir ;
if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nExitId, "TDir", vtDir))
// recupero i dati dell'uscita
Exit* pExit = GetExit( nExitId) ;
if ( pExit == nullptr)
return false ;
// recupero i dati dell'utensile
if ( ! LoadTool( sHead, nExit, sTool))
@@ -73,8 +73,9 @@ Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
m_nCalcHeadId = nHeadId ;
m_nCalcExitId = nExitId ;
m_nCalcToolId = nToolId ;
m_ptCalcPos = ptPos ;
m_vtCalcDir = vtDir ;
m_ptCalcPos = pExit->GetPos() ;
m_vtCalcDir = pExit->GetTDir() ;
m_vtCalcADir = pHead->GetADir() ;
m_dCalcTLen = dTLen ;
// determino la catena cinematica
return CalculateKinematicChain() ;
@@ -147,26 +148,18 @@ Machine::CalculateKinematicChain( void)
bool
Machine::AddKinematicAxis( bool bOnHead, int nId)
{
// recupero il gestore dell'asse
Axis* pAx = GetAxis( nId) ;
if ( pAx == nullptr)
return false ;
// ne recupero i dati
KinAxis kAx ;
// assegno id
kAx.nGrpId = nId ;
// recupero il tipo di asse
string sType ;
if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Type", sType))
return false ;
kAx.bLinear = ( sType != MCH_AXIS + ToString( MCH_AT_ROTARY)) ;
// assegno posizione su catena cinematica
kAx.bHead = bOnHead ;
// recupero la posizione
if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Pos", kAx.ptPos))
return false ;
// recupero la direzione e la normalizzo
if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Dir", kAx.vtDir) || ! kAx.vtDir.Normalize())
return false ;
// recupero i limiti di corsa
string sStroke ;
if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Stroke", sStroke) || ! FromString( sStroke, kAx.stroke.v))
return false ;
kAx.bLinear = ( pAx->GetType() != MCH_AT_ROTARY) ;
kAx.bHead = bOnHead ; // posizione su catena cinematica
kAx.ptPos = pAx->GetPos() ;
kAx.vtDir = pAx->GetDir() ;
kAx.stroke = pAx->GetStroke() ;
// se lineare di tavola, devo invertirlo
if ( kAx.bLinear && ! kAx.bHead)
kAx.vtDir.Invert() ;
@@ -180,7 +173,7 @@ Machine::AddKinematicAxis( bool bOnHead, int nId)
//----------------------------------------------------------------------------
bool
Machine::GetAngles( const Vector3d& vtDirT,
Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2)
{
// annullo tutti gli angoli
@@ -194,8 +187,13 @@ Machine::GetAngles( const Vector3d& vtDirT,
Vector3d vtDirTn = vtDirT ;
if ( ! vtDirTn.Normalize())
return false ;
// direzione ausiliaria normalizzata
Vector3d vtDirAn = vtDirA ;
vtDirAn.Normalize() ;
// direzione fresa su testa a riposo
Vector3d vtDirH = m_vtCalcDir ;
// direzione ausiliaria su testa a riposo
Vector3d vtDirI = m_vtCalcADir ;
// direzione primo asse rotante
Vector3d vtAx1 = m_vCalcRotAx[0].vtDir ;
// se asse di tavola, ne inverto la direzione
@@ -206,6 +204,7 @@ Machine::GetAngles( const Vector3d& vtDirT,
// se c'è secondo asse rotante, si calcola angolo per avere il componente appena calcolato
bool bDet = true ;
Vector3d vtDirH1, vtDirH2 ;
Vector3d vtDirI1, vtDirI2 ;
if ( m_vCalcRotAx.size() == 2) {
// direzione secondo asse rotante
Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
@@ -222,10 +221,14 @@ Machine::GetAngles( const Vector3d& vtDirT,
// eseguo aggiornamento
vtDirH1 = vtDirH ;
vtDirH1.Rotate( vtAx2, dAngB1) ;
vtDirI1 = vtDirI ;
vtDirI1.Rotate( vtAx2, dAngB1) ;
}
if ( nStat == 2) {
vtDirH2 = vtDirH ;
vtDirH2.Rotate( vtAx2, dAngB2) ;
vtDirI2 = vtDirI ;
vtDirI2.Rotate( vtAx2, dAngB2) ;
}
}
// altrimenti verifico se compatibili
@@ -248,33 +251,55 @@ Machine::GetAngles( const Vector3d& vtDirT,
}
// calcolo primo angolo di rotazione per seconda soluzione
bool bDet2 = true ;
if ( nStat == 2) {
bool bDet ;
if ( ! vtDirH2.GetRotation( vtDirTn, vtAx1, dAngA2, bDet) )
if ( ! vtDirH2.GetRotation( vtDirTn, vtAx1, dAngA2, bDet2) )
nStat = 1 ;
else {
// se indeterminato ...
if ( ! bDet) {
nStat = - 2 ;
dAngA2 = 0 ;
// se indeterminato, provo a determinarlo con la direzione ausiliaria
if ( ! bDet2) {
bool bDetX ;
vtDirI2.GetRotation( vtDirAn, vtAx1, dAngA2, bDetX) ;
}
}
}
// calcolo primo angolo di rotazione per prima soluzione
bool bDet1 = true ;
if ( nStat >= 1) {
bool bDet ;
if ( ! vtDirH1.GetRotation( vtDirTn, vtAx1, dAngA1, bDet) )
if ( ! vtDirH1.GetRotation( vtDirTn, vtAx1, dAngA1, bDet1) )
nStat = 0 ;
else {
// se indeterminato ...
if ( ! bDet) {
nStat = - nStat ;
dAngA1 = 0 ;
// se indeterminato, provo a determinarlo con la direzione ausiliaria
if ( ! bDet1) {
bool bDetX ;
vtDirI1.GetRotation( vtDirAn, vtAx1, dAngA1, bDetX) ;
}
}
}
// verifiche dei limiti di corsa
if ( nStat >= 2) {
// se non riesco ad aggiustare, elimino
if ( ! AdjustAngleInStroke( 1, dAngA2) || ! AdjustAngleInStroke( 2, dAngB2))
-- nStat ;
}
if ( nStat >= 1) {
// se non riesco ad aggiustare, elimino
if ( ! AdjustAngleInStroke( 1, dAngA1) || ! AdjustAngleInStroke( 2, dAngB1)) {
-- nStat ;
// riloco eventuale soluzione rimasta
if ( nStat >= 1) {
dAngA1 = dAngA2 ;
dAngB1 = dAngB2 ;
}
}
}
// modifico stato per angolo indeterminato
if ( ( nStat >= 2 && ! bDet2) || ( nStat >= 1 && ! bDet1))
nStat = - nStat ;
return true ;
}
@@ -310,6 +335,9 @@ Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB,
vtDirH.Rotate( vtAx1, dAngA) ;
}
// assegno l'offset tesat
Vector3d vtDtHe = ORIG - m_ptCalcPos ;
// calcolo il recupero degli assi : è l'opposto dello spostamento della posizione
Vector3d vtDtAx = m_ptCalcPos - ptPosH ;
@@ -317,13 +345,42 @@ Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB,
Vector3d vtDtTL = vtDirH * m_dCalcTLen ;
// calcolo le posizioni degli assi lineari
dX = ptP.x + vtDtAx.x + vtDtTL.x ;
dY = ptP.y + vtDtAx.y + vtDtTL.y ;
dZ = ptP.z + vtDtAx.z + vtDtTL.z ;
dX = ptP.x + vtDtHe.x + vtDtAx.x + vtDtTL.x ;
dY = ptP.y + vtDtHe.y + vtDtAx.y + vtDtTL.y ;
dZ = ptP.z + vtDtHe.z + vtDtAx.z + vtDtTL.z ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::AdjustAngleInStroke( int nId, double& dAng)
{
// se non ci sono assi rotanti, non c'è alcunchè da fare
if ( m_vCalcRotAx.size() == 0)
return true ;
// se primo angolo ed esiste primo asse rotante
if ( nId == 1 && m_vCalcRotAx.size() >= 1) {
while ( dAng < m_vCalcRotAx[0].stroke.Min)
dAng += ANG_FULL ;
while ( dAng > m_vCalcRotAx[0].stroke.Max)
dAng -= ANG_FULL ;
return ( dAng >= m_vCalcRotAx[0].stroke.Min &&
dAng <= m_vCalcRotAx[0].stroke.Max) ;
}
// se secondo angolo ed esiste secondo asse rotante
if ( nId == 2 && m_vCalcRotAx.size() >= 2) {
while ( dAng < m_vCalcRotAx[1].stroke.Min)
dAng += ANG_FULL ;
while ( dAng > m_vCalcRotAx[1].stroke.Max)
dAng -= ANG_FULL ;
return ( dAng >= m_vCalcRotAx[1].stroke.Min &&
dAng <= m_vCalcRotAx[1].stroke.Max) ;
}
// errore
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat)