Untitled

public Frame screwWithCamera(String S_prevScrew, String S_currentScrew, String S_currentScrewAbove, String S_photoPos){
		ObjectFrame prevScrew = Data.getFrame(S_prevScrew);
		ObjectFrame currentScrew = Data.getFrame(S_currentScrew);
		ObjectFrame currentScrewAbove = Data.getFrame(S_currentScrewAbove);
		ObjectFrame photoPos = Data.getFrame(S_photoPos);
		
		Frame screwIn = new Frame(currentScrew.getX(), currentScrew.getY(), currentScrew.getZ(),
				  				  currentScrew.getAlphaRad(), currentScrew.getBetaRad(), currentScrew.getGammaRad() );
		Frame screwInAfter = new Frame(currentScrew.getX(), currentScrew.getY(), currentScrew.getZ() + 10,
				  currentScrew.getAlphaRad(), currentScrew.getBetaRad(), currentScrew.getGammaRad() );
		
		Spline spline1 = new Spline(
									lin(prevScrew).setCartVelocity(velocity),
									spl(photoPos).setCartVelocity(velocity)
									);
		
		Spline spline2 = new Spline(			  
								    spl(currentScrewAbove).setCartVelocity(velocity),
								    lin(screwIn).setCartVelocity(velocityReduced)
								    );
		
		lbr.move(spline1);
		io.setDO_requestPhoto(true);
		
		int timeOut = 4000; // milisec
		long timeStart = System.currentTimeMillis();
		
		
		while (true) {
			long timeEnd = System.currentTimeMillis();
			long timeDiff = timeEnd - timeStart;
			if ( timeDiff > timeOut ) {
				xOffset = 0;
				yOffset = 0;
				io.setDO_requestPhoto(false);
				System.out.println("Time expired: " + timeDiff + ". The value is over the limit. The offsets = 0.");
				break;
			}
			
			if (io.getDI_photoResult_NOK()){
				xOffset = 0;
				yOffset = 0;
				System.out.println("Time expired: " + timeDiff + ". Result not OK.");
				break;
			}
			
			if (io.getDI_photoResult_OK() || io.getDI_photoResult_NOK()){
				ThreadUtil.milliSleep(50);
				xOffset = - io.getDI_offset_x() * 1.0 / 10000.0;
				yOffset = - io.getDI_offset_y() * 1.0 / 10000.0;
				
				if (xOffset > 50){
					xOffset = 50;
				}
				
				if (xOffset < -50){
					xOffset = -50;
				}
				
				if (yOffset > 50){
					yOffset = 50;
				}
				
				if (yOffset < -50){
					yOffset = -50;
				}
				
				screwIn.setX(currentScrew.getX() + xOffset);
				screwIn.setY(currentScrew.getY() + yOffset);
				
				screwInAfter.setX(currentScrew.getX() + xOffset);
				screwInAfter.setY(currentScrew.getY() + yOffset);
				io.setDO_requestPhoto(false);
				break;
			}
		}
		
		lbr.move(spline2);
		
		return screwInAfter;
	}