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
MachMgr.h
+2 -2
View File
@@ -354,9 +354,9 @@ class MachMgr : public IMachMgr
bool GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const override ;
bool GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB,
int& nStat, double& dX, double& dY, double& dZ) const override ;
double& dX, double& dY, double& dZ) const override ;
bool GetCalcPositions( const Point3d& ptP, const DBLVECTOR& vAng,
int& nStat, double& dX, double& dY, double& dZ) const override ;
double& dX, double& dY, double& dZ) const override ;
bool GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vector3d& vtDirA,
DBLVECTOR& vAng1, DBLVECTOR& vAng2) const override ;
bool GetCalcTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB,
MachMgrMachines.cpp
+4 -4
View File
@@ -780,20 +780,20 @@ MachMgr::GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
//----------------------------------------------------------------------------
bool
MachMgr::GetCalcPositions( const Point3d& ptP, double dAngA, double dAngB,
int& nStat, double& dX, double& dY, double& dZ) const
double& dX, double& dY, double& dZ) const
{
DBLVECTOR vAng( 2) ; vAng[0] = dAngA ; vAng[1] = dAngB ;
Machine* pMch = GetCurrMachine() ;
return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, nStat, dX, dY, dZ) : false) ;
return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, dX, dY, dZ) : false) ;
}
//----------------------------------------------------------------------------
bool
MachMgr::GetCalcPositions( const Point3d& ptP, const DBLVECTOR& vAng,
int& nStat, double& dX, double& dY, double& dZ) const
double& dX, double& dY, double& dZ) const
{
Machine* pMch = GetCurrMachine() ;
return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, nStat, dX, dY, dZ) : false) ;
return ( ( pMch != nullptr) ? pMch->GetPositions( ptP, vAng, dX, dY, dZ) : false) ;
}
//----------------------------------------------------------------------------
Machine.h
+1 -1
View File
@@ -171,7 +171,7 @@ class Machine
bool GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const ;
bool GetPositions( const Point3d& ptP, const DBLVECTOR& vAng,
int& nStat, double& dX, double& dY, double& dZ) const ;
double& dX, double& dY, double& dZ) const ;
bool GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vector3d& vtDirA,
DBLVECTOR& vAng1, DBLVECTOR& vAng2) const ;
bool GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng,
MachineCalc.cpp
+40 -16
View File
@@ -1294,7 +1294,7 @@ Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
//----------------------------------------------------------------------------
bool
Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng,
int& nStat, double& dX, double& dY, double& dZ) const
double& dX, double& dY, double& dZ) const
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// il punto è dato rispetto alla posizione home della tavola
@@ -1355,7 +1355,6 @@ Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng,
dZ = ptPos.z ;
// tutto ok
nStat = 0 ;
return true ;
}
@@ -1701,8 +1700,19 @@ Machine::AdjustAngleInStroke( const STROKE& Stroke, double& dAng) const
dAng += ANG_FULL ;
while ( dAng > Stroke.Max)
dAng -= ANG_FULL ;
// verifico
return ( dAng >= Stroke.Min && dAng <= Stroke.Max) ;
// se nei limiti, tutto bene
if ( dAng >= Stroke.Min && dAng <= Stroke.Max)
return true ;
// altrimenti, determino il valore con il minimo outstroke
if ( dAng < Stroke.Min) {
if ( abs( dAng - Stroke.Min) > abs( dAng + ANG_FULL - Stroke.Max))
dAng += ANG_FULL ;
}
else {
if ( abs( dAng - Stroke.Max) > abs( dAng - ANG_FULL - Stroke.Min))
dAng -= ANG_FULL ;
}
return false ;
}
// altrimenti asse rotante senza corsa
else {
@@ -2265,7 +2275,14 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
double dCosB = ( dDistR2R3 * dDistR2R3 + dDistR2Cen * dDistR2Cen - dDistR3R5 * dDistR3R5) / ( 2 * dDistR2R3 * dDistR2Cen) ;
double dCosC = ( dDistR3R5 * dDistR3R5 + dDistR2Cen * dDistR2Cen - dDistR2R3 * dDistR2R3) / ( 2 * dDistR3R5 * dDistR2Cen) ;
if ( abs( dCosB) > 1 || abs( dCosC) > 1) {
LOG_ERROR( GetEMkLogger(), "Error : position unreachable (robot)")
double dDeltaOut = dDistR2Cen - ( dDistR2R3 + dDistR3R5) ;
double dDeltaIn = dDistR2Cen - abs( dDistR2R3 - dDistR3R5) ;
string sErr = "Error : position for the robot unreachable" ;
if ( dDeltaOut > 0)
sErr += " (Out=" + ToString( dDeltaOut, 3) + ")" ;
else if ( dDeltaIn < 0)
sErr += " (In=" + ToString( dDeltaIn, 3) + ")" ;
LOG_ERROR( GetEMkLogger(), sErr.c_str())
return false ;
}
double dAngB = acos( dCosB) * RADTODEG ;
@@ -2325,21 +2342,28 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
vAng2.push_back( dAng6 + ( dAng6 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT)) ;
// verifiche dei limiti di corsa
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) {
if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng2[i])) {
vAng2.clear() ;
break ;
}
}
bool bAng1 = true, bAng2 = true ;
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng1[i])) {
if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng1[i]))
bAng1 = false ;
if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng2[i]))
bAng2 = false ;
}
if ( bAng2) {
if ( ! bAng1) {
vAng1 = vAng2 ;
break ;
vAng2.clear() ;
swap( bAng1, bAng2) ;
}
}
if ( vAng1.empty()) {
LOG_ERROR( GetEMkLogger(), "Error : angles out stroke (robot)")
return false ;
else {
vAng2.clear() ;
}
if ( ! bAng1) {
int nStat ;
VerifyOutstroke( 0, 0, 0, vAng1, true, nStat) ;
LOG_ERROR( GetEMkLogger(), GetOutstrokeInfo().c_str())
return false ;
}
return true ;
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 ;