Dario Sassi
6fce5b366d
- prima versione funzionante di lavorazione generica con script lua - corretta funzione per Lua EmtAddArcMove - aggiunta VerifyAngleOutstroke - corretta assegnazione valore precedente ad asse indeterminato - corretta assegnazione angoli tenendo conto dei precedenti durante lavorazione.
1366 lines
47 KiB
C++
1366 lines
47 KiB
C++
//----------------------------------------------------------------------------
|
|
// 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/EMkToolConst.h"
|
|
#include "/EgtDev/Include/EGkGeoVector3d.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 EVAR_HEAD = ".HEAD" ; // (string) nome della testa
|
|
static const string EVAR_EXIT = ".EXIT" ; // (int) numero dell'uscita
|
|
static const string EVAR_TOOL = ".TOOL" ; // (string) nome dell'utensile
|
|
static const string EVAR_TOTDIAM = ".TOTDIAM" ; // (num) diametro di ingombro dell'utensile
|
|
static const string EVAR_TOTLEN = ".TOTLEN" ; // (num) lunghezza di ingombro dell'utensile
|
|
static const string ON_SET_TABLE = "OnSetTable" ;
|
|
static const string ON_SET_HEAD = "OnSetHead" ;
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
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)) {
|
|
// definisco variabili
|
|
bool bOk = LuaCreateGlobTable( EMC_VAR) ;
|
|
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TABNAME, sTable) ;
|
|
// chiamo funzione
|
|
bOk = bOk && LuaCallFunction( ON_SET_TABLE) ;
|
|
// reset variabili
|
|
bOk = bOk && LuaResetGlobVar( EMC_VAR) ;
|
|
// restituisco risultato
|
|
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::GetCurrTableDeltaRef1( Vector3d& vtDelta1) const
|
|
{
|
|
Table* pTab = GetTable( m_nCalcTabId) ;
|
|
if ( pTab == nullptr)
|
|
return false ;
|
|
// recupero la posizione corrente del riferimento 1 della tavola
|
|
// riferimento globale del gruppo tavola
|
|
Frame3d frTable ;
|
|
m_pGeomDB->GetGroupGlobFrame( m_nCalcTabId, frTable) ;
|
|
// recupero il primo riferimento della tavola
|
|
int nRef1 = m_pGeomDB->GetFirstNameInGroup( m_nCalcTabId, MCH_TREF + "1") ;
|
|
if ( nRef1 == GDB_ID_NULL || m_pGeomDB->GetGeoType( nRef1) != GEO_FRAME3D)
|
|
return false ;
|
|
// recupero frame
|
|
const Frame3d& frFrame = GetGeoFrame3d( m_pGeomDB->GetGeoObj( nRef1))->GetFrame() ;
|
|
// ne calcolo l'origine in globale
|
|
Point3d ptPos = frFrame.Orig() ;
|
|
ptPos.ToGlob( frTable) ;
|
|
// calcolo il delta
|
|
vtDelta1 = ptPos - pTab->GetRef1() ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetCurrTableIsTilting( bool& bTilting) const
|
|
{
|
|
// default
|
|
bTilting = false ;
|
|
// verifico esistenza tavola
|
|
if ( m_nCalcTabId == GDB_ID_NULL)
|
|
return false ;
|
|
// recupero gli eventuali assi rotanti della tavola
|
|
int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ;
|
|
if ( nTParId == GDB_ID_NULL)
|
|
return false ;
|
|
while ( IsAxisGroup( nTParId)) {
|
|
// recupero il gestore dell'asse
|
|
Axis* pAx = GetAxis( nTParId) ;
|
|
if ( pAx == nullptr)
|
|
return false ;
|
|
// verifico se asse rotante orizzontale
|
|
if ( pAx->GetType() == MCH_AT_ROTARY && abs( pAx->GetDir().z) < EPS_SMALL) {
|
|
bTilting = true ;
|
|
return true ;
|
|
}
|
|
// risalgo lungo la catena
|
|
nTParId = m_pGeomDB->GetParentId( nTParId) ;
|
|
}
|
|
|
|
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 ;
|
|
m_dCalcTOvLen = 0 ;
|
|
m_dCalcTOvRad = 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 + " for tool " + sTool ;
|
|
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 ;
|
|
double dTOvLen = 0 ;
|
|
double dTOvDiam = 0 ;
|
|
// se definito
|
|
if ( ! sTool.empty()) {
|
|
if ( ! LoadTool( sHead, nExit, sTool)) {
|
|
string sOut = "Missing tool " + sTool ;
|
|
LOG_INFO( GetEMkLogger(), sOut.c_str())
|
|
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) ||
|
|
! m_pMchMgr->TdbGetCurrToolParam( TPA_TOTLEN, dTOvLen) ||
|
|
! m_pMchMgr->TdbGetCurrToolParam( TPA_TOTDIAM, dTOvDiam))
|
|
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 ;
|
|
dTOvLen = 0 ;
|
|
dTOvDiam = 0 ;
|
|
m_pMchMgr->TdbSetCurrTool( "") ;
|
|
}
|
|
// 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 ;
|
|
m_dCalcTOvLen = dTOvLen ;
|
|
m_dCalcTOvRad = dTOvDiam / 2 ;
|
|
// lancio eventuale funzione lua di personalizzazione
|
|
if ( LuaExistsFunction( ON_SET_HEAD)) {
|
|
// definisco variabili
|
|
bool bOk = LuaCreateGlobTable( EMC_VAR) ;
|
|
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_HEAD, sHead) ;
|
|
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_EXIT, nExit) ;
|
|
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TOOL, sTool) ;
|
|
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TOTDIAM, dTOvDiam) ;
|
|
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TOTLEN, dTOvLen) ;
|
|
// chiamo funzione
|
|
bOk = bOk && LuaCallFunction( ON_SET_HEAD) ;
|
|
// reset variabili
|
|
bOk = bOk && LuaResetGlobVar( EMC_VAR) ;
|
|
// in caso di errore esco
|
|
if ( ! bOk)
|
|
return false ;
|
|
}
|
|
// 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) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
Machine::GetCurrHead( void) const
|
|
{
|
|
// controllo GeomDB
|
|
if ( m_pGeomDB == nullptr)
|
|
return GDB_ID_NULL ;
|
|
// recupero identificativo dell'utensile
|
|
return m_nCalcHeadId ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetCurrHead( std::string& sHead) const
|
|
{
|
|
// controllo GeomDB
|
|
if ( m_pGeomDB == nullptr)
|
|
return false ;
|
|
// recupero nome gruppo della testa
|
|
return m_pGeomDB->GetName( m_nCalcHeadId, sHead) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
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 {
|
|
LOG_INFO( GetEMkLogger(), "Linear Axes not aligned with Global XYZ")
|
|
return false ;
|
|
}
|
|
}
|
|
if ( ! m_vCalcLinAx[1].vtDir.IsYplus()) {
|
|
if ( m_vCalcLinAx[2].vtDir.IsYplus())
|
|
swap( m_vCalcLinAx[1], m_vCalcLinAx[2]) ;
|
|
else {
|
|
LOG_INFO( GetEMkLogger(), "Linear Axes not aligned with Global XYZ")
|
|
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 ;
|
|
}
|
|
// se 3 va bene ( uno dovrà poi avere valore assegnato)
|
|
if ( m_vCalcRotAx.size() == 3) {
|
|
// se ultimi due di testa, devo invertirne l'ordine
|
|
if ( m_vCalcRotAx[1].bHead && m_vCalcRotAx[2].bHead)
|
|
swap( m_vCalcRotAx[1], m_vCalcRotAx[2]) ;
|
|
// impongo limiti di corsa sul secondo asse rotante di testa
|
|
if ( m_vCalcRotAx[2].bHead) {
|
|
Head* pHead = GetHead( m_nCalcHeadId) ;
|
|
if ( pHead != nullptr) {
|
|
m_vCalcRotAx[2].stroke.Min = max( m_vCalcRotAx[2].stroke.Min, pHead->GetRot2Stroke().Min) ;
|
|
m_vCalcRotAx[2].stroke.Max = min( m_vCalcRotAx[2].stroke.Max, pHead->GetRot2Stroke().Max) ;
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
// altrimenti non ancora gestito, quindi errore
|
|
LOG_INFO( GetEMkLogger(), "Rotary Axes not manageable")
|
|
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() ;
|
|
kAx.dHomeVal = pAx->GetHomeVal() ;
|
|
// 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::BlockKinematicRotAxis( const string& sName, double dVal)
|
|
{
|
|
return BlockKinematicRotAxis( GetAxisId( sName), dVal) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::BlockKinematicRotAxis( int nId, double dVal)
|
|
{
|
|
// verifico identificativo
|
|
if ( nId == GDB_ID_NULL)
|
|
return false ;
|
|
// cerco l'asse rotante di calcolo con questo identificativo
|
|
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
|
|
if ( m_vCalcRotAx[i].nGrpId == nId) {
|
|
if ( dVal < m_vCalcRotAx[i].stroke.Min || dVal > m_vCalcRotAx[i].stroke.Max) {
|
|
m_vCalcRotAx[i].bFixed = false ;
|
|
return false ;
|
|
}
|
|
m_vCalcRotAx[i].bFixed = true ;
|
|
m_vCalcRotAx[i].dFixVal = dVal ;
|
|
return true ;
|
|
}
|
|
}
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::FreeKinematicRotAxis( const string& sName)
|
|
{
|
|
return FreeKinematicRotAxis( GetAxisId( sName)) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::FreeKinematicRotAxis( int nId)
|
|
{
|
|
// verifico identificativo
|
|
if ( nId == GDB_ID_NULL)
|
|
return false ;
|
|
// cerco l'asse rotante di calcolo con questo identificativo
|
|
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
|
|
if ( m_vCalcRotAx[i].nGrpId == nId) {
|
|
m_vCalcRotAx[i].bFixed = false ;
|
|
return true ;
|
|
}
|
|
}
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::IsKinematicRotAxisBlocked( int nInd) const
|
|
{
|
|
int nRotAxTot = int( m_vCalcRotAx.size()) ;
|
|
if ( nInd < 0 || nInd >= nRotAxTot)
|
|
return false ;
|
|
return m_vCalcRotAx[nInd].bFixed ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetKinematicRotAxisBlocked( int nInd, string& sName, double& dVal) const
|
|
{
|
|
int nRotAxTot = int( m_vCalcRotAx.size()) ;
|
|
if ( nInd < 0 || nInd >= nRotAxTot || ! m_vCalcRotAx[nInd].bFixed)
|
|
return false ;
|
|
m_pGeomDB->GetName( m_vCalcRotAx[nInd].nGrpId, sName) ;
|
|
dVal = m_vCalcRotAx[nInd].dFixVal ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::SetSolCh( int nScc)
|
|
{
|
|
// se standard o nullo o suo opposto
|
|
if ( nScc == MCH_SCC_STD || nScc == MCH_SCC_NONE || nScc == MCH_SCC_OPPOSITE) {
|
|
// recupero i dati della testa
|
|
Head* pHead = GetHead( m_nCalcHeadId) ;
|
|
if ( pHead == nullptr) {
|
|
LOG_INFO( GetEMkLogger(), "Missing head in SetSolCh")
|
|
return false ;
|
|
}
|
|
// assegno il criterio definito nella testa (standard)
|
|
m_nCalcSolCh = pHead->GetSolCh() ;
|
|
if ( nScc == MCH_SCC_STD || nScc == MCH_SCC_NONE)
|
|
return true ;
|
|
// essendo opposto, devo invertirlo
|
|
switch ( m_nCalcSolCh) {
|
|
case MCH_SCC_NONE : break ;
|
|
case MCH_SCC_ADIR_XP : m_nCalcSolCh = MCH_SCC_ADIR_XM ; break ;
|
|
case MCH_SCC_ADIR_XM : m_nCalcSolCh = MCH_SCC_ADIR_XP ; break ;
|
|
case MCH_SCC_ADIR_YP : m_nCalcSolCh = MCH_SCC_ADIR_YM ; break ;
|
|
case MCH_SCC_ADIR_YM : m_nCalcSolCh = MCH_SCC_ADIR_YP ; break ;
|
|
case MCH_SCC_ADIR_ZP : m_nCalcSolCh = MCH_SCC_ADIR_ZM ; break ;
|
|
case MCH_SCC_ADIR_ZM : m_nCalcSolCh = MCH_SCC_ADIR_ZP ; break ;
|
|
case MCH_SCC_ADIR_NEAR : m_nCalcSolCh = MCH_SCC_ADIR_FAR ; break ;
|
|
case MCH_SCC_ADIR_FAR : m_nCalcSolCh = MCH_SCC_ADIR_NEAR ; break ;
|
|
default : m_nCalcSolCh = MCH_SCC_NONE ; break ;
|
|
}
|
|
return true ;
|
|
}
|
|
// altri casi
|
|
if ( IsValidHeadScc( nScc)) {
|
|
m_nCalcSolCh = nScc ;
|
|
return true ;
|
|
}
|
|
// non previsto
|
|
m_nCalcSolCh = MCH_SCC_NONE ;
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
|
|
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const
|
|
{
|
|
// assegno dati
|
|
Vector3d vtDirH = m_vtCalcDir ;
|
|
Vector3d vtDirI = m_vtCalcADir ;
|
|
int nNumRotAx = 0 ;
|
|
KinAxis RotAx1 ;
|
|
if ( m_vCalcRotAx.size() >= 1) {
|
|
++ nNumRotAx ;
|
|
RotAx1 = m_vCalcRotAx[0] ;
|
|
}
|
|
KinAxis RotAx2 ;
|
|
if ( m_vCalcRotAx.size() >= 2) {
|
|
++ nNumRotAx ;
|
|
RotAx2 = m_vCalcRotAx[1] ;
|
|
}
|
|
|
|
// eseguo calcolo
|
|
return GetMyAngles( vtDirT, vtDirA, vtDirH, vtDirI, nNumRotAx, RotAx1, RotAx2,
|
|
nStat, dAngA1, dAngB1, dAngA2, dAngB2) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
|
|
int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const
|
|
{
|
|
// verifiche e aggiustamenti degli assi rotanti
|
|
Vector3d vtDirTm = vtDirT ;
|
|
Vector3d vtDirAm = vtDirA ;
|
|
Vector3d vtDirH = m_vtCalcDir ;
|
|
Vector3d vtDirI = m_vtCalcADir ;
|
|
int nNumRotAx = 0 ;
|
|
KinAxis RotAx[2] ;
|
|
INTVECTOR vBloHeAx ;
|
|
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
|
|
// se asse libero
|
|
if ( ! m_vCalcRotAx[i].bFixed) {
|
|
// verifico di non superare il limite
|
|
if ( nNumRotAx >= 2)
|
|
return false ;
|
|
// assegno l'asse
|
|
RotAx[nNumRotAx] = m_vCalcRotAx[i] ;
|
|
// se asse di testa, lo aggiorno con precedenti bloccati di testa (applicati in ordine contrario)
|
|
if ( RotAx[nNumRotAx].bHead) {
|
|
for ( size_t k = vBloHeAx.size() ; k >= 1 ; -- k) {
|
|
int nA = vBloHeAx[k-1] ;
|
|
RotAx[nNumRotAx].ptPos.Rotate( m_vCalcRotAx[nA].ptPos, m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
|
|
RotAx[nNumRotAx].vtDir.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
|
|
}
|
|
}
|
|
// incremento contatore
|
|
++ nNumRotAx ;
|
|
}
|
|
// altrimenti asse bloccato
|
|
else {
|
|
// se asse di tavola
|
|
if ( ! m_vCalcRotAx[i].bHead) {
|
|
// aggiorno direzioni utensile e ausiliaria richieste
|
|
vtDirTm.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
|
|
vtDirAm.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
|
|
// aggiorno eventuali assi già inseriti (sicuramente di tavola)
|
|
for ( size_t j = 0 ; int( j) < nNumRotAx ; ++ j) {
|
|
RotAx[j].ptPos.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
|
|
RotAx[j].vtDir.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
|
|
}
|
|
}
|
|
// altrimenti asse di testa
|
|
else {
|
|
// inserisco in lista assi di testa bloccati
|
|
vBloHeAx.push_back( int( i)) ;
|
|
}
|
|
}
|
|
|
|
}
|
|
// aggiorno direzioni utensile e ausiliaria su testa con eventuali assi bloccati di testa (applico in ordine contrario)
|
|
for ( size_t k = vBloHeAx.size() ; k >= 1 ; -- k) {
|
|
int nA = vBloHeAx[k-1] ;
|
|
vtDirH.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
|
|
vtDirI.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
|
|
}
|
|
|
|
// eseguo calcolo
|
|
double dAngA1, dAngB1, dAngA2, dAngB2 ;
|
|
if ( ! GetMyAngles( vtDirTm, vtDirAm, vtDirH, vtDirI, nNumRotAx, RotAx[0], RotAx[1],
|
|
nStat, dAngA1, dAngB1, dAngA2, dAngB2))
|
|
return false ;
|
|
// assegno gli angoli
|
|
int nRotAxInd = 1 ;
|
|
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++i) {
|
|
if ( m_vCalcRotAx[i].bFixed) {
|
|
vAng1.push_back( m_vCalcRotAx[i].dFixVal) ;
|
|
vAng2.push_back( m_vCalcRotAx[i].dFixVal) ;
|
|
}
|
|
else {
|
|
if ( nRotAxInd == 1) {
|
|
vAng1.push_back( dAngA1) ;
|
|
vAng2.push_back( dAngA2) ;
|
|
}
|
|
else if ( nRotAxInd == 2) {
|
|
vAng1.push_back( dAngB1) ;
|
|
vAng2.push_back( dAngB2) ;
|
|
}
|
|
else
|
|
return false ;
|
|
++ nRotAxInd ;
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
|
|
const Vector3d& vtDirH, const Vector3d& vtDirI,
|
|
int nNumRotAx, const KinAxis& RotAx1, const KinAxis& RotAx2,
|
|
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 ;
|
|
|
|
// 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 vtDirHn = vtDirH ;
|
|
// direzione ausiliaria su testa a riposo
|
|
Vector3d vtDirIn = vtDirI ;
|
|
|
|
// se nessun asse rotante, devo solo verificare corrispondenza tra direzione richiesta e fresa
|
|
if ( nNumRotAx == 0) {
|
|
if ( AreSameVectorApprox( vtDirTn, vtDirHn))
|
|
nStat = 1 ;
|
|
else
|
|
nStat = 0 ;
|
|
return true ;
|
|
}
|
|
|
|
// direzione primo asse rotante
|
|
Vector3d vtAx1 = RotAx1.vtDir ;
|
|
// se asse di tavola, ne inverto la direzione
|
|
if ( ! RotAx1.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 ( nNumRotAx == 2) {
|
|
// direzione secondo asse rotante
|
|
Vector3d vtAx2 = RotAx2.vtDir ;
|
|
// se asse di tavola, ne inverto la direzione
|
|
if ( ! RotAx2.bHead)
|
|
vtAx2.Invert() ;
|
|
// calcolo secondo angolo di rotazione
|
|
nStat = GetRotationComponent( vtDirHn, 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 = vtDirHn ;
|
|
vtDirH1.Rotate( vtAx2, dAngB1) ;
|
|
vtDirI1 = vtDirIn ;
|
|
vtDirI1.Rotate( vtAx2, dAngB1) ;
|
|
}
|
|
if ( nStat == 2) {
|
|
vtDirH2 = vtDirHn ;
|
|
vtDirH2.Rotate( vtAx2, dAngB2) ;
|
|
vtDirI2 = vtDirIn ;
|
|
vtDirI2.Rotate( vtAx2, dAngB2) ;
|
|
}
|
|
}
|
|
// altrimenti verifico se compatibili
|
|
else {
|
|
// componente versore utensile su direzione primo asse
|
|
double dCompHSuAxR1 = vtDirHn * 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 = vtDirHn ;
|
|
vtDirI1 = vtDirIn ;
|
|
// 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 ;
|
|
Vector3d vtSccDir ;
|
|
bool bSccDir = GetSccDir( m_nCalcSolCh, vtDirAn, vtSccDir) ;
|
|
vtDirI2.GetRotation( vtSccDir, vtAx1, dAngA2, bDetX) ;
|
|
if ( bDetX)
|
|
bDet2 = bSccDir ;
|
|
else
|
|
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 ;
|
|
Vector3d vtSccDir ;
|
|
bool bSccDir = GetSccDir( m_nCalcSolCh, vtDirAn, vtSccDir) ;
|
|
vtDirI1.GetRotation( vtSccDir, vtAx1, dAngA1, bDetX) ;
|
|
if ( bDetX)
|
|
bDet1 = bSccDir ;
|
|
else
|
|
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( RotAx1.stroke, dAngA2) ||
|
|
( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, dAngB2)))
|
|
-- nStat ;
|
|
}
|
|
if ( nStat >= 1) {
|
|
// se non riesco ad aggiustare, elimino
|
|
if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA1) ||
|
|
( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, 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
|
|
{
|
|
DBLVECTOR vAng( 2) ; vAng[0] = dAngA ; vAng[1] = dAngB ;
|
|
return GetPositions( ptP, vAng, nStat, dX, dY, dZ) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng,
|
|
int& nStat, double& dX, double& dY, double& dZ) const
|
|
{
|
|
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
|
|
// il punto è dato rispetto alla posizione home della tavola
|
|
|
|
// 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) {
|
|
// 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) {
|
|
// 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) ;
|
|
}
|
|
|
|
// 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
|
|
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 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetHeadOffsetDelta( const 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, const DBLVECTOR& vAng, Vector3d& vtNew) const
|
|
{
|
|
// è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa)
|
|
|
|
// direzione a riposo
|
|
vtNew = vtDir ;
|
|
|
|
// se c'è terzo asse rotante di testa
|
|
if ( m_vCalcRotAx.size() >= 3 && m_vCalcRotAx[2].bHead) {
|
|
// posizione e direzione secondo asse rotante
|
|
Point3d ptAx3 = m_vCalcRotAx[2].ptPos ;
|
|
Vector3d vtAx3 = m_vCalcRotAx[2].vtDir ;
|
|
// ruoto dati a riposo
|
|
vtNew.Rotate( vtAx3, vAng[2]) ;
|
|
}
|
|
|
|
// se c'è secondo asse rotante di testa
|
|
if ( m_vCalcRotAx.size() >= 2 && m_vCalcRotAx[1].bHead) {
|
|
// posizione e direzione secondo asse rotante
|
|
Point3d ptAx2 = m_vCalcRotAx[1].ptPos ;
|
|
Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
|
|
// ruoto dati a riposo
|
|
vtNew.Rotate( vtAx2, vAng[1]) ;
|
|
}
|
|
|
|
// 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, vAng[0]) ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng,
|
|
bool bOverall, bool bBottom, Point3d& ptTip) const
|
|
{
|
|
// 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) ;
|
|
|
|
// Se richiesto ingombro totale o punto sotto del tip utensile
|
|
if ( bOverall || bBottom) {
|
|
// calcolo la direzione fresa
|
|
Vector3d vtDirT ;
|
|
if ( ! GetDirection( m_vtCalcDir, vAng, vtDirT))
|
|
return false ;
|
|
// se richiesto ingombro totale
|
|
if ( bOverall)
|
|
ptTip -= vtDirT * max( m_dCalcTOvLen - m_dCalcTLen, 0.) ;
|
|
// se richiesto punto più basso e direzione fresa non esattamente verticale
|
|
if ( bBottom && ! vtDirT.IsZplus() && ! vtDirT.IsZminus()) {
|
|
// calcolo la direzione perpendicolare più verticale possibile
|
|
Vector3d vtCorr = FromUprightOrtho( vtDirT) ;
|
|
// correggo il tip
|
|
if ( bOverall)
|
|
ptTip -= vtCorr * max( m_dCalcTOvRad, m_dCalcTRad) ;
|
|
else
|
|
ptTip -= vtCorr * m_dCalcTRad ;
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetToolDirFromAngles( double dAngA, double dAngB, Vector3d& vtDir) const
|
|
{
|
|
DBLVECTOR vAng( 2) ; vAng[0] = dAngA ; vAng[1] = dAngB ;
|
|
return GetDirection( m_vtCalcDir, vAng, vtDir) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetSccDir( int nSolCh, const Vector3d& vtDirA, Vector3d& vtDirScc) const
|
|
{
|
|
switch ( nSolCh) {
|
|
case MCH_SCC_ADIR_XP : vtDirScc = X_AX ; return true ;
|
|
case MCH_SCC_ADIR_XM : vtDirScc = - X_AX ; return true ;
|
|
case MCH_SCC_ADIR_YP : vtDirScc = Y_AX ; return true ;
|
|
case MCH_SCC_ADIR_YM : vtDirScc = - Y_AX ; return true ;
|
|
case MCH_SCC_ADIR_ZP : vtDirScc = Z_AX ; return true ;
|
|
case MCH_SCC_ADIR_ZM : vtDirScc = - Z_AX ; return true ;
|
|
case MCH_SCC_ADIR_NEAR : vtDirScc = vtDirA ; return true ;
|
|
case MCH_SCC_ADIR_FAR : vtDirScc = - vtDirA ; return true ;
|
|
}
|
|
vtDirScc = vtDirA ;
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
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 ( vtDirA.IsSmall() || ( vtDirI * vtDirA) > cos( 60 * DEGTORAD)) ;
|
|
case MCH_SCC_ADIR_FAR :
|
|
return ( vtDirA.IsSmall() || ( vtDirI * vtDirA) < cos( 120 * DEGTORAD)) ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::AdjustAngleInStroke( const STROKE& Stroke, double& dAng) const
|
|
{
|
|
// eseguo gli aggiustamenti
|
|
while ( dAng < Stroke.Min)
|
|
dAng += ANG_FULL ;
|
|
while ( dAng > Stroke.Max)
|
|
dAng -= ANG_FULL ;
|
|
return ( dAng >= Stroke.Min && dAng <= Stroke.Max) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetNearestAngleInStroke( int nInd, double dAngRef, double& dAng) const
|
|
{
|
|
// se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare
|
|
if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size()))
|
|
return true ;
|
|
// cerco l'angolo più vicino stando nella corsa
|
|
while ( dAng - dAngRef > ANG_STRAIGHT && dAng - ANG_FULL >= m_vCalcRotAx[nInd].stroke.Min)
|
|
dAng -= ANG_FULL ;
|
|
while ( dAng - dAngRef < - ANG_STRAIGHT && dAng + ANG_FULL <= m_vCalcRotAx[nInd].stroke.Max)
|
|
dAng += ANG_FULL ;
|
|
return ( dAng >= m_vCalcRotAx[nInd].stroke.Min &&
|
|
dAng <= m_vCalcRotAx[nInd].stroke.Max) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::LimitAngleToStroke( int nInd, double& dAng) const
|
|
{
|
|
// se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare
|
|
if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size()))
|
|
return true ;
|
|
// se angolo fuori corsa, lo porto all'estremo più vicino
|
|
if ( dAng < m_vCalcRotAx[nInd].stroke.Min)
|
|
dAng = m_vCalcRotAx[nInd].stroke.Min ;
|
|
else if ( dAng > m_vCalcRotAx[nInd].stroke.Max)
|
|
dAng = m_vCalcRotAx[nInd].stroke.Max ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::VerifyAngleOutstroke( int nInd, double dAng) const
|
|
{
|
|
// se angolo fittizio (non esiste l'asse rotante corrispondente), va bene
|
|
if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size()))
|
|
return true ;
|
|
// se angolo fuori corsa, errore
|
|
if ( dAng < m_vCalcRotAx[nInd].stroke.Min)
|
|
return false ;
|
|
else if ( dAng > m_vCalcRotAx[nInd].stroke.Max)
|
|
return false ;
|
|
// va bene
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::VerifyOutstroke( double dX, double dY, double dZ, const DBLVECTOR& vAng, int& nStat) const
|
|
{
|
|
// default tutto ok
|
|
nStat = 0 ;
|
|
m_sOutstrokeInfo.clear() ;
|
|
// verifica degli assi lineari
|
|
DBLVECTOR vLin( 3) ; vLin[0] = dX ; vLin[1] = dY ; vLin[2] = dZ ;
|
|
for ( size_t i = 0 ; i < m_vCalcLinAx.size() && i < vLin.size() ; ++ i) {
|
|
if ( vLin[i] < m_vCalcLinAx[i].stroke.Min) {
|
|
nStat += ( 1 << ( 2 * i)) ;
|
|
string sAxName = GetAxis( m_vCalcLinAx[i].nGrpId)->GetName() ;
|
|
m_sOutstrokeInfo += sAxName + "=" + ToString( vLin[i] - m_vCalcLinAx[i].stroke.Min, 1) +
|
|
" (L" + ToString( int(i) + 1) + "-) " ;
|
|
}
|
|
else if( vLin[i] > m_vCalcLinAx[i].stroke.Max) {
|
|
nStat += ( 2 << ( 2 * i)) ;
|
|
string sAxName = GetAxis( m_vCalcLinAx[i].nGrpId)->GetName() ;
|
|
m_sOutstrokeInfo += sAxName + "=" + ToString( vLin[i] - m_vCalcLinAx[i].stroke.Max, 1) +
|
|
" (L" + ToString( int(i) + 1) + "+) " ;
|
|
}
|
|
}
|
|
// verifica degli assi rotanti
|
|
for ( size_t i = 0 ; i < m_vCalcRotAx.size() && i < vAng.size() ; ++ i) {
|
|
if ( vAng[i] < m_vCalcRotAx[i].stroke.Min) {
|
|
nStat += ( 64 << ( 2 * i)) ;
|
|
string sAxName = GetAxis( m_vCalcRotAx[i].nGrpId)->GetName() ;
|
|
m_sOutstrokeInfo += sAxName + "=" + ToString( vAng[i] - m_vCalcRotAx[i].stroke.Min, 1) +
|
|
" (R" + ToString( int(i) + 1) + "-) " ;
|
|
}
|
|
else if( vAng[i] > m_vCalcRotAx[i].stroke.Max) {
|
|
nStat += ( 128 << ( 2 * i)) ;
|
|
string sAxName = GetAxis( m_vCalcRotAx[i].nGrpId)->GetName() ;
|
|
m_sOutstrokeInfo += sAxName + "=" + ToString( vAng[i] - m_vCalcRotAx[i].stroke.Max, 1) +
|
|
" (R" + ToString( int(i) + 1) + "+) " ;
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
Machine::GetCurrLinAxes( void) const
|
|
{
|
|
return int( m_vCalcLinAx.size()) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
Machine::GetCurrRotAxes( void) const
|
|
{
|
|
return int( m_vCalcRotAx.size()) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetCurrAxisName( int nInd, string& sAxName) const
|
|
{
|
|
int nLinAxes = int( m_vCalcLinAx.size()) ;
|
|
int nRotAxes = int( m_vCalcRotAx.size()) ;
|
|
if ( nInd >= 0 && nInd < nLinAxes) {
|
|
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
|
|
if ( pAx == nullptr)
|
|
return false ;
|
|
sAxName = pAx->GetName() ;
|
|
return true ;
|
|
}
|
|
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
|
|
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
|
|
if ( pAx == nullptr)
|
|
return false ;
|
|
sAxName = pAx->GetName() ;
|
|
return true ;
|
|
}
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
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::GetCurrAxisToken( int nInd, string& sAxToken) const
|
|
{
|
|
int nLinAxes = int( m_vCalcLinAx.size()) ;
|
|
int nRotAxes = int( m_vCalcRotAx.size()) ;
|
|
if ( nInd >= 0 && nInd < nLinAxes) {
|
|
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
|
|
if ( pAx == nullptr)
|
|
return false ;
|
|
sAxToken = pAx->GetToken() ;
|
|
return true ;
|
|
}
|
|
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
|
|
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
|
|
if ( pAx == nullptr)
|
|
return false ;
|
|
sAxToken = pAx->GetToken() ;
|
|
return true ;
|
|
}
|
|
return false ;
|
|
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetAllCurrAxesToken( STRVECTOR& vAxToken) const
|
|
{
|
|
vAxToken.clear() ;
|
|
bool bOk = true ;
|
|
// ciclo sugli assi lineari correnti
|
|
for ( auto& CalcLinAx : m_vCalcLinAx) {
|
|
Axis* pAx = GetAxis( CalcLinAx.nGrpId) ;
|
|
if ( pAx != nullptr)
|
|
vAxToken.emplace_back( pAx->GetToken()) ;
|
|
else
|
|
bOk = false ;
|
|
}
|
|
// ciclo sugli assi rotanti correnti
|
|
for ( auto& CalcRotAx : m_vCalcRotAx) {
|
|
Axis* pAx = GetAxis( CalcRotAx.nGrpId) ;
|
|
if ( pAx != nullptr)
|
|
vAxToken.emplace_back( pAx->GetToken()) ;
|
|
else
|
|
bOk = false ;
|
|
}
|
|
return bOk ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetCurrAxisHomePos( int nInd, double& dHome) const
|
|
{
|
|
int nLinAxes = int( m_vCalcLinAx.size()) ;
|
|
int nRotAxes = int( m_vCalcRotAx.size()) ;
|
|
if ( nInd >= 0 && nInd < nLinAxes) {
|
|
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
|
|
if ( pAx == nullptr)
|
|
return false ;
|
|
dHome = pAx->GetHomeVal() ;
|
|
return true ;
|
|
}
|
|
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
|
|
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
|
|
if ( pAx == nullptr)
|
|
return false ;
|
|
dHome = pAx->GetHomeVal() ;
|
|
return true ;
|
|
}
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
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 ;
|
|
}
|