Egalware/EgtMachKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
31b4099d0f461da123759943e8dd383d775cfb16
EgtMachKernel/MachineCalc.cpp
T
Dario Sassi 31b4099d0f EgtMachKernel 1.6r1 : 
- in SplitFlatRawPartW se c'è curva di kerf si splitta e si riporta nei nuovi grezzi
- in Simulator migliorata gestione Home e Stop, corretto posizione precedente degli assi al cambio utensile/testa.
2016-05-12 15:27:25 +00:00

810 lines
27 KiB
C++
Raw Blame History

//----------------------------------------------------------------------------
// EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : MachineCalc.cpp Data : 12.05.15 Versione : 1.6e3
// Contenuto : Implementazione gestione macchina : funzioni di calcolo.
//
//
//
// Modifiche : 12.05.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------
//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "MachMgr.h"
#include "GeoCalc.h"
#include "DllMain.h"
#include "Table.h"
#include "Axis.h"
#include "Head.h"
#include "Exit.h"
#include "/EgtDev/Include/EGkGeoVector3d.h"
#include "/EgtDev/Include/EMkToolConst.h"
#include "/EgtDev/Include/EGnStringUtils.h"
#include "/EgtDev/Include/EGnFileUtils.h"
using namespace std ;
//----------------------------------------------------------------------------
static const string EMC_VAR = "EMC" ; // tabella variabili locali per calcolo
static const string EVAR_TABNAME = ".TABNAME" ; // (string) nome della tavola macchina
static const string ON_SET_TABLE = "OnSetTable" ;
//----------------------------------------------------------------------------
bool
Machine::SetCurrTable( const string& sTable)
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero il gruppo della tavola
m_nCalcTabId = GetGroup( sTable) ;
if ( m_nCalcTabId == GDB_ID_NULL || ! IsTableGroup( m_nCalcTabId)) {
m_nCalcTabId = GDB_ID_NULL ;
return false ;
}
// lancio eventuale funzione lua di personalizzazione
if ( LuaExistsFunction( ON_SET_TABLE)) {
bool bOk = LuaCreateGlobTable( EMC_VAR) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TABNAME, sTable) ;
bOk = bOk && LuaCallFunction( ON_SET_TABLE) ;
bOk = bOk && LuaResetGlobVar( EMC_VAR) ;
return bOk ;
}
else
return true ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrTable( void) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return GDB_ID_NULL ;
// recupero identificativo della tavola corrente
return m_nCalcTabId ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTable( string& sTable) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero nome della tavola corrente
return m_pGeomDB->GetName( m_nCalcTabId, sTable) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTableRef1( Point3d& ptRef1) const
{
Table* pTab = GetTable( m_nCalcTabId) ;
if ( pTab == nullptr)
return false ;
ptRef1 = pTab->GetRef1() ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTableArea1( BBox3d& b3Area1) const
{
Table* pTab = GetTable( m_nCalcTabId) ;
if ( pTab == nullptr)
return false ;
b3Area1 = pTab->GetArea1() ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// azzero tutto
m_nCalcHeadId = GDB_ID_NULL ;
m_nCalcExitId = GDB_ID_NULL ;
m_nCalcToolId = GDB_ID_NULL ;
m_dCalcRot1W = 1 ;
m_nCalcSolCh = MCH_SCC_NONE ;
m_dCalcTLen = 0 ;
m_dCalcTRad = 0 ;
// recupero il gruppo della testa
int nHeadId = GetGroup( sHead) ;
// recupero i dati della testa
Head* pHead = GetHead( nHeadId) ;
if ( pHead == nullptr) {
string sOut = "Missing head " + sHead ;
LOG_INFO( GetEMkLogger(), sOut.c_str())
return false ;
}
// recupero il gruppo dell'uscita
string sExit = MCH_EXIT + ToString( nExit) ;
int nExitId = m_pGeomDB->GetFirstNameInGroup( nHeadId, sExit) ;
// recupero i dati dell'uscita
Exit* pExit = GetExit( nExitId) ;
if ( pExit == nullptr) {
string sOut = "Missing exit " + ToString( nExit) + " on head " + sHead ;
LOG_INFO( GetEMkLogger(), sOut.c_str())
return false ;
}
// recupero i dati dell'utensile
int nToolId = GDB_ID_NULL ;
double dTLen = 0 ;
double dTDiam = 0 ;
// se definito
if ( ! sTool.empty()) {
if ( ! LoadTool( sHead, nExit, sTool))
return false ;
nToolId = m_pGeomDB->GetFirstNameInGroup( nExitId, sTool) ;
if ( nToolId == GDB_ID_NULL || m_pGeomDB->GetGdbType( nToolId) != GDB_TY_GROUP)
return false ;
if ( ! m_pMchMgr->TdbSetCurrTool( sTool) ||
! m_pMchMgr->TdbGetCurrToolParam( TPA_LEN, dTLen) ||
! m_pMchMgr->TdbGetCurrToolParam( TPA_DIAM, dTDiam))
return false ;
}
// altrimenti casi speciali senza utensile
else {
// tolgo eventuale utensile dalla testa
if ( ! ResetHeadSet( sHead))
return false ;
nToolId = GDB_ID_NULL ;
dTLen = 0 ;
dTDiam = 0 ;
m_pMchMgr->TdbSetCurrTool( sTool) ;
}
// assegno tutti i dati
m_nCalcHeadId = nHeadId ;
m_nCalcExitId = nExitId ;
m_nCalcToolId = nToolId ;
m_dCalcRot1W = pHead->GetRot1W() ;
m_nCalcSolCh = pHead->GetSolCh() ;
m_ptCalcPos = pExit->GetPos() ;
m_vtCalcDir = pExit->GetTDir() ;
m_vtCalcADir = pHead->GetADir() ;
m_dCalcTLen = dTLen ;
m_dCalcTRad = dTDiam / 2 ;
// determino la catena cinematica
return CalculateKinematicChain() ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrTool( void) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return GDB_ID_NULL ;
// recupero identificativo dell'utensile
return m_nCalcToolId ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTool( string& sTool) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero nome gruppo dell'utensile
return m_pGeomDB->GetName( m_nCalcToolId, sTool) ;
}
//----------------------------------------------------------------------------
double
Machine::GetCurrRot1W( void) const
{
// restituisco il peso del primo asse rotante nei confronti di movimento
return m_dCalcRot1W ;
}
//----------------------------------------------------------------------------
bool
Machine::CalculateKinematicChain( void)
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// azzero tutti gli assi della catena cinematica
m_vCalcLinAx.clear() ;
m_vCalcRotAx.clear() ;
// recupero gli assi di tavola
if ( m_nCalcTabId == GDB_ID_NULL)
return false ;
int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ;
if ( nTParId == GDB_ID_NULL)
return false ;
while ( IsAxisGroup( nTParId)) {
if ( ! AddKinematicAxis( false, nTParId))
return false ;
nTParId = m_pGeomDB->GetParentId( nTParId) ;
}
// recupero gli assi di testa
if ( m_nCalcHeadId == GDB_ID_NULL)
return false ;
int nHParId = m_pGeomDB->GetParentId( m_nCalcHeadId) ;
if ( nHParId == GDB_ID_NULL)
return false ;
while ( IsAxisGroup( nHParId)) {
if ( ! AddKinematicAxis( true, nHParId))
return false ;
nHParId = m_pGeomDB->GetParentId( nHParId) ;
}
// verifiche sugli assi lineari :
// devono essere 3
if ( m_vCalcLinAx.size() != 3)
return false ;
// devono essere ordinabili come XYZ
if ( ! m_vCalcLinAx[0].vtDir.IsXplus()) {
if ( m_vCalcLinAx[1].vtDir.IsXplus())
swap( m_vCalcLinAx[0], m_vCalcLinAx[1]) ;
else if ( m_vCalcLinAx[2].vtDir.IsXplus())
swap( m_vCalcLinAx[0], m_vCalcLinAx[2]) ;
else
return false ;
}
if ( ! m_vCalcLinAx[1].vtDir.IsYplus()) {
if ( m_vCalcLinAx[2].vtDir.IsYplus())
swap( m_vCalcLinAx[1], m_vCalcLinAx[2]) ;
else
return false ;
}
// verifiche sugli assi rotanti :
// se 0 o 1 va bene
if ( m_vCalcRotAx.size() <= 1)
return true ;
// se 2 va bene
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]) ;
// impongo limiti di corsa sul secondo asse rotante di testa
if ( m_vCalcRotAx[1].bHead) {
Head* pHead = GetHead( m_nCalcHeadId) ;
if ( pHead != nullptr) {
m_vCalcRotAx[1].stroke.Min = max( m_vCalcRotAx[1].stroke.Min, pHead->GetRot2Stroke().Min) ;
m_vCalcRotAx[1].stroke.Max = min( m_vCalcRotAx[1].stroke.Max, pHead->GetRot2Stroke().Max) ;
}
}
return true ;
}
// altrimenti non ancora gestito, quindi errore
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::AddKinematicAxis( bool bOnHead, int nId)
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero il gestore dell'asse
Axis* pAx = GetAxis( nId) ;
if ( pAx == nullptr)
return false ;
// ne recupero i dati
KinAxis kAx ;
kAx.nGrpId = nId ;
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() ;
// lo inserisco nella opportuna lista degli assi
if ( kAx.bLinear)
m_vCalcLinAx.emplace_back( kAx) ;
else
m_vCalcRotAx.emplace_back( kAx) ;
return true ;
}
//----------------------------------------------------------------------------
string
Machine::GetKinematicAxis( int nInd) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return "" ;
// controllo indice
int nLinAxTot = int( m_vCalcLinAx.size()) ;
int nRotAxTot = int( m_vCalcRotAx.size()) ;
if ( nInd < 0 || nInd >= nLinAxTot + nRotAxTot)
return "" ;
// recupero nome
string sName ;
if ( nInd < nLinAxTot)
m_pGeomDB->GetName( m_vCalcLinAx[nInd].nGrpId, sName) ;
else
m_pGeomDB->GetName( m_vCalcRotAx[nInd-nLinAxTot].nGrpId, sName) ;
return sName ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const
{
// annullo tutti gli angoli
nStat = 0 ; dAngA1 = 0 ; dAngB1 = 0 ; dAngA2 = 0 ; dAngB2 = 0 ;
// se nessun asse rotante, non c'è alcunchè da calcolare
if ( m_vCalcRotAx.size() == 0) {
nStat = 1 ;
return true ;
}
// direzione fresa normalizzata
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
if ( ! m_vCalcRotAx[0].bHead)
vtAx1.Invert() ;
// componente versore fresa desiderato su direzione primo asse rotante
double dCompTSuAxR1 = vtDirTn * vtAx1 ;
// 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 ;
// se asse di tavola, ne inverto la direzione
if ( ! m_vCalcRotAx[1].bHead)
vtAx2.Invert() ;
// calcolo secondo angolo di rotazione
nStat = GetRotationComponent( vtDirH, dCompTSuAxR1, vtAx1, vtAx2, dAngB1, dAngB2, bDet) ;
// aggiornamento direzioni fresa e ausiliaria su testa
if ( nStat >= 1) {
// se indeterminato lo azzero
if ( ! bDet)
dAngB1 = 0 ;
// 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
else {
// componente versore utensile su direzione primo asse
double dCompHSuAxR1 = vtDirH * vtAx1 ;
// componenti versori fresa e utensile perpendicolari direzione primo asse
double dTemp = 1 - dCompTSuAxR1 * dCompTSuAxR1 ;
double dCompTOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ;
dTemp = 1 - dCompHSuAxR1 * dCompHSuAxR1 ;
double dCompHOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ;
// verifica ( max delta angolare < 0.002 deg)
const double SIN_ANG_ERROR = sin( 0.002 * DEGTORAD) ;
if ( fabs( dCompTOrtAxR1 * dCompHSuAxR1 - dCompHOrtAxR1 * dCompTSuAxR1) < SIN_ANG_ERROR) {
nStat = 1 ;
vtDirH1 = vtDirH ;
vtDirI1 = vtDirI ;
// reset secondo angolo
dAngB1 = 0 ;
}
}
// calcolo primo angolo di rotazione per seconda soluzione
bool bDet2 = true ;
if ( nStat == 2) {
if ( ! vtDirH2.GetRotation( vtDirTn, vtAx1, dAngA2, bDet2) )
nStat = 1 ;
else {
// se indeterminato, provo a determinarlo con la direzione ausiliaria
if ( ! bDet2) {
bool bDetX ;
vtDirI2.GetRotation( vtDirAn, vtAx1, dAngA2, bDetX) ;
if ( ! bDetX)
dAngA2 = 0 ;
}
}
// aggiornamento direzioni fresa e ausiliaria su testa
vtDirH2.Rotate( vtAx1, dAngA2) ;
vtDirI2.Rotate( vtAx1, dAngA2) ;
}
// calcolo primo angolo di rotazione per prima soluzione
bool bDet1 = true ;
if ( nStat >= 1) {
if ( ! vtDirH1.GetRotation( vtDirTn, vtAx1, dAngA1, bDet1) )
nStat = 0 ;
else {
// se indeterminato, provo a determinarlo con la direzione ausiliaria
if ( ! bDet1) {
bool bDetX ;
vtDirI1.GetRotation( vtDirAn, vtAx1, dAngA1, bDetX) ;
if ( ! bDetX)
dAngA1 = 0 ;
}
}
// aggiornamento direzioni fresa e ausiliaria su testa
vtDirH1.Rotate( vtAx1, dAngA1) ;
vtDirI1.Rotate( vtAx1, dAngA1) ;
}
// verifiche per criterio scelta soluzione
if ( nStat >= 2) {
if ( ! VerifyScc( vtDirI2, vtDirAn, m_nCalcSolCh))
-- nStat ;
}
if ( nStat >= 1) {
if ( ! VerifyScc( vtDirI1, vtDirAn, m_nCalcSolCh)) {
-- nStat ;
// riloco eventuale soluzione rimasta
if ( nStat >= 1) {
dAngA1 = dAngA2 ;
dAngB1 = dAngB2 ;
}
}
}
// 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 ;
}
//----------------------------------------------------------------------------
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
// per ora gestisco solo gli assi rotanti di testa
// 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 = 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 ;
// tutto ok
nStat = 0 ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetDirection( const Vector3d& vtDir, double dAngA, double dAngB, Vector3d& vtNew) const
{
// per ora gestisco solo gli assi rotanti di testa
// direzione a riposo
vtNew = vtDir ;
// 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
vtNew.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
vtNew.Rotate( vtAx1, dAngA) ;
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB,
bool bBottom, Point3d& ptTip) const
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// per ora gestisco solo gli assi rotanti 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))
return false ;
ptTip.Set( dX - dRecX, dY - dRecY, dZ - dRecZ) ;
// Se richiesto punto sotto del tip utensile
if ( bBottom) {
// calcolo la direzione fresa
Vector3d vtDirT ;
if ( ! GetDirection( m_vtCalcDir, dAngA, dAngB, vtDirT))
return false ;
// se direzione fresa non esattamente verticale, esiste un punto più basso del'utensile
if ( ! vtDirT.IsZplus() && ! vtDirT.IsZminus()) {
// calcolo la direzione perpendicolare più verticale possibile
Vector3d vtCorr = FromUprightOrtho( vtDirT) ;
// correggo il tip
ptTip -= vtCorr * m_dCalcTRad ;
}
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetToolDirFromAngles( double dAngA, double dAngB, Vector3d& vtDir) const
{
return GetDirection( m_vtCalcDir, dAngA, dAngB, vtDir) ;
}
//----------------------------------------------------------------------------
bool
Machine::VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh) const
{
switch ( nSolCh) {
default :
return true ;
case MCH_SCC_ADIR_XP :
return ( vtDirI.x > - EPS_ZERO) ;
case MCH_SCC_ADIR_XM :
return ( vtDirI.x < EPS_ZERO) ;
case MCH_SCC_ADIR_YP :
return ( vtDirI.y > - EPS_ZERO) ;
case MCH_SCC_ADIR_YM :
return ( vtDirI.y < EPS_ZERO) ;
case MCH_SCC_ADIR_ZP :
return ( vtDirI.z > - EPS_ZERO) ;
case MCH_SCC_ADIR_ZM :
return ( vtDirI.z < EPS_ZERO) ;
case MCH_SCC_ADIR_NEAR :
return ( ( vtDirI * vtDirA) > cos( 60 * DEGTORAD)) ;
case MCH_SCC_ADIR_FAR :
return ( ( vtDirI * vtDirA) < cos( 120 * DEGTORAD)) ;
}
}
//----------------------------------------------------------------------------
bool
Machine::AdjustAngleInStroke( int nId, double& dAng) const
{
// se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare
if ( nId <= 0 || nId > int( m_vCalcRotAx.size()))
return true ;
// eseguo gli aggiustamenti
while ( dAng < m_vCalcRotAx[nId-1].stroke.Min)
dAng += ANG_FULL ;
while ( dAng > m_vCalcRotAx[nId-1].stroke.Max)
dAng -= ANG_FULL ;
return ( dAng >= m_vCalcRotAx[nId-1].stroke.Min &&
dAng <= m_vCalcRotAx[nId-1].stroke.Max) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetNearestAngleInStroke( int nId, double dAngRef, double& dAng) const
{
// se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare
if ( nId <= 0 || nId > int( m_vCalcRotAx.size()))
return true ;
// cerco l'angolo più vicino stando nella corsa
while ( dAng - dAngRef > ANG_STRAIGHT && dAng - ANG_FULL >= m_vCalcRotAx[nId-1].stroke.Min)
dAng -= ANG_FULL ;
while ( dAng - dAngRef < - ANG_STRAIGHT && dAng + ANG_FULL <= m_vCalcRotAx[nId-1].stroke.Max)
dAng += ANG_FULL ;
return ( dAng >= m_vCalcRotAx[nId-1].stroke.Min &&
dAng <= m_vCalcRotAx[nId-1].stroke.Max) ;
}
//----------------------------------------------------------------------------
bool
Machine::VerifyOutstroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat) const
{
// default tutto ok
nStat = 0 ;
m_sOutstrokeInfo.clear() ;
// primo lineare
if ( m_vCalcLinAx.size() >= 1) {
if ( dX < m_vCalcLinAx[0].stroke.Min) {
nStat += 1 ;
string sAxName = GetAxis( m_vCalcLinAx[0].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dX - m_vCalcLinAx[0].stroke.Min, 1) + " (L1-) " ;
}
else if( dX > m_vCalcLinAx[0].stroke.Max) {
nStat += 2 ;
string sAxName = GetAxis( m_vCalcLinAx[0].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dX - m_vCalcLinAx[0].stroke.Max, 1) + " (L1+) " ;
}
}
// secondo lineare
if ( m_vCalcLinAx.size() >= 2) {
if ( dY < m_vCalcLinAx[1].stroke.Min) {
nStat += 4 ;
string sAxName = GetAxis( m_vCalcLinAx[1].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dY - m_vCalcLinAx[1].stroke.Min, 1) + " (L2-) " ;
}
else if( dY > m_vCalcLinAx[1].stroke.Max) {
nStat += 8 ;
string sAxName = GetAxis( m_vCalcLinAx[1].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dY - m_vCalcLinAx[1].stroke.Max, 1) + " (L2+) " ;
}
}
// terzo lineare
if ( m_vCalcLinAx.size() >= 3) {
if ( dZ < m_vCalcLinAx[2].stroke.Min) {
nStat += 16 ;
string sAxName = GetAxis( m_vCalcLinAx[2].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dZ - m_vCalcLinAx[2].stroke.Min, 1) + " (L3-) " ;
}
else if( dZ > m_vCalcLinAx[2].stroke.Max) {
nStat += 32 ;
string sAxName = GetAxis( m_vCalcLinAx[2].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dZ - m_vCalcLinAx[2].stroke.Max, 1) + " (L3+) " ;
}
}
// eventuale primo rotante
if ( m_vCalcRotAx.size() >= 1) {
if ( dAngA < m_vCalcRotAx[0].stroke.Min) {
nStat += 64 ;
string sAxName = GetAxis( m_vCalcRotAx[0].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dAngA - m_vCalcRotAx[0].stroke.Min, 1) + " (R1-) " ;
}
else if( dAngA > m_vCalcRotAx[0].stroke.Max) {
nStat += 128 ;
string sAxName = GetAxis( m_vCalcRotAx[0].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dAngA - m_vCalcRotAx[0].stroke.Max, 1) + " (R1+) " ;
}
}
// eventuale secondo rotante
if ( m_vCalcRotAx.size() >= 2) {
if ( dAngB < m_vCalcRotAx[1].stroke.Min) {
nStat += 256 ;
string sAxName = GetAxis( m_vCalcRotAx[1].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dAngB - m_vCalcRotAx[1].stroke.Min, 1) + " (R2-) " ;
}
else if( dAngB > m_vCalcRotAx[1].stroke.Max) {
nStat += 512 ;
string sAxName = GetAxis( m_vCalcRotAx[1].nGrpId)->GetName() ;
m_sOutstrokeInfo += sAxName + "=" + ToString( dAngB - m_vCalcRotAx[1].stroke.Max, 1) + " (R2+) " ;
}
}
return true ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrLinAxes( void) const
{
return int( m_vCalcLinAx.size()) ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrRotAxes( void) const
{
return int( m_vCalcRotAx.size()) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAllCurrAxesName( STRVECTOR& vAxName) const
{
vAxName.clear() ;
bool bOk = true ;
// ciclo sugli assi lineari correnti
for ( auto& CalcLinAx : m_vCalcLinAx) {
Axis* pAx = GetAxis( CalcLinAx.nGrpId) ;
if ( pAx != nullptr)
vAxName.emplace_back( pAx->GetName()) ;
else
bOk = false ;
}
// ciclo sugli assi rotanti correnti
for ( auto& CalcRotAx : m_vCalcRotAx) {
Axis* pAx = GetAxis( CalcRotAx.nGrpId) ;
if ( pAx != nullptr)
vAxName.emplace_back( pAx->GetName()) ;
else
bOk = false ;
}
return bOk ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAllCurrAxesHomePos( DBLVECTOR& vAxHomeVal) const
{
vAxHomeVal.clear() ;
bool bOk = true ;
// ciclo sugi assi lineari correnti
for ( auto& CalcLinAx : m_vCalcLinAx) {
Axis* pAx = GetAxis( CalcLinAx.nGrpId) ;
if ( pAx != nullptr)
vAxHomeVal.emplace_back( pAx->GetHomeVal()) ;
else
bOk = false ;
}
// ciclo sugi assi rotanti correnti
for ( auto& CalcRotAx : m_vCalcRotAx) {
Axis* pAx = GetAxis( CalcRotAx.nGrpId) ;
if ( pAx != nullptr)
vAxHomeVal.emplace_back( pAx->GetHomeVal()) ;
else
bOk = false ;
}
return bOk ;
}
Reference in New Issue View Git Blame Copy Permalink