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

EgtMachKernel 1.6e5 :

Browse Source 
- aggiunti calcolo angoli e assi lineari macchina.
This commit is contained in:
Dario Sassi
2015-05-12 21:56:41 +00:00
parent 79bbc3ec0e
commit 8ac8373376
12 changed files with 623 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EgtMachKernel.rc
BIN
View File
Binary file not shown.
EgtMachKernel.vcxproj
+4
View File
@@ -205,8 +205,10 @@ copy $(TargetPath) \EgtProg\Dll64</Command>
</ItemDefinitionGroup>
<ItemGroup>
<ClCompile Include="EMkDllMain.cpp" />
<ClCompile Include="GeoCalc.cpp" />
<ClCompile Include="Machine.cpp" />
<ClCompile Include="MachineAxes.cpp" />
<ClCompile Include="MachineCalc.cpp" />
<ClCompile Include="MachineLua.cpp" />
<ClCompile Include="MachineHeads.cpp" />
<ClCompile Include="MachMgrBasic.cpp" />
@@ -225,7 +227,9 @@ copy $(TargetPath) \EgtProg\Dll64</Command>
<ClInclude Include="..\Include\EMkDllMain.h" />
<ClInclude Include="..\Include\EMkMachMgr.h" />
<ClInclude Include="DllMain.h" />
<ClInclude Include="GeoCalc.h" />
<ClInclude Include="MachConst.h" />
<ClInclude Include="Machine.h" />
<ClInclude Include="MachMgr.h" />
<ClInclude Include="resource.h" />
<ClInclude Include="stdafx.h" />
EgtMachKernel.vcxproj.filters
+12
View File
@@ -54,6 +54,12 @@
<ClCompile Include="MachineAxes.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="MachineCalc.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="GeoCalc.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="DllMain.h">
@@ -77,6 +83,12 @@
<ClInclude Include="MachConst.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="GeoCalc.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="Machine.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
<ItemGroup>
<ResourceCompile Include="EgtMachKernel.rc">
GeoCalc.cpp
+102
View File
@@ -0,0 +1,102 @@
//----------------------------------------------------------------------------
// EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : GeoCalc.cpp Data : 12.05.15 Versione : 1.6e3
// Contenuto : Funzioni varie e speciali di calcolo geometrico.
//
//
//
// Modifiche : 12.05.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------
//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "GeoCalc.h"
//----------------------------------------------------------------------------
int
GetRotationComponent( const Vector3d& vtDir1, double dComp, const Vector3d& vtDir2, const Vector3d& vtRotAx,
double& dAng1Deg, double& dAng2Deg, bool& bDet)
{
// Vettori normalizzati.
// Si rimanda alle note, con vtDir1 == vtT, vtDir2 == vtZ, vtRotAx == vtW, dComp == dTaz.
// calcolo della componenete di T0 lungo W
double dT0w = vtDir1 * vtRotAx ;
// calcolo dell'asse U del riferimento UVW, coincidente con X di XYZ
Vector3d vtPvZW = vtDir2 ^ vtRotAx ;
// se Z e W sono allineati (o quasi)
Vector3d vtU = vtPvZW ;
if ( ! vtU.Normalize()) {
// se le componenti concordano, angolo indeterminato
if ( fabs( dT0w - dComp) < 0.5 * SIN_EPS_ANG_SMALL) {
bDet = false ;
return 1 ;
}
// altrimenti, nessuna soluzione
else
return 0 ;
}
// calcolo della componenete di T0 lungo U
double dT0u = vtDir1 * vtU ;
// calcolo dell'asse V di UVW
Vector3d vtV = vtRotAx ^ vtU ;
// dovrebbe essere sempre normalizzabile, ma ...
if ( ! vtV.Normalize())
return 0 ;
// calcolo della componenete di T0 lungo V
double dT0v = vtDir1 * vtV ;
// calcolo coseno e seno dell'angolo comprezo tra vtZ e vtW, visto da vtU
double dCosG = vtDir2 * vtRotAx ;
double dSinG = vtPvZW * vtU ;
// calcolo della prima parte dell'angolo
double dOffsAngRad = PIGRECO / 2 - atan2( dT0v, dT0u) ;
// calcolo della seconda parte dell'angolo
// denominatore nullo ...
if ( ( fabs( dT0v) < EPS_ZERO && fabs( dT0u) < EPS_ZERO) || fabs( dSinG) < SIN_EPS_ANG_ZERO) {
// calcolo della componenete di T0 lungo Z
double dT0z = vtDir1 * vtDir2 ;
// se le componenti concordano, angolo indeterminato
if ( fabs( dT0z - dComp) < 0.5 * SIN_EPS_ANG_SMALL) {
bDet = false ;
return 1 ;
}
// altrimenti, nessuna soluzione
else
return 0 ;
}
double dT0uv = sqrt( dT0v * dT0v + dT0u * dT0u) ;
double dDenom = dT0uv * dSinG ;
double dNumer = dComp - dT0w * dCosG ;
// due angoli possibili
if ( fabs( dDenom) > fabs( dNumer)) {
double dDeltaAngRad = acos( dNumer / dDenom) ;
dAng1Deg = ( dOffsAngRad + dDeltaAngRad) * RADTODEG ;
dAng2Deg = ( dOffsAngRad - dDeltaAngRad) * RADTODEG ;
bDet = true ;
return 2 ;
}
// un angolo possibile
else if ( fabs( dDenom) > fabs( dNumer) - SIN_EPS_ANG_ZERO) {
if ( dDenom * dNumer > 0)
dAng1Deg = dOffsAngRad * RADTODEG ;
else
dAng1Deg = dOffsAngRad * RADTODEG + ANG_STRAIGHT ;
bDet = true ;
return 1 ;
}
// nessun angolo possibile
else
return 0 ;
}
GeoCalc.h
+21
View File
@@ -0,0 +1,21 @@
//----------------------------------------------------------------------------
// EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : GeoCalc.h Data : 12.05.15 Versione : 1.6e5
// Contenuto : Prototipi funzioni di calcolo geometrico.
//
//
//
// Modifiche : 12.05.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------
#pragma once
#include "/EgtDev/Include/EGkVector3d.h"
//----------------------------------------------------------------------------
int GetRotationComponent( const Vector3d& vtDir1, double dComp, const Vector3d& vtDir2, const Vector3d& vtRotAx,
double& dAng1Deg, double& dAng2Deg, bool& bDet) ;
MachConst.h
+1 -1
View File
@@ -1,5 +1,5 @@
//----------------------------------------------------------------------------
// EgalTech 20135-2015
// EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : MachConst.h Data : 24.03.15 Versione : 1.6c6
// Contenuto : Costanti generali per calcoli lavorazioni.
MachMgr.h
+7
View File
@@ -84,6 +84,13 @@ class MachMgr : public IMachMgr
virtual bool ResetAxisPos( const std::string& sAxis) ;
virtual bool LoadTool( const std::string& sHead, int nExit, const std::string& sTool) ;
virtual bool ResetHeadSet( const std::string& sHead) ;
virtual bool SetCalcTable( const std::string& sTable) ;
virtual bool SetCalcTool( const std::string& sTool, const std::string& sHead, int nExit) ;
virtual bool GetCalcAngles( const Vector3d& vtDirT,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) ;
virtual bool GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB,
int& nStat, double& dX, double& dY, double& dZ) ;
virtual bool VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat) ;
public :
MachMgr( void) ;
MachMgrMachines.cpp
+52
View File
@@ -129,3 +129,55 @@ MachMgr::ResetHeadSet( const string& sHead)
return false ;
return m_vMachines[m_nCurrMch]->ResetHeadSet( sHead) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::SetCalcTable( const string& sTable)
{
if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
m_vMachines[m_nCurrMch] == nullptr)
return false ;
return m_vMachines[m_nCurrMch]->SetCurrTable( sTable) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::SetCalcTool( const string& sTool, const string& sHead, int nExit)
{
if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
m_vMachines[m_nCurrMch] == nullptr)
return false ;
return m_vMachines[m_nCurrMch]->SetCurrTool( sTool, sHead, nExit) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::GetCalcAngles( const Vector3d& vtDirT,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2)
{
if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
m_vMachines[m_nCurrMch] == nullptr)
return false ;
return m_vMachines[m_nCurrMch]->GetAngles( vtDirT, nStat, dAngA1, dAngB1, dAngA2, dAngB2) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB,
int& nStat, double& dX, double& dY, double& dZ)
{
if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
m_vMachines[m_nCurrMch] == nullptr)
return false ;
return m_vMachines[m_nCurrMch]->GetPositions( ptP, dAngA, dAngB, nStat, dX, dY, dZ) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat)
{
if ( m_nCurrMch < 0 || m_nCurrMch >= int( m_vMachines.size()) ||
m_vMachines[m_nCurrMch] == nullptr)
return false ;
return m_vMachines[m_nCurrMch]->VerifyOutOfStroke( dX, dY, dZ, dAngA, dAngB, nStat) ;
}
Machine.cpp
+16 -20
View File
@@ -280,14 +280,12 @@ Machine::LoadMachineStdHead( const string& sName, const string& sParent, const s
// gli assegno il tipo
string sInfo = MCH_HEAD + ToString( MCH_HT_STD) ;
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "Type", sInfo) ;
// gli assegno la posizione
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "Pos", ptPos) ;
// gli assegno la direzione utensile
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "TDir", vtTDir) ;
// gli assegno la direzione ausiliaria
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "ADir", vtADir) ;
// trasformazione del riferimento di uscita in gruppo di uscita
if ( ! CreateExitGroups( nLay, 1))
MUEXITVECTOR vMuExit ;
vMuExit.emplace_back( ptPos, vtTDir) ;
if ( ! CreateExitGroups( nLay, vMuExit))
return false ;
// lo inserisco nel dizionario dei gruppi della macchina
return m_mapGroups.emplace( sName, nLay).second ;
@@ -296,7 +294,7 @@ Machine::LoadMachineStdHead( const string& sName, const string& sParent, const s
//----------------------------------------------------------------------------
bool
Machine::LoadMachineMultiHead( const string& sName, const string& sParent, const std::string& sHSet,
MUEXITVECTOR vMuExit,
const MUEXITVECTOR& vMuExit,
const Vector3d& vtADir, const string& sGeo)
{
// recupero pezzo e layer della geometria originale dell'asse
@@ -317,24 +315,17 @@ Machine::LoadMachineMultiHead( const string& sName, const string& sParent, const
m_pMchMgr->m_pGeomDB->SetStatus( nLay, ( bShow ? GDB_ST_ON : GDB_ST_OFF)) ;
// gli assegno il nome
m_pMchMgr->m_pGeomDB->SetName( nLay, sName) ;
// gli assegno il tipo
string sInfo = MCH_HEAD + ToString( MCH_HT_MULTI) ;
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "Type", sInfo) ;
// gli assegno l'insieme teste e aggiorno il capostipite
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "HSet", sHSet) ;
if ( ! AddHeadToSet( sHSet, sName))
return false ;
// gli assegno le posizioni e direzioni delle uscite
for ( int i = 0 ; i < int( vMuExit.size()) ; ++ i) {
string sPos = "Pos" + ToString( i + 1) ;
m_pMchMgr->m_pGeomDB->SetInfo( nLay, sPos, vMuExit[i].ptPos) ;
string sTDir = "TDir" + ToString( i + 1) ;
m_pMchMgr->m_pGeomDB->SetInfo( nLay, sTDir, vMuExit[i].vtTDir) ;
}
// gli assegno il tipo
string sInfo = MCH_HEAD + ToString( MCH_HT_MULTI) ;
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "Type", sInfo) ;
// gli assegno la direzione ausiliaria
m_pMchMgr->m_pGeomDB->SetInfo( nLay, "ADir", vtADir) ;
// trasformazione dei riferimenti di uscita in gruppi di uscita
if ( ! CreateExitGroups( nLay, int( vMuExit.size())))
if ( ! CreateExitGroups( nLay, vMuExit))
return false ;
// lo inserisco nel dizionario dei gruppi della macchina
return m_mapGroups.emplace( sName, nLay).second ;
@@ -430,11 +421,11 @@ Machine::EnableHeadInSet( const string& sHead)
//----------------------------------------------------------------------------
bool
Machine::CreateExitGroups( int nLay, int nExitNbr)
Machine::CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit)
{
// ciclo sulle uscite
for ( int i = 1 ; i <= nExitNbr ; ++ i) {
string sName = "T" + ToString( i) ;
for ( int i = 0 ; i < int( vMuExit.size()) ; ++ i) {
string sName = "T" + ToString( i+1) ;
// se trovo riferimento per uscita, lo sostituisco con gruppo equivalente
int nT = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nLay, sName) ;
if ( nT != GDB_ID_NULL && m_pMchMgr->m_pGeomDB->GetGeoType( nT) == GEO_FRAME3D) {
@@ -446,7 +437,12 @@ Machine::CreateExitGroups( int nLay, int nExitNbr)
return false ;
}
m_pMchMgr->m_pGeomDB->Erase( nT) ;
// assegno nome
m_pMchMgr->m_pGeomDB->SetName( nGT, sName) ;
// gli assegno la posizione
m_pMchMgr->m_pGeomDB->SetInfo( nGT, "Pos", vMuExit[i].ptPos) ;
// gli assegno la direzione utensile
m_pMchMgr->m_pGeomDB->SetInfo( nGT, "TDir", vMuExit[i].vtTDir) ;
}
else {
string sOut = "Error finding frame " + sName ;
Machine.h
+38 -9
View File
@@ -37,6 +37,17 @@ struct MuExit {
} ;
typedef std::vector<MuExit> MUEXITVECTOR ;
//----------------------------------------------------------------------------
struct KinAxis {
int nGrpId ;
bool bLinear ;
bool bHead ;
Point3d ptPos ;
Vector3d vtDir ;
STROKE stroke ;
};
typedef std::vector<KinAxis> KINAXISVECTOR ;
//----------------------------------------------------------------------------
class Machine
{
@@ -54,6 +65,13 @@ class Machine
bool GetAxisPos( const std::string& sAxis, double& dVal) ;
bool GetAxisHomePos( const std::string& sAxis, double& dHomeVal) ;
bool ResetAxisPos( const std::string& sAxis) ;
bool SetCurrTable( const std::string& sTable) ;
bool SetCurrTool( const std::string& sTool, const std::string& sHead, int nExit) ;
bool GetAngles( const Vector3d& vtDirT,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) ;
bool GetPositions( const Point3d& ptP, double dA, double dB,
int& nStat, double& dX, double& dY, double& dZ) ;
bool VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat) ;
private :
void Clear( void) ;
@@ -68,7 +86,7 @@ class Machine
const Point3d& ptPos, const Vector3d& vtTDir,
const Vector3d& vtADir, const std::string& sGeo) ;
bool LoadMachineMultiHead( const std::string& sName, const std::string& sParent, const std::string& sHSet,
MUEXITVECTOR vMuExit,
const MUEXITVECTOR& vMuExit,
const Vector3d& vtADir, const std::string& sGeo) ;
int GetGroup( const std::string& sGroup) ;
bool IsAxisGroup( int nGroup) ;
@@ -77,7 +95,9 @@ class Machine
bool AddHeadToSet( const std::string& sHSet, const std::string& sName) ;
bool GetHSet( const std::string& sHead, STRVECTOR& vsHSet) ;
bool EnableHeadInSet( const std::string& sHead) ;
bool CreateExitGroups( int nLay, int nExitNbr) ;
bool CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit) ;
bool CalculateKinematicChain( void) ;
bool AddKinematicAxis( bool bOnHead, int nId) ;
bool LuaInit( const std::string& sMachineName) ;
bool LuaExit( void) ;
@@ -87,13 +107,22 @@ class Machine
typedef std::unordered_map< std::string, int> STRINT_UMAP ;
private :
MachMgr* m_pMchMgr ; // puntatore al gestore di tutte le lavorazioni
LuaMgr m_LuaMgr ; // interprete lua della macchina
std::string m_sName ; // nome della macchina
std::string m_sMachineDir ; // direttorio della macchina
int m_nGroupId ; // Id del gruppo della macchina
int m_nTempGroupId ; // Id del gruppo temporaneo macchina per carico
STRINT_UMAP m_mapGroups ; // dizionario dei gruppi della macchina
MachMgr* m_pMchMgr ; // puntatore al gestore di tutte le lavorazioni
LuaMgr m_LuaMgr ; // interprete lua della macchina
std::string m_sName ; // nome della macchina
std::string m_sMachineDir ; // direttorio della macchina
int m_nGroupId ; // Id del gruppo della macchina
int m_nTempGroupId ; // Id del gruppo temporaneo macchina per carico
STRINT_UMAP m_mapGroups ; // dizionario dei gruppi della macchina
int m_nCalcTabId ; // tavola corrente per calcoli
int m_nCalcHeadId ; // testa corrente per calcoli
int m_nCalcExitId ; // uscita corrente per calcoli
int m_nCalcToolId ; // utensile corrente per calcoli
Point3d m_ptCalcPos ; // posizione utensile a riposo per calcoli
Vector3d m_vtCalcDir ; // direzione utensile a riposo per calcoli
double m_dCalcTLen ; // lunghezza utensile corrente per calcoli
KINAXISVECTOR m_vCalcLinAx ; // vettore assi lineari attivi per calcoli
KINAXISVECTOR m_vCalcRotAx ; // vettore assi rotanti attivi per calcoli
// Static per interprete Lua di macchina
private :
MachineCalc.cpp
+363
View File
@@ -0,0 +1,363 @@
//----------------------------------------------------------------------------
// 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 ;
}
MachineHeads.cpp
+7
View File
@@ -46,6 +46,13 @@ Machine::LoadTool( const string& sHead, int nExit, const string& sTool)
// inserisco l'utensile nel gruppo
if ( ! m_pMchMgr->m_pGeomDB->Load( sToolFile, nExGrp))
return false ;
int nTGrpId = m_pMchMgr->m_pGeomDB->GetFirstGroupInGroup( nExGrp) ;
int nSolidId = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nTGrpId, "SOLID") ;
if ( nSolidId == GDB_ID_NULL)
return false ;
m_pMchMgr->m_pGeomDB->RelocateGlob( nSolidId, nExGrp, GDB_FIRST_SON) ;
m_pMchMgr->m_pGeomDB->Erase( nTGrpId) ;
m_pMchMgr->m_pGeomDB->SetName( nSolidId, sTool) ;
// lo ruoto 90 deg attorno alla X locale
int nT = m_pMchMgr->m_pGeomDB->GetFirstGroupInGroup( nExGrp) ;
if ( nT == GDB_ID_NULL)