EgtMachKernel :

- in GetRobotAngles aggiunti controlli dei limiti di corsa.
2025-08-13 18:09:06 +02:00
parent e92a37aa4f
commit dcbf9a497a
1 changed files with 18 additions and 0 deletions
@@ -2300,5 +2300,23 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
   vAng1.push_back( dAng6) ;
   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 ;
+      }
+   }
+   for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
+      if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng1[i])) {
+         vAng1 = vAng2 ;
+         break ;
+      }
+   }
+   if ( vAng1.empty()) {
+      LOG_ERROR( GetEMkLogger(), "Error : angles out stroke (robot)")
+      return false ;      
+   }
+
   return true ;
 }