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

EgtMachKernel :

Browse Source 
- da GetPositions di Machine eliminato parametro nStat inutile
- da GetCalcPositions di MachMgr eliminato lo stesso parametro
- nei robot ora si assegna sempre BackAuxDir
- migliorato calcolo angoli nei robot.
This commit is contained in:
Dario Sassi
2025-09-14 23:07:30 +02:00
parent ec021af82f
commit 2e5caf2def
5 changed files with 103 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Operation.cpp
+56 -25
View File
@@ -2005,6 +2005,8 @@ Operation::MyCalculateClPathMcentAxesValues( int nClPathId, double dAngDeltaMinF
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
return false ;
Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
if ( pMachine == nullptr)
return false ;
// predispongo variabile per valori assi
DBLVECTOR vAxVal ;
vAxVal.reserve( 8) ;
@@ -2048,10 +2050,9 @@ Operation::MyCalculateClPathMcentAxesValues( int nClPathId, double dAngDeltaMinF
Point3d ptP = pCamData->GetEndPoint() ;
AdjustEndPointForAxesCalc( pCamData, ptP) ;
// calcolo gli assi lineari della macchina
int nLStat ;
double dX, dY, dZ ;
bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAxRot, nLStat, dX, dY, dZ) ;
if ( ! bLOk || nLStat != 0) {
bool bLOk = m_pMchMgr->GetCalcPositions( ptP, vAxRot, dX, dY, dZ) ;
if ( ! bLOk) {
bOk = false ;
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
continue ;
@@ -2160,10 +2161,9 @@ Operation::MyCalculateClPathMcentAxesValues( int nClPathId, double dAngDeltaMinF
DBLVECTOR vAngMid( vAxRot.size()) ;
for ( size_t i = 0 ; i < vAxRot.size() ; ++ i)
vAngMid[i] = ( vAxRotPrec[i] + vAxRot[i]) / 2 ;
int nLmidStat ;
double dXmid, dYmid, dZmid ;
bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, vAngMid, nLmidStat, dXmid, dYmid, dZmid) ;
if ( ! bLmidOk || nLmidStat != 0) {
bool bLmidOk = m_pMchMgr->GetCalcPositions( ptMid, vAngMid, dXmid, dYmid, dZmid) ;
if ( ! bLmidOk) {
bOk = false ;
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
continue ;
@@ -2174,10 +2174,9 @@ Operation::MyCalculateClPathMcentAxesValues( int nClPathId, double dAngDeltaMinF
DBLVECTOR vAngEnd( vAxRot.size()) ;
for ( size_t i = 0 ; i < vAxRot.size() ; ++ i)
vAngEnd[i] = vAxRot[i] ;
int nLendStat ;
double dXend, dYend, dZend ;
bool bLendOk = m_pMchMgr->GetCalcPositions( ptEnd, vAngEnd, nLendStat, dXend, dYend, dZend) ;
if ( ! bLendOk || nLendStat != 0) {
bool bLendOk = m_pMchMgr->GetCalcPositions( ptEnd, vAngEnd, dXend, dYend, dZend) ;
if ( ! bLendOk) {
bOk = false ;
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
continue ;
@@ -2553,10 +2552,9 @@ Operation::VerifyMcentLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDir
for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i)
vAxMid[i] = vAxRotMid[i-3] ;
// determino il nuovo valore degli assi lineari sul punto medio
int nMidLStat ;
double dMidX, dMidY, dMidZ ;
bool bMidLOk = m_pMchMgr->GetCalcPositions( ptMid, vAxRotMid, nMidLStat, dMidX, dMidY, dMidZ) ;
if ( ! bMidLOk || nMidLStat != 0) {
bool bMidLOk = m_pMchMgr->GetCalcPositions( ptMid, vAxRotMid, dMidX, dMidY, dMidZ) ;
if ( ! bMidLOk) {
pMidCamData->SetAxes( CamData::AS_ERR, vAxMid) ;
bAxError = true ;
return true ;
@@ -2684,6 +2682,8 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, double dAngDeltaMinFor
}
else {
pCamData->SetAxes( CamData::AS_OK, vAxVal) ;
// l'asse ausiliario di testa riportato sul grezzo coincide con quello già impostato
pCamData->SetBackAuxDir( vtAux) ;
}
// tipo di movimento
int nMoveType = pCamData->GetMoveType() ;
@@ -2726,25 +2726,29 @@ Operation::CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vect
double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxVal)
{
// macchina
Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
if ( pMachine == nullptr)
return false ;
// continuità su assi rotanti nei movimenti successivi al primo per tutte le lavorazioni, tranne disposizioni
bool bRotContOnNext = ( GetType() != OPER_DISP) ;
// calcolo angoli dei giunti
DBLVECTOR vAng1, vAng2 ;
bool bRaOk = m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptP, vtTool, vtAux, vAng1, vAng2) ;
bool bRaOk = pMachine->GetRobotAngles( ptP, vtTool, vtAux, vAng1, vAng2) ;
if ( ! bRaOk)
return false ;
// se inizio
if ( bFirst) {
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
}
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) {
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
}
bFirst = false ;
}
@@ -2755,13 +2759,37 @@ Operation::CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vect
if ( bRotContOnNext)
vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ;
else
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
}
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) {
if ( bRotContOnNext)
vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ;
else
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
}
// verifico che le soluzioni siano nelle corse
bool bAng1 = true, bAng2 = true ;
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
if ( ! pMachine->VerifyAngleOutstroke( i, vAng1[i]))
bAng1 = false ;
if ( ! pMachine->VerifyAngleOutstroke( i, vAng2[i]))
bAng2 = false ;
}
if ( bAng2) {
if ( ! bAng1) {
vAng1 = vAng2 ;
vAng2.clear() ;
swap( bAng1, bAng2) ;
}
}
else {
vAng2.clear() ;
}
if ( ! bAng1) {
int nStat ;
pMachine->VerifyOutstroke( 0, 0, 0, vAng1, true, nStat) ;
LOG_ERROR( GetEMkLogger(), pMachine->GetOutstrokeInfo().c_str())
return false ;
}
}
// Scelgo la soluzione più vicina
@@ -2779,7 +2807,7 @@ Operation::CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vect
else
vAxVal = vAng2 ;
// controllo continuità di R6 dalla posizione precedente
m_pMchMgr->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ;
pMachine->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ;
return true ;
}
@@ -2789,10 +2817,14 @@ Operation::VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDir
const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal,
int nCnt, int nEntId, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError)
{
// macchina
Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
if ( pMachine == nullptr)
return false ;
// impostazioni
bAdded = false ;
bAxError = false ;
Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
// se disposizione non vanno fatti controlli
if ( GetType() == OPER_DISP)
@@ -2909,7 +2941,7 @@ Operation::VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDir
}
// verifico di essere nelle corse degli assi
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAxMid, false, nStat) ;
bool bOsOk = pMachine->VerifyOutstroke( 0, 0, 0, vAxMid, false, nStat) ;
if ( ! bOsOk || nStat != 0) {
pMidCamData->SetAxes( CamData::AS_OUTSTROKE, vAxMid) ;
bAxError = true ;
@@ -2917,9 +2949,8 @@ Operation::VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDir
}
// assegno i valori degli assi
pMidCamData->SetAxes( CamData::AS_OK, vAxMid) ;
// calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo
Vector3d vtBackAux ; pMachine->GetBackAuxDirFromAngles( vAxMid, vtBackAux) ;
pMidCamData->SetBackAuxDir( vtBackAux) ;
// l'asse ausiliario di testa riportato sul grezzo coincide con quello già impostato
pMidCamData->SetBackAuxDir( vtAuxMid) ;
// devo verificare le due nuove parti
++ nCnt ;