Dario Sassi
363 lines
12 KiB
C++
363 lines
12 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 "/EgtDev/Include/EGkGeoVector3d.h"
|
|
#include "/EgtDev/Include/EGnStringUtils.h"
|
|
#include "/EgtDev/Include/EGnFileUtils.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::SetCurrTable( const string& sTable)
|
|
{
|
|
// 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 ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
|
|
{
|
|
// azzero tutto
|
|
m_nCalcHeadId = GDB_ID_NULL ;
|
|
m_nCalcExitId = GDB_ID_NULL ;
|
|
m_nCalcToolId = GDB_ID_NULL ;
|
|
m_dCalcTLen = 0 ;
|
|
// recupero il gruppo della testa
|
|
int nHeadId = GetGroup( sHead) ;
|
|
if ( nHeadId == GDB_ID_NULL || ! IsHeadGroup( nHeadId))
|
|
return false ;
|
|
// recupero il gruppo dell'uscita
|
|
string sExit = "T" + 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))
|
|
return false ;
|
|
// recupero i dati dell'utensile
|
|
if ( ! LoadTool( sHead, nExit, sTool))
|
|
return false ;
|
|
int nToolId = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nExitId, sTool) ;
|
|
if ( nToolId == GDB_ID_NULL || m_pMchMgr->m_pGeomDB->GetGdbType( nToolId) != GDB_TY_GROUP)
|
|
return false ;
|
|
double dTLen ;
|
|
if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nToolId, "L", dTLen))
|
|
return false ;
|
|
// assegno tutti i dati
|
|
m_nCalcHeadId = nHeadId ;
|
|
m_nCalcExitId = nExitId ;
|
|
m_nCalcToolId = nToolId ;
|
|
m_ptCalcPos = ptPos ;
|
|
m_vtCalcDir = vtDir ;
|
|
m_dCalcTLen = dTLen ;
|
|
// determino la catena cinematica
|
|
return CalculateKinematicChain() ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::CalculateKinematicChain( void)
|
|
{
|
|
// 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_pMchMgr->m_pGeomDB->GetParentId( m_nCalcTabId) ;
|
|
if ( nTParId == GDB_ID_NULL)
|
|
return false ;
|
|
while ( IsAxisGroup( nTParId)) {
|
|
if ( ! AddKinematicAxis( false, nTParId))
|
|
return false ;
|
|
nTParId = m_pMchMgr->m_pGeomDB->GetParentId( nTParId) ;
|
|
}
|
|
// recupero gli assi di testa
|
|
if ( m_nCalcHeadId == GDB_ID_NULL)
|
|
return false ;
|
|
int nHParId = m_pMchMgr->m_pGeomDB->GetParentId( m_nCalcHeadId) ;
|
|
if ( nHParId == GDB_ID_NULL)
|
|
return false ;
|
|
while ( IsAxisGroup( nHParId)) {
|
|
if ( ! AddKinematicAxis( true, nHParId))
|
|
return false ;
|
|
nHParId = m_pMchMgr->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]) ;
|
|
return true ;
|
|
}
|
|
// altrimenti non ancora gestito, quindi errore
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::AddKinematicAxis( bool bOnHead, int nId)
|
|
{
|
|
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 ;
|
|
// 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 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetAngles( const Vector3d& vtDirT,
|
|
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2)
|
|
{
|
|
// 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 fresa su testa a riposo
|
|
Vector3d vtDirH = m_vtCalcDir ;
|
|
// 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 ;
|
|
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 direzione fresa su testa
|
|
if ( nStat >= 1) {
|
|
// se indeterminato lo azzero
|
|
if ( ! bDet)
|
|
dAngB1 = 0 ;
|
|
// eseguo aggiornamento
|
|
vtDirH1 = vtDirH ;
|
|
vtDirH1.Rotate( vtAx2, dAngB1) ;
|
|
}
|
|
if ( nStat == 2) {
|
|
vtDirH2 = vtDirH ;
|
|
vtDirH2.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 ;
|
|
// reset secondo angolo
|
|
dAngB1 = 0 ;
|
|
}
|
|
}
|
|
|
|
// calcolo primo angolo di rotazione per seconda soluzione
|
|
if ( nStat == 2) {
|
|
bool bDet ;
|
|
if ( ! vtDirH2.GetRotation( vtDirTn, vtAx1, dAngA2, bDet) )
|
|
nStat = 1 ;
|
|
else {
|
|
// se indeterminato ...
|
|
if ( ! bDet) {
|
|
nStat = - 2 ;
|
|
dAngA2 = 0 ;
|
|
}
|
|
}
|
|
}
|
|
|
|
// calcolo primo angolo di rotazione per prima soluzione
|
|
if ( nStat >= 1) {
|
|
bool bDet ;
|
|
if ( ! vtDirH1.GetRotation( vtDirTn, vtAx1, dAngA1, bDet) )
|
|
nStat = 0 ;
|
|
else {
|
|
// se indeterminato ...
|
|
if ( ! bDet) {
|
|
nStat = - nStat ;
|
|
dAngA1 = 0 ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB,
|
|
int& nStat, double& dX, double& dY, double& dZ)
|
|
{
|
|
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
|
|
// per ora gestisco solo gli assi 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) ;
|
|
}
|
|
|
|
// 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 + vtDtAx.x + vtDtTL.x ;
|
|
dY = ptP.y + vtDtAx.y + vtDtTL.y ;
|
|
dZ = ptP.z + vtDtAx.z + vtDtTL.z ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
Machine::VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat)
|
|
{
|
|
// default tutto ok
|
|
nStat = 0 ;
|
|
// primo lineare
|
|
if ( dX < m_vCalcLinAx[0].stroke.Min)
|
|
nStat += 1 ;
|
|
else if( dX > m_vCalcLinAx[0].stroke.Max)
|
|
nStat += 2 ;
|
|
// secondo lineare
|
|
if ( dY < m_vCalcLinAx[1].stroke.Min)
|
|
nStat += 4 ;
|
|
else if( dY > m_vCalcLinAx[1].stroke.Max)
|
|
nStat += 8 ;
|
|
// terzo lineare
|
|
if ( dZ < m_vCalcLinAx[2].stroke.Min)
|
|
nStat += 16 ;
|
|
else if( dZ > m_vCalcLinAx[2].stroke.Max)
|
|
nStat += 32 ;
|
|
// eventuale primo rotante
|
|
if ( abs( nStat) >= 1) {
|
|
if ( dAngA < m_vCalcRotAx[0].stroke.Min)
|
|
nStat += 64 ;
|
|
else if( dAngA > m_vCalcRotAx[0].stroke.Max)
|
|
nStat += 128 ;
|
|
}
|
|
// eventuale secondo rotante
|
|
if ( abs( nStat) >= 2) {
|
|
if ( dAngB < m_vCalcLinAx[1].stroke.Min)
|
|
nStat += 256 ;
|
|
else if( dAngB > m_vCalcLinAx[1].stroke.Max)
|
|
nStat += 512 ;
|
|
}
|
|
return true ;
|
|
} |