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

EgtMachKernel 1.6r2 :

Browse Source 
- modifiche per gestire fino a 4 assi rotanti contemporaneamente attivi (almeno 2 bloccati)
- correzioni e migliorie varie.
This commit is contained in:
Dario Sassi
2016-05-17 07:00:08 +00:00
parent 31b4099d0f
commit 05a69a650b
20 changed files with 551 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Disposition.cpp
+12 -1
View File
@@ -343,7 +343,18 @@ Disposition::Apply(void)
FixData.nId = nId ;
}
}
// aggiornamento movimento pezzi
// annullo movimento grezzi
int nRawId = m_pMchMgr->GetFirstRawPart() ;
while ( nRawId != GDB_ID_NULL) {
if ( m_pMchMgr->VerifyRawPartPhase( nRawId, m_nPhase)) {
// recupero riferimento del grezzo
Frame3d* pfrRaw = m_pGeomDB->GetGroupFrame( nRawId) ;
// lo trasformo nel riferimento globale
pfrRaw->Reset() ;
}
nRawId = m_pMchMgr->GetNextRawPart( nRawId) ;
}
// aggiornamento movimento grezzi
for ( const auto& vMvrData : m_vMvrData) {
switch ( vMvrData.nType) {
case MoveRawData::COR :
EgtMachKernel.rc
BIN
View File
Binary file not shown.
Generator.cpp
+7 -1
View File
@@ -29,7 +29,7 @@ using namespace std ;
//----------------------------------------------------------------------------
static const string ERR_EXT = ".err" ;
static const int MAX_AXES = 5 ;
static const int MAX_AXES = 7 ;
//----------------------------------------------------------------------------
Generator::Generator( void)
@@ -708,6 +708,8 @@ Generator::GetGlobVarAxisValue( int nAx)
case 3 : return ( GLOB_VAR + GVAR_L3) ;
case 4 : return ( GLOB_VAR + GVAR_R1) ;
case 5 : return ( GLOB_VAR + GVAR_R2) ;
case 6 : return ( GLOB_VAR + GVAR_R3) ;
case 7 : return ( GLOB_VAR + GVAR_R4) ;
default : return "" ;
}
}
@@ -722,6 +724,8 @@ Generator::GetGlobVarAxisPrev( int nAx)
case 3 : return ( GLOB_VAR + GVAR_L3P) ;
case 4 : return ( GLOB_VAR + GVAR_R1P) ;
case 5 : return ( GLOB_VAR + GVAR_R2P) ;
case 6 : return ( GLOB_VAR + GVAR_R3P) ;
case 7 : return ( GLOB_VAR + GVAR_R4P) ;
default : return "" ;
}
}
@@ -736,6 +740,8 @@ Generator::GetGlobVarAxisToken( int nAx)
case 3 : return ( GLOB_VAR + GVAR_L3T) ;
case 4 : return ( GLOB_VAR + GVAR_R1T) ;
case 5 : return ( GLOB_VAR + GVAR_R2T) ;
case 6 : return ( GLOB_VAR + GVAR_R3T) ;
case 7 : return ( GLOB_VAR + GVAR_R4T) ;
default : return "" ;
}
}
GeoCalc.cpp
+3 -1
View File
@@ -32,8 +32,10 @@ GetRotationComponent( const Vector3d& vtDir1, double dComp, const Vector3d& vtDi
// se Z e W sono allineati (o quasi)
Vector3d vtU = vtPvZW ;
if ( ! vtU.Normalize()) {
// determino se equiversi o controversi
bool bEquiv = ( vtDir2 * vtRotAx) > 0 ;
// se le componenti concordano, angolo indeterminato
if ( fabs( dT0w - dComp) < 0.5 * SIN_EPS_ANG_SMALL) {
if ( fabs( dT0w - ( bEquiv ? dComp : - dComp)) < 0.5 * SIN_EPS_ANG_SMALL) {
bDet = false ;
return 1 ;
}
MachMgr.h
+10 -2
View File
@@ -74,7 +74,7 @@ class MachMgr : public IMachMgr
int GetCurrMachGroup( void) const override ;
// Phases
int AddPhase( void) override ;
bool SetCurrPhase( int nPhase) override ;
bool SetCurrPhase( int nPhase, bool bForced = false) override ;
int GetCurrPhase( void) const override ;
bool RemoveLastPhase( void) override ;
int GetPhaseCount( void) const override ;
@@ -222,8 +222,14 @@ class MachMgr : public IMachMgr
bool GetCalcTool( std::string& sTool) override ;
bool GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) override ;
bool BlockKinematicRotAxis( const std::string& sAxis, double dVal) ;
bool FreeKinematicRotAxis( const std::string& sAxis) ;
bool GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) ;
bool GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB,
int& nStat, double& dX, double& dY, double& dZ) override ;
bool GetCalcPositions( const Point3d& ptP, DBLVECTOR& vAng,
int& nStat, double& dX, double& dY, double& dZ) ;
bool GetCalcTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB,
bool bBottom, Point3d& ptTip) override ;
bool GetCalcToolDirFromAngles( double dAngA, double dAngB, Vector3d& vtDir) override ;
@@ -273,6 +279,7 @@ class MachMgr : public IMachMgr
int GetCurrOperId( void) const
{ return ( ExistsCurrMachGroup() ? m_cCurrMGrp.OperGroupId : GDB_ID_NULL) ; }
// RawParts
bool GetRawPartPhases( int nRawId, INTVECTOR& vPhase) const ;
bool GetRawPartCenter( int nRawId, Point3d& ptCen) ;
// Machines
Machine* GetCurrMachine( void) const ;
@@ -284,6 +291,7 @@ class MachMgr : public IMachMgr
bool GetAllCurrAxesName( STRVECTOR& vAxName) ;
bool GetAllCalcAxesHomePos( DBLVECTOR& vAxHomeVal) ;
double GetCalcRot1W( void) ;
bool IsKinematicRotAxisBlocked( int nInd) ;
// operations
bool GetOperationNewName( std::string& sName) const ;
const ToolData* GetMachiningToolData( void) const ;
@@ -308,7 +316,7 @@ class MachMgr : public IMachMgr
int GetMachine( const std::string& sMachineName) const ;
int GetCurrMachineId( void) const ;
// Phases
bool PrepareCurrPhase( int nPhase, bool bDoDisp) ;
bool PrepareCurrPhase( int nPhase, bool bDoDisp, bool bForced) ;
int CalcPhaseCount( void) const ;
// RawParts
int AddRawPart( int nCrvId, double dOverMat, double dZmin, double dHeight, Color cCol) ;
MachMgrBasic.cpp
+5 -5
View File
@@ -64,6 +64,11 @@ MachMgr::~MachMgr( void)
void
MachMgr::Clear( void)
{
// cancello simulatore, se necessario
if ( m_pSimul != nullptr) {
delete m_pSimul ;
m_pSimul = nullptr ;
}
// pulisco le macchine
while ( ! m_vMachines.empty()) {
if ( m_vMachines.back().pMsMgr != nullptr)
@@ -74,11 +79,6 @@ MachMgr::Clear( void)
delete m_vMachines.back().pMachine ;
m_vMachines.pop_back() ;
}
// cancello simulatore, se necessario
if ( m_pSimul != nullptr) {
delete m_pSimul ;
m_pSimul = nullptr ;
}
}
//----------------------------------------------------------------------------
MachMgrMachGroups.cpp
+4
View File
@@ -303,6 +303,8 @@ MachMgr::PrepareCurrMachGroup( int nId)
return false ;
// se esiste gruppo corrente, lo disabilito
if ( m_nCurrMGrpId != GDB_ID_NULL) {
// termino eventuale simulazione attiva
SimStop() ;
// riporto i pezzi nei grezzi precedenti nella loro posizione standard
SwapParts( false) ;
// nascondo precedente gruppo corrente e la relativa macchina
@@ -334,6 +336,8 @@ MachMgr::ResetCurrMachGroup( void)
// se non c'è gruppo corrente, posso uscire subito
if ( m_nCurrMGrpId == GDB_ID_NULL)
return true ;
// termino eventuale simulazione attiva
SimStop() ;
// riporto i pezzi nei grezzi nella loro posizione standard
SwapParts( false) ;
// visualizzo pezzi rimasti sotto la radice
MachMgrMachines.cpp
+43
View File
@@ -302,6 +302,39 @@ MachMgr::GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
return ( ( pMch != nullptr) ? pMch->GetAngles( vtDirT, vtDirA, nStat, dAngA1, dAngB1, dAngA2, dAngB2) : false) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::BlockKinematicRotAxis( const string& sAxis, double dVal)
{
Machine* pMch = GetCurrMachine() ;
return ( ( pMch != nullptr) ? pMch->BlockKinematicRotAxis( sAxis, dVal) : false) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::FreeKinematicRotAxis( const string& sAxis)
{
Machine* pMch = GetCurrMachine() ;
return ( ( pMch != nullptr) ? pMch->FreeKinematicRotAxis( sAxis) : false) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::IsKinematicRotAxisBlocked( int nInd)
{
Machine* pMch = GetCurrMachine() ;
return ( ( pMch != nullptr) ? pMch->IsKinematicRotAxisBlocked( nInd) : false) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2)
{
Machine* pMch = GetCurrMachine() ;
return ( ( pMch != nullptr) ? pMch->GetAngles( vtDirT, vtDirA, nStat, vAng1, vAng2) : false) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB,
@@ -311,6 +344,16 @@ MachMgr::GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB,
return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, dAngA, dAngB, nStat, dX, dY, dZ) : false) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::GetCalcPositions( const Point3d& ptP, DBLVECTOR& vAng,
int& nStat, double& dX, double& dY, double& dZ)
{
Machine* pMch = GetCurrMachine() ;
return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, nStat, dX, dY, dZ) : false) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::GetCalcTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB,
MachMgrPhases.cpp
+17 -26
View File
@@ -39,7 +39,7 @@ MachMgr::AddPhase( void)
// incremento il numero totale delle fasi
++ m_nPhasesCount ;
// imposto la nuova fase corrente
PrepareCurrPhase( m_nPhasesCount, false) ;
PrepareCurrPhase( m_nPhasesCount, false, true) ;
// inserisco la prima operazione di fase, sempre una disposizione
string sName = "Disp" + ToString( m_nCurrPhase, 2) ;
m_nCurrDispId = AddDisposition( sName) ;
@@ -56,34 +56,34 @@ MachMgr::AddPhase( void)
//----------------------------------------------------------------------------
bool
MachMgr::SetCurrPhase( int nPhase)
MachMgr::SetCurrPhase( int nPhase, bool bForced)
{
return PrepareCurrPhase( nPhase, true) ;
return PrepareCurrPhase( nPhase, true, bForced) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::PrepareCurrPhase( int nPhase, bool bDoDisp)
MachMgr::PrepareCurrPhase( int nPhase, bool bDoDisp, bool bForced)
{
// verifico esistenza fase
if ( nPhase <= 0 || nPhase > m_nPhasesCount)
return false ;
// se coincide con la vecchia fase, non devo fare alcunché
if ( nPhase == m_nCurrPhase)
// se non forzato e coincide con la vecchia fase, non devo fare alcunché
if ( ! bForced && nPhase == m_nCurrPhase)
return true ;
// se c'è lavorazione corrente e non appartiene a questa fase, reset
const Machining* pMch = GetMachining( m_pGeomDB->GetUserObj( GetCurrMachining())) ;
if ( pMch != nullptr && pMch->GetPhase() != nPhase)
ResetCurrMachining() ;
// riporto tutti i grezzi linkati nel gruppo dei grezzi
Machine* pMach = GetCurrMachine() ;
if ( pMach != nullptr)
pMach->UnlinkAllRawPartsFromGroups() ;
// tolgo i pezzi dai grezzi che non appartengono alla fase
int nRawId = GetFirstRawPart() ;
while ( nRawId != GDB_ID_NULL) {
// recupero le fasi in cui è presente il grezzo (se manca è fase 1)
INTVECTOR vPhase ;
if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty())
vPhase.emplace_back( 1) ;
// se non appartiene
if ( find( vPhase.begin(), vPhase.end(), nPhase) == vPhase.end())
// se non appartiene alla fase
if ( ! VerifyRawPartPhase( nRawId, nPhase))
SwapRawPartParts( nRawId, false) ;
// passo al prossimo
nRawId = GetNextRawPart( nRawId) ;
@@ -91,12 +91,8 @@ MachMgr::PrepareCurrPhase( int nPhase, bool bDoDisp)
// inserisco i pezzi nei grezzi che appartengono alla fase
nRawId = GetFirstRawPart() ;
while ( nRawId != GDB_ID_NULL) {
// recupero le fasi in cui è presente il grezzo (se manca è fase 1)
INTVECTOR vPhase ;
if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty())
vPhase.emplace_back( 1) ;
// se appartiene
if ( find( vPhase.begin(), vPhase.end(), nPhase) != vPhase.end())
// se appartiene alla fase
if ( VerifyRawPartPhase( nRawId, nPhase))
SwapRawPartParts( nRawId, true) ;
// passo al prossimo
nRawId = GetNextRawPart( nRawId) ;
@@ -104,12 +100,8 @@ MachMgr::PrepareCurrPhase( int nPhase, bool bDoDisp)
// aggiorno stato grezzi
nRawId = GetFirstRawPart() ;
while ( nRawId != GDB_ID_NULL) {
// recupero le fasi in cui è presente il grezzo
INTVECTOR vPhase ;
if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty())
vPhase.emplace_back( 1) ;
// verifico
bool bOn = ( find( vPhase.begin(), vPhase.end(), nPhase) != vPhase.end()) ;
// verifico se appartiene alla fase
bool bOn = VerifyRawPartPhase( nRawId, nPhase) ;
m_pGeomDB->SetStatus( nRawId, ( bOn ? GDB_ST_ON : GDB_ST_OFF)) ;
// passo al prossimo
nRawId = GetNextRawPart( nRawId) ;
@@ -185,8 +177,7 @@ MachMgr::RemoveLastPhase( void)
int nNextRawId = GetNextRawPart( nRawId) ;
// recupero le fasi in cui è presente il grezzo
INTVECTOR vPhase ;
if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty())
vPhase.emplace_back( 1) ;
GetRawPartPhases( nRawId, vPhase) ;
// se appartiene alla fase
auto iIter = find( vPhase.begin(), vPhase.end(), m_nPhasesCount) ;
if ( iIter != vPhase.end()) {
MachMgrRawParts.cpp
+21 -15
View File
@@ -519,15 +519,27 @@ MachMgr::ModifyRawPartHeight( int nRawId, double dHeight)
//----------------------------------------------------------------------------
bool
MachMgr::KeepRawPart( int nRawId)
MachMgr::GetRawPartPhases( int nRawId, INTVECTOR& vPhase) const
{
// pulisco parametro di ritorno
vPhase.clear() ;
// verifica validità grezzo
if ( ! VerifyRawPart( nRawId))
return false ;
// recupero le fasi in cui è presente il grezzo (se manca è fase 1)
INTVECTOR vPhase ;
if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty())
vPhase.emplace_back( 1) ;
return true ;
}
//----------------------------------------------------------------------------
bool
MachMgr::KeepRawPart( int nRawId)
{
// verifico validità e recupero fasi in cui è presente
INTVECTOR vPhase ;
if ( ! GetRawPartPhases( nRawId, vPhase))
return false ;
// se fase corrente già presente, non devo fare alcunché
if ( find( vPhase.begin(), vPhase.end(), m_nCurrPhase) != vPhase.end())
return true ;
@@ -545,13 +557,10 @@ MachMgr::KeepRawPart( int nRawId)
bool
MachMgr::VerifyRawPartPhase( int nRawId, int nPhase) const
{
// verifico validità grezzo
if ( ! VerifyRawPart( nRawId))
return false ;
// verifico sia attivo nella fase indicata
// verifico validità e recupero fasi in cui è presente
INTVECTOR vPhase ;
if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty())
vPhase.emplace_back( 1) ;
if ( ! GetRawPartPhases( nRawId, vPhase))
return false ;
return ( find( vPhase.begin(), vPhase.end(), nPhase) != vPhase.end()) ;
}
@@ -559,13 +568,10 @@ MachMgr::VerifyRawPartPhase( int nRawId, int nPhase) const
bool
MachMgr::RemoveRawPartFromCurrPhase( int nRawId)
{
// verifica validità grezzo
if ( ! VerifyRawPart( nRawId))
return false ;
// recupero le fasi in cui è presente il grezzo (se manca è fase 1)
// verifico validità e recupero fasi in cui è presente
INTVECTOR vPhase ;
if ( ! m_pGeomDB->GetInfo( nRawId, MACH_RAW_PHASE, vPhase) || vPhase.empty())
vPhase.emplace_back( 1) ;
if ( ! GetRawPartPhases( nRawId, vPhase))
return false ;
// se non appartiene alla fase corrente, non devo fare alcunché
auto iIter = find( vPhase.begin(), vPhase.end(), m_nPhasesCount) ;
if ( iIter == vPhase.end())
@@ -726,7 +732,7 @@ MachMgr::GetRawPartCenter( int nRawId, Point3d& ptCen)
// provo a crearlo
SetRawPartCenter( nRawId) ;
nGPntId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_CENTER) ;
// se c'è ancora errore
// se non riesco, errore
if ( nGPntId == GDB_ID_NULL)
return false ;
}
MachMgrSimulation.cpp
+1 -2
View File
@@ -112,8 +112,7 @@ MachMgr::SimStop( void)
// verifico simulatore
if ( m_pSimul == nullptr)
return true ;
// lo fermo e cancello
m_pSimul->Stop() ;
// lo cancello (il distruttore provvede ovviamente a fermarlo)
delete m_pSimul ;
m_pSimul = nullptr ;
return true ;
Machine.cpp
+26 -17
View File
@@ -211,7 +211,7 @@ Machine::LoadMachineTable( const string& sName, const string& sParent, int nType
if ( ! AdjustAuxGeometry( vsAux, nLay))
return false ;
// aggiusto la posizione della tavola
if ( ! AdjustTablePos( nLay, ptRef1))
if ( ! AdjustTable( nLay, ptRef1))
return false ;
// recupero l'area valida
int nAreaId = m_pGeomDB->GetFirstNameInGroup( nLay, MCH_TAREA + "1") ;
@@ -230,7 +230,7 @@ Machine::LoadMachineTable( const string& sName, const string& sParent, int nType
//----------------------------------------------------------------------------
bool
Machine::AdjustTablePos( int nLay, const Point3d& ptRef1)
Machine::AdjustTable( int nLay, const Point3d& ptRef1)
{
// riferimento globale del gruppo tavola
Frame3d frTable ;
@@ -261,8 +261,8 @@ Machine::AdjustTablePos( int nLay, const Point3d& ptRef1)
//----------------------------------------------------------------------------
bool
Machine::LoadMachineAxis( const string& sName, const string& sParent, int nType,
const Point3d& ptPos, const Vector3d& vtDir,
const STROKE& Stroke, double dHome, const string& sGeo, const STRVECTOR& vsAux)
const Point3d& ptPos, const Vector3d& vtDir, const STROKE& Stroke, double dHome,
bool bAdjustAux, const string& sGeo, const STRVECTOR& vsAux)
{
// verifico sia di tipo ammesso
if ( nType != MCH_AT_LINEAR && nType != MCH_AT_ROTARY) {
@@ -305,7 +305,7 @@ Machine::LoadMachineAxis( const string& sName, const string& sParent, int nType,
pAxis->Set( sName, nType, ptPos, vtDir, Stroke, dHome) ;
m_pGeomDB->SetUserObj( nLay, pAxis) ;
// verifico il vettore rappresentativo dell'asse
if ( ! AdjustAxisVector( nLay, sPart, sName, nType, ptPos, vtDir))
if ( ! AdjustAxis( nLay, sPart, sName, nType, ptPos, vtDir, bAdjustAux))
return false ;
// lo inserisco nel dizionario dei gruppi della macchina
return m_mapGroups.emplace( sName, nLay).second ;
@@ -313,8 +313,8 @@ Machine::LoadMachineAxis( const string& sName, const string& sParent, int nType,
//----------------------------------------------------------------------------
bool
Machine::AdjustAxisVector( int nLay, const string& sPart, const string& sName,
int nType, const Point3d& ptPos, const Vector3d& vtDir)
Machine::AdjustAxis( int nLay, const string& sPart, const string& sName,
int nType, const Point3d& ptPos, const Vector3d& vtDir, bool bAdjustAux)
{
// verifico presenza vettore asse
int nId = m_pGeomDB->GetFirstNameInGroup( nLay, sPart) ;
@@ -355,10 +355,9 @@ Machine::AdjustAxisVector( int nLay, const string& sPart, const string& sName,
LOG_ERROR( GetEMkLogger(), sOut.c_str()) ;
return false ;
}
return true ;
}
// altrimenti asse rotante, devo verificarne direzione e coassialità
// altrimenti asse rotante, devo verificarne direzione e coassialità (questa solo se non abilitato movimento)
else {
Vector3d vtDirN = vtDir ;
if ( ! vtDirN.Normalize() || ! AreSameVectorApprox( vtAxis, vtDirN)) {
@@ -367,10 +366,10 @@ Machine::AdjustAxisVector( int nLay, const string& sPart, const string& sName,
return false ;
}
Vector3d vtDelta = ptPos - ptBase ;
vtDelta -= ( vtDelta * vtDirN) * vtDirN ;
if ( ! vtDelta.IsSmall()) {
if ( vtDelta.Len() > m_dAxisMaxAdjust) {
string sOut = " Wrong Axis Base move = (" + ToString( vtDelta) + ")" ;
Vector3d vtDeltaPerp = vtDelta - ( vtDelta * vtDirN) * vtDirN ;
if ( ! vtDeltaPerp.IsSmall()) {
if ( vtDeltaPerp.Len() > m_dAxisMaxAdjust && ! bAdjustAux) {
string sOut = " Wrong Axis Base move = (" + ToString( vtDeltaPerp) + ")" ;
LOG_ERROR( GetEMkLogger(), sOut.c_str()) ;
return false ;
}
@@ -380,8 +379,18 @@ Machine::AdjustAxisVector( int nLay, const string& sPart, const string& sName,
m_pGeomDB->TranslateGlob( nId, vtDelta) ;
}
}
return true ;
}
// se richiesto, muovo la geometria ausiliaria
if ( bAdjustAux) {
Vector3d vtDelta = ptPos - ptBase ;
int nIdAux = m_pGeomDB->GetFirstInGroup( nLay) ;
while ( nIdAux != GDB_ID_NULL) {
if ( nIdAux != nId)
m_pGeomDB->TranslateGlob( nIdAux, vtDelta) ;
nIdAux = m_pGeomDB->GetNext( nIdAux) ;
}
}
return true ;
}
//----------------------------------------------------------------------------
@@ -874,7 +883,7 @@ Machine::LinkRawPartToGroup( int nRawPartId, const std::string& sGroupName)
if ( m_pGeomDB->GetParentId( nRawPartId) != nRawGrpId)
return false ;
// recupero il gruppo di macchina indicato
int nGrpId = m_pMchLua->GetGroup( sGroupName) ;
int nGrpId = GetGroup( sGroupName) ;
if ( nGrpId == GDB_ID_NULL)
return false ;
// aggancio il grezzo al gruppo
@@ -893,7 +902,7 @@ Machine::UnlinkRawPartFromGroup( int nRawPartId)
if ( m_pGeomDB == nullptr || m_pMchMgr == nullptr)
return false ;
// recupero il gruppo dei grezzi dalla macchina
int nRawGrpId = m_pMchLua->m_pMchMgr->GetCurrRawGroupId() ;
int nRawGrpId = m_pMchMgr->GetCurrRawGroupId() ;
if ( nRawGrpId == GDB_ID_NULL)
return false ;
// verifico che il grezzo indicato sia nell'elenco dei linkati
@@ -901,7 +910,7 @@ Machine::UnlinkRawPartFromGroup( int nRawPartId)
if ( iIter == m_vLinkedRawParts.end())
return false ;
// riporto il grezzo nel gruppo dei grezzi
if ( ! m_pMchLua->m_pGeomDB->RelocateGlob( nRawPartId, nRawGrpId))
if ( ! m_pGeomDB->RelocateGlob( nRawPartId, nRawGrpId))
return false ;
// tolgo il grezzo dall'elenco dei linkati
m_vLinkedRawParts.erase( iIter) ;
Machine.h
+19 -6
View File
@@ -68,14 +68,23 @@ class Machine
bool GetCurrTool( std::string& sTool) const ;
double GetCurrRot1W( void) const ;
std::string GetKinematicAxis( int nInd) const ;
bool BlockKinematicRotAxis( const std::string& sName, double dVal) ;
bool BlockKinematicRotAxis( int nId, double dVal) ;
bool FreeKinematicRotAxis( const std::string& sName) ;
bool FreeKinematicRotAxis( int nId) ;
bool IsKinematicRotAxisBlocked( int nInd) ;
int GetCurrLinAxes( void) const ;
int GetCurrRotAxes( void) const ;
bool GetAllCurrAxesName( STRVECTOR& vAxName) const ;
bool GetAllCurrAxesHomePos( DBLVECTOR& vAxHomeVal) const ;
bool GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const ;
bool GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const ;
bool GetPositions( const Point3d& ptP, double dA, double dB,
int& nStat, double& dX, double& dY, double& dZ) const ;
bool GetPositions( const Point3d& ptP, DBLVECTOR vAng,
int& nStat, double& dX, double& dY, double& dZ) const ;
bool GetTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB,
bool bBottom, Point3d& ptTip) const ;
bool GetToolDirFromAngles( double dAngA, double dAngB, Vector3d& vtDir) const ;
@@ -105,12 +114,12 @@ class Machine
bool AdjustAuxGeometry( const STRVECTOR& vsAux, int nLay) ;
bool LoadMachineTable( const std::string& sName, const std::string& sParent, int nType,
const Point3d& ptRef1, const std::string& sGeo, const STRVECTOR& vsAux) ;
bool AdjustTablePos( int nLay, const Point3d& ptRef1) ;
bool AdjustTable( int nLay, const Point3d& ptRef1) ;
bool LoadMachineAxis( const std::string& sName, const std::string& sParent, int nType,
const Point3d& ptPos, const Vector3d& vtDir,
const STROKE& Stroke, double dHome, const std::string& sGeo, const STRVECTOR& vsAux) ;
bool AdjustAxisVector( int nLay, const std::string& sPart, const std::string& sName,
int nType, const Point3d& ptPos, const Vector3d& vtDir) ;
const Point3d& ptPos, const Vector3d& vtDir, const STROKE& Stroke, double dHome,
bool bAdjustAux, const std::string& sGeo, const STRVECTOR& vsAux) ;
bool AdjustAxis( int nLay, const std::string& sPart, const std::string& sName,
int nType, const Point3d& ptPos, const Vector3d& vtDir, bool bAdjustAux) ;
bool ModifyMachineAxisStroke( const std::string& sName, const STROKE& Stroke) ;
bool ModifyMachineAxisHome( const std::string& sName, double dHome) ;
bool LoadMachineStdHead( const std::string& sName, const std::string& sParent, const std::string& sHSet,
@@ -148,9 +157,13 @@ class Machine
bool CreateExitGroups( int nLay, const MUEXITVECTOR& vMuExit) ;
bool CalculateKinematicChain( void) ;
bool AddKinematicAxis( bool bOnHead, int nId) ;
bool 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 ;
bool GetDirection( const Vector3d& vtDir, double dAngA, double dAngB, Vector3d& vtNew) const ;
bool VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh) const ;
bool AdjustAngleInStroke( int nId, double& dAng) const ;
bool AdjustAngleInStroke( const STROKE& Stroke, double& dAng) const ;
bool LuaInit( const std::string& sMachineName) ;
bool LuaExit( void) ;
bool LuaLoadMachine( const std::string& sFile) ;
MachineCalc.cpp
+299 -33
View File
@@ -277,6 +277,22 @@ Machine::CalculateKinematicChain( void)
}
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
return false ;
}
@@ -332,18 +348,193 @@ Machine::GetKinematicAxis( int nInd) const
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)
{
int nRotAxTot = int( m_vCalcRotAx.size()) ;
if ( nInd < 0 || nInd >= nRotAxTot)
return false ;
return m_vCalcRotAx[nInd].bFixed ;
}
//----------------------------------------------------------------------------
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 ;
// 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())
@@ -352,50 +543,61 @@ Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
Vector3d vtDirAn = vtDirA ;
vtDirAn.Normalize() ;
// direzione fresa su testa a riposo
Vector3d vtDirH = m_vtCalcDir ;
Vector3d vtDirHn = vtDirH ;
// direzione ausiliaria su testa a riposo
Vector3d vtDirI = m_vtCalcADir ;
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 = m_vCalcRotAx[0].vtDir ;
// se asse di tavola, ne inverto la direzione
if ( ! m_vCalcRotAx[0].bHead)
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 ( m_vCalcRotAx.size() == 2) {
if ( nNumRotAx == 2) {
// direzione secondo asse rotante
Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
Vector3d vtAx2 = RotAx2.vtDir ;
// se asse di tavola, ne inverto la direzione
if ( ! m_vCalcRotAx[1].bHead)
if ( ! RotAx2.bHead)
vtAx2.Invert() ;
// calcolo secondo angolo di rotazione
nStat = GetRotationComponent( vtDirH, dCompTSuAxR1, vtAx1, vtAx2, dAngB1, dAngB2, bDet) ;
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 = vtDirH ;
vtDirH1 = vtDirHn ;
vtDirH1.Rotate( vtAx2, dAngB1) ;
vtDirI1 = vtDirI ;
vtDirI1 = vtDirIn ;
vtDirI1.Rotate( vtAx2, dAngB1) ;
}
if ( nStat == 2) {
vtDirH2 = vtDirH ;
vtDirH2 = vtDirHn ;
vtDirH2.Rotate( vtAx2, dAngB2) ;
vtDirI2 = vtDirI ;
vtDirI2 = vtDirIn ;
vtDirI2.Rotate( vtAx2, dAngB2) ;
}
}
// altrimenti verifico se compatibili
else {
// componente versore utensile su direzione primo asse
double dCompHSuAxR1 = vtDirH * vtAx1 ;
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) ;
@@ -405,8 +607,8 @@ Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
const double SIN_ANG_ERROR = sin( 0.002 * DEGTORAD) ;
if ( fabs( dCompTOrtAxR1 * dCompHSuAxR1 - dCompHOrtAxR1 * dCompTSuAxR1) < SIN_ANG_ERROR) {
nStat = 1 ;
vtDirH1 = vtDirH ;
vtDirI1 = vtDirI ;
vtDirH1 = vtDirHn ;
vtDirI1 = vtDirIn ;
// reset secondo angolo
dAngB1 = 0 ;
}
@@ -469,12 +671,14 @@ Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
// verifiche dei limiti di corsa
if ( nStat >= 2) {
// se non riesco ad aggiustare, elimino
if ( ! AdjustAngleInStroke( 1, dAngA2) || ! AdjustAngleInStroke( 2, dAngB2))
if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA2) ||
( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, dAngB2)))
-- nStat ;
}
if ( nStat >= 1) {
// se non riesco ad aggiustare, elimino
if ( ! AdjustAngleInStroke( 1, dAngA1) || ! AdjustAngleInStroke( 2, dAngB1)) {
if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA1) ||
( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, dAngB1))) {
-- nStat ;
// riloco eventuale soluzione rimasta
if ( nStat >= 1) {
@@ -497,7 +701,28 @@ 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 punto di lavoro con pezzo immobile
Point3d ptW = ptP ;
// se c'è primo asse rotante di tavola
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 punto di lavoro
ptW.Rotate( ptAx1, vtAx1, dAngA) ;
}
// se c'è secondo asse rotante di tavola
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 punto di lavoro
ptW.Rotate( ptAx2, vtAx2, dAngB) ;
}
// posizione e direzione fresa su testa a riposo
Point3d ptPosH = m_ptCalcPos ;
@@ -533,9 +758,54 @@ Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB,
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 ;
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::GetPositions( const Point3d& ptP, DBLVECTOR vAng,
int& nStat, double& dX, double& dY, double& dZ) const
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// aggiorno punto di lavoro mediante ciclo diretto sugli assi di tavola
Point3d ptW = ptP ;
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]) ;
}
// 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 ;
@@ -639,18 +909,14 @@ Machine::VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh)
//----------------------------------------------------------------------------
bool
Machine::AdjustAngleInStroke( int nId, double& dAng) const
Machine::AdjustAngleInStroke( const STROKE& Stroke, 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)
while ( dAng < Stroke.Min)
dAng += ANG_FULL ;
while ( dAng > m_vCalcRotAx[nId-1].stroke.Max)
while ( dAng > Stroke.Max)
dAng -= ANG_FULL ;
return ( dAng >= m_vCalcRotAx[nId-1].stroke.Min &&
dAng <= m_vCalcRotAx[nId-1].stroke.Max) ;
return ( dAng >= Stroke.Min && dAng <= Stroke.Max) ;
}
//----------------------------------------------------------------------------
MachineLua.cpp
+6 -1
View File
@@ -40,6 +40,7 @@ static const string FLD_POS = "Pos" ;
static const string FLD_DIR = "Dir" ;
static const string FLD_STROKE = "Stroke" ;
static const string FLD_HOME = "Home" ;
static const string FLD_ADJUSTAUX = "AdjustAux" ;
static const string FLD_HSET = "HSet" ;
static const string FLD_TDIR = "TDir" ;
static const string FLD_ADIR = "ADir" ;
@@ -384,6 +385,9 @@ Machine::LuaEmtAxis( lua_State* L)
// lettura eventuale campo 'Home' dalla tabella (default 0)
double dHome = 0 ;
LuaGetTabFieldParam( L, 1, FLD_HOME, dHome) ;
// lettura eventuale campo 'AdjustAux' dalla tabella (defualt false)
bool bAdjustAux = false ;
LuaGetTabFieldParam( L, 1, FLD_ADJUSTAUX, bAdjustAux) ;
// lettura campo 'Geo' dalla tabella
string sGeo ;
LuaCheckTabFieldParam( L, 1, FLD_GEO, sGeo)
@@ -401,7 +405,8 @@ Machine::LuaEmtAxis( lua_State* L)
return luaL_error( L, " Unknown Machine") ;
// carico i dati dell'asse
if ( ! m_pMchLua->LoadMachineAxis( sName, sParent, nType, ptPos, vtDir, Stroke, dHome, sGeo, vsAux))
if ( ! m_pMchLua->LoadMachineAxis( sName, sParent, nType, ptPos, vtDir,
Stroke, dHome, bAdjustAux, sGeo, vsAux))
return luaL_error( L, " Load Machine Axis failed") ;
return 0 ;
MachineStruConst.h
+7 -1
View File
@@ -14,6 +14,7 @@
#pragma once
#include "/EgtDev/Include/EGkPoint3d.h"
#include "/EgtDev/Include/EGkGdbConst.h"
#include <string>
#include <vector>
@@ -46,7 +47,12 @@ struct KinAxis {
Point3d ptPos ;
Vector3d vtDir ;
STROKE stroke ;
};
bool bFixed ;
double dFixVal ;
KinAxis( void)
: nGrpId( GDB_ID_NULL), bLinear( true), bHead( true), ptPos(), vtDir(), bFixed( false), dFixVal( 0)
{ stroke.Min = 0 ; stroke.Max = 0 ; }
} ;
typedef std::vector<KinAxis> KINAXISVECTOR ;
//----------------------------------------------------------------------------
Operation.cpp
+55 -50
View File
@@ -454,24 +454,19 @@ Operation::CalculateAxesValues( void)
int nRotAxes = m_pMchMgr->GetCurrRotAxes() ;
// assegno gli angoli iniziali
double dAngAprec = 0 ;
double dAngBprec = 0 ;
DBLVECTOR vAxRotPrec( nRotAxes, 0.) ;
DBLVECTOR vAxVal ;
// se utensile non cambiato, uso gli angoli finali della lavorazione precedente
if ( ! sPrevTool.empty() && GetToolName() == sPrevTool &&
pPrevOp->GetFinalAxesValues( vAxVal)) {
if ( nRotAxes >= 1)
dAngAprec = vAxVal[nLinAxes - 1 + 1] ;
if ( nRotAxes >= 2)
dAngBprec = vAxVal[nLinAxes - 1 + 2] ;
for ( int i = 0 ; i < nRotAxes ; ++ i)
vAxRotPrec[i] = vAxVal[nLinAxes + i] ;
}
// altrimenti uso gli angoli home
else {
m_pMchMgr->GetAllCalcAxesHomePos( vAxVal) ;
if ( nRotAxes >= 1)
dAngAprec = vAxVal[nLinAxes - 1 + 1] ;
if ( nRotAxes >= 2)
dAngBprec = vAxVal[nLinAxes - 1 + 2] ;
for ( int i = 0 ; i < nRotAxes ; ++ i)
vAxRotPrec[i] = vAxVal[nLinAxes + i] ;
}
// recupero peso primo asse rotante di testa
@@ -486,15 +481,14 @@ Operation::CalculateAxesValues( void)
bool bOk = true ;
int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ;
while ( nClPathId != GDB_ID_NULL) {
double dAngAprecOri = dAngAprec ;
double dAngBprecOri = dAngBprec ;
DBLVECTOR vAxRotPrecOri = vAxRotPrec ;
int nOutStrC = 0 ;
if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, dAngAprec, dAngBprec, nOutStrC)) {
if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, vAxRotPrec, nOutStrC)) {
// se extracorsa dell'asse C, provo a precaricarlo al contrario
if ( nOutStrC != 0) {
dAngAprec = dAngAprecOri + ( nOutStrC > 0 ? - ANG_FULL : ANG_FULL) ;
dAngBprec = dAngBprecOri ;
if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, dAngAprec, dAngBprec, nOutStrC))
vAxRotPrec[0] = vAxRotPrecOri[0] + ( nOutStrC > 0 ? - ANG_FULL : ANG_FULL) ;
vAxRotPrec[1] = vAxRotPrecOri[1] ;
if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, vAxRotPrec, nOutStrC))
bOk = false ;
}
else
@@ -509,7 +503,7 @@ Operation::CalculateAxesValues( void)
//----------------------------------------------------------------------------
bool
Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
double& dAngAprec, double& dAngBprec, int& nOutStrC)
DBLVECTOR& vAxRotPrec, int& nOutStrC)
{
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
return false ;
@@ -538,8 +532,9 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
}
// calcolo degli assi rotanti della macchina
int nRStat ;
double dAngA1, dAngB1, dAngA2, dAngB2 ;
bool bROk = m_pMchMgr->GetCalcAngles( pCamData->GetToolDir(), pCamData->GetAuxDir(), nRStat, dAngA1, dAngB1, dAngA2, dAngB2) ;
//m_pMchMgr->BlockKinematicRotAxis( "C", 90) ;
DBLVECTOR vAng1, vAng2 ;
bool bROk = m_pMchMgr->GetCalcAngles( pCamData->GetToolDir(), pCamData->GetAuxDir(), nRStat, vAng1, vAng2) ;
if ( ! bROk || nRStat == 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ;
@@ -549,41 +544,46 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
// se primo movimento
if ( bFirst) {
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
m_pMchMgr->GetNearestAngleInStroke( 1, dAngAprec, dAngA1) ;
m_pMchMgr->GetNearestAngleInStroke( 2, dAngBprec, dAngB1) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i)
m_pMchMgr->GetNearestAngleInStroke( int( i), vAxRotPrec[i], vAng1[i]) ;
}
// per movimenti successivi
else {
// scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse
dAngA1 = AngleNearAngle( dAngA1, dAngAprec) ;
dAngB1 = AngleNearAngle( dAngB1, dAngBprec) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i)
vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ;
}
}
if ( nRStat == 2) {
// se primo movimento
if ( bFirst) {
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
m_pMchMgr->GetNearestAngleInStroke( 1, dAngAprec, dAngA1) ;
m_pMchMgr->GetNearestAngleInStroke( 2, dAngBprec, dAngB1) ;
m_pMchMgr->GetNearestAngleInStroke( 1, dAngAprec, dAngA2) ;
m_pMchMgr->GetNearestAngleInStroke( 2, dAngBprec, dAngB2) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i) {
m_pMchMgr->GetNearestAngleInStroke( int( i), vAxRotPrec[i], vAng1[i]) ;
m_pMchMgr->GetNearestAngleInStroke( int( i), vAxRotPrec[i], vAng2[i]) ;
}
}
else {
// scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse
dAngA1 = AngleNearAngle( dAngA1, dAngAprec) ;
dAngB1 = AngleNearAngle( dAngB1, dAngBprec) ;
dAngA2 = AngleNearAngle( dAngA2, dAngAprec) ;
dAngB2 = AngleNearAngle( dAngB2, dAngBprec) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i) {
vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ;
vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ;
}
}
// scelgo la soluzione più vicina ai precedenti
double dDeltaA1 = dRot1W * fabs( dAngA1 - dAngAprec) ;
double dDeltaB1 = fabs( dAngB1 - dAngBprec) ;
double dDeltaA2 = dRot1W * fabs( dAngA2 - dAngAprec) ;
double dDeltaB2 = fabs( dAngB2 - dAngBprec) ;
if ( dDeltaA2 + dDeltaB2 < dDeltaA1 + dDeltaB1 - 1.) {
dAngA1 = dAngA2 ;
dAngB1 = dAngB2 ;
double dDelta1 = 0 ;
double dDelta2 = 0 ;
bool bFirst = true ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i) {
// ignoro gli assi bloccati
if ( ! m_pMchMgr->IsKinematicRotAxisBlocked( int( i)))
continue ;
// calcolo i delta asse con eventuale peso
dDelta1 += fabs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ;
dDelta2 += fabs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? dRot1W : 1) ;
}
if ( dDelta2 < dDelta1 - 1.)
vAng1 = vAng2 ;
}
// ricavo posizione con eventuali modifiche dipendenti dalla lavorazione
Point3d ptP = pCamData->GetEndPoint() ;
@@ -591,7 +591,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
// calcolo gli assi lineari della macchina
int nLStat ;
double dX, dY, dZ ;
bool bLOk = m_pMchMgr->GetCalcPositions( ptP, dAngA1, dAngB1, nLStat, dX, dY, dZ) ;
bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAng1, nLStat, dX, dY, dZ) ;
if ( ! bLOk || nLStat != 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
@@ -602,11 +602,13 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
vAxVal.emplace_back( dX) ;
vAxVal.emplace_back( dY) ;
vAxVal.emplace_back( dZ) ;
vAxVal.emplace_back( dAngA1) ;
vAxVal.emplace_back( dAngB1) ;
for ( auto dAng : vAng1)
vAxVal.emplace_back( dAng) ;
// verifico i limiti di corsa degli assi
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( dX, dY, dZ, dAngA1, dAngB1, nStat) ;
double dAngA = ( vAng1.size() >= 1 ? vAng1[0] : 0) ;
double dAngB = ( vAng1.size() >= 2 ? vAng1[1] : 0) ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( dX, dY, dZ, dAngA, dAngB, nStat) ;
if ( ! bOsOk || nStat != 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
@@ -630,11 +632,12 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
Point3d ptMid = ptP ;
ptMid.Rotate( ptCen, Z_AX, - dAngCen / 2) ;
// determino i valori degli assi al punto medio
double dAngAmid = ( dAngAprec + dAngA1) / 2 ;
double dAngBmid = ( dAngBprec + dAngB1) / 2 ;
DBLVECTOR vAngMid( vAng1.size()) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i)
vAngMid[i] = ( vAxRotPrec[i] + vAng1[i]) / 2 ;
int nLmidStat ;
double dXmid, dYmid, dZmid ;
bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, dAngAmid, dAngBmid, nLmidStat, dXmid, dYmid, dZmid) ;
bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, vAngMid, nLmidStat, dXmid, dYmid, dZmid) ;
if ( ! bLmidOk || nLmidStat != 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
@@ -680,10 +683,11 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
vtCurr.Rotate( Z_AX, dAngCenStep) ;
Point3d ptCurr = ptCen + vtCurr ;
double dCoeff = i / NUM_VERIF_STEP ;
double dAngA = dAngAprec * ( 1 - dCoeff) + dAngA1 * dCoeff ;
double dAngB = dAngBprec * ( 1 - dCoeff) + dAngB1 * dCoeff ;
DBLVECTOR vAng( vAng1.size()) ;
for ( size_t i = 0 ; i < vAng1.size() ; ++ i)
vAng[i] = vAxRotPrec[i] * ( 1 - dCoeff) + vAng1[i] * dCoeff ;
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, dAngA, dAngB, nStat) ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( ptCurr.x, ptCurr.y, ptCurr.z, vAng[0], vAng[1], nStat) ;
if ( ! bOsOk || nStat != 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
@@ -700,8 +704,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
dYprec = dY ;
dZprec = dZ ;
// memorizzo i valori degli angoli come nuovi precedenti
dAngAprec = dAngA1 ;
dAngBprec = dAngB1 ;
vAxRotPrec = vAng1 ;
}
return bOk ;
@@ -892,6 +895,8 @@ Operation::AddRise( DBLVECTOR& vAxVal, double dDelta)
return false ;
// recupero i valori degli assi
vAxVal = pCam->GetAxesVal() ;
if ( vAxVal.size() < 3)
return false ;
// flag per tipo di movimento in rapido
int nFlag = 0 ;
// se delta positivo lo uso come incremento di Z
Operation.h
+1 -1
View File
@@ -67,7 +67,7 @@ class Operation : public IUserObj
bool GetClPathFinalAxesValues( int nClPathId, DBLVECTOR& vAxVal) ;
bool CalculateAxesValues( void) ;
bool CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
double& dAngAprec, double& dAngBprec, int& nOutStrC) ;
DBLVECTOR& vAxRotPrec, int& nOutStrC) ;
bool AdjustStartEndMovements( void) ;
bool AdjustOneStartMovement( int nClPathId, const DBLVECTOR& vAxPrev) ;
bool ToolChangeNeeded( const Operation& Op1, const Operation& Op2) ;
OutputConst.h
+6
View File
@@ -59,6 +59,8 @@ static const std::string GVAR_L2 = ".L2" ; // (num) valore del
static const std::string GVAR_L3 = ".L3" ; // (num) valore del terzo asse lineare
static const std::string GVAR_R1 = ".R1" ; // (num) valore del primo asse rotante
static const std::string GVAR_R2 = ".R2" ; // (num) valore del secondo asse rotante
static const std::string GVAR_R3 = ".R3" ; // (num) valore del terzo asse rotante
static const std::string GVAR_R4 = ".R4" ; // (num) valore del quartoo asse rotante
static const std::string GVAR_C1 = ".C1" ; // (num) valore del primo asse lineare per centro arco
static const std::string GVAR_C2 = ".C2" ; // (num) valore del secondo asse lineare per centro arco
static const std::string GVAR_C3 = ".C3" ; // (num) valore del terzo asse lineare per centro arco
@@ -69,11 +71,15 @@ static const std::string GVAR_L2P = ".L2p" ; // (num) valore pre
static const std::string GVAR_L3P = ".L3p" ; // (num) valore precedente del terzo asse lineare
static const std::string GVAR_R1P = ".R1p" ; // (num) valore precedente del primo asse rotante
static const std::string GVAR_R2P = ".R2p" ; // (num) valore precedente del secondo asse rotante
static const std::string GVAR_R3P = ".R3p" ; // (num) valore precedente del terzo asse rotante
static const std::string GVAR_R4P = ".R4p" ; // (num) valore precedente del quarto asse rotante
static const std::string GVAR_L1T = ".L1t" ; // (num) token del primo asse lineare
static const std::string GVAR_L2T = ".L2t" ; // (num) token del secondo asse lineare
static const std::string GVAR_L3T = ".L3t" ; // (num) token del terzo asse lineare
static const std::string GVAR_R1T = ".R1t" ; // (num) token del primo asse rotante
static const std::string GVAR_R2T = ".R2t" ; // (num) token del secondo asse rotante
static const std::string GVAR_R3T = ".R3t" ; // (num) token del terzo asse rotante
static const std::string GVAR_R4T = ".R4t" ; // (num) token del quarto asse rotante
static const std::string GVAR_C1T = ".C1t" ; // (num) token del primo asse lineare per centro arco
static const std::string GVAR_C2T = ".C2t" ; // (num) token del secondo asse lineare per centro arco
static const std::string GVAR_C3T = ".C3t" ; // (num) token del terzo asse lineare per centro arco
Simulator.cpp
+9 -3
View File
@@ -45,7 +45,7 @@ Simulator::Simulator( void)
//----------------------------------------------------------------------------
Simulator::~Simulator( void)
{
Stop() ;
}
//----------------------------------------------------------------------------
@@ -215,6 +215,8 @@ Simulator::Move( int& nStatus)
nStatus = MCH_SIM_ERR ;
return false ;
}
// recupero eventuali forzature assi da gestione inizio disposizione
UpdateAxes() ;
break ;
}
// altrimenti disposizione passiva
@@ -496,8 +498,12 @@ Simulator::UpdateAxes( void)
if ( ! pMachine->GetAllCurrAxesName( m_AxesName))
return false ;
// Aggiorno la posizione corrente degli assi macchina attivi
for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i)
m_pMachine->GetAxisPos( m_AxesName[i], m_AxesVal[i]) ;
m_AxesVal.clear() ;
for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i) {
double dVal ;
m_pMachine->GetAxisPos( m_AxesName[i], dVal) ;
m_AxesVal.emplace_back( dVal) ;
}
return true ;
}