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
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 ;