Adding goniometer motors initialization/control

src/main/java/Aberration_Calibration.java

@@ -817,6 +817,7 @@ if (MORE_BUTTONS) {
 		panelGoniometer = new Panel();
 		panelGoniometer.setLayout(new GridLayout(1, 0, 5, 5));
 		addButton("Configure Goniometer",panelGoniometer,color_configure); 
+		addButton("Goniometer Move",panelGoniometer,color_debug);
 		addButton("Goniometer Scan",panelGoniometer,color_conf_process);
 		addButton("Filter Grids",panelGoniometer,color_bundle);
 		addButton("Update Image Set",panelGoniometer);
@@ -5806,6 +5807,37 @@ if (MORE_BUTTONS) {
 			return;
 		}
 /* ======================================================================== */
+		if       (label.equals("Goniometer Move")) {
+			DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
+			
+			CAMERAS.setNumberOfThreads(THREADS_MAX);
+			CAMERAS.debugLevel=DEBUG_LEVEL;
+
+			if (GONIOMETER==null) {
+				GONIOMETER= new Goniometer(
+						CAMERAS, // CalibrationHardwareInterface.CamerasInterface cameras,
+						DISTORTION, //MatchSimulatedPattern.DistortionParameters distortion,
+						PATTERN_DETECT, //MatchSimulatedPattern.PatternDetectParameters patternDetectParameters,
+						EYESIS_CAMERA_PARAMETERS, //Distortions.EyesisCameraParameters eyesisCameraParameters,
+						LASER_POINTERS, // MatchSimulatedPattern.LaserPointer laserPointers
+						SIMUL,                       //SimulationPattern.SimulParameters  simulParametersDefault,
+						GONIOMETER_PARAMETERS, //LensAdjustment.FocusMeasurementParameters focusMeasurementParameters,
+						DISTORTION_PROCESS_CONFIGURATION
+				);
+				if (DEBUG_LEVEL>1){
+					System.out.println("Initiaslizing Goniometer class");
+				}
+			} else if (DEBUG_LEVEL>1){
+				System.out.println("GONIOMETER was initialized");
+			}
+
+			while (GONIOMETER.manualMove(
+					this.SYNC_COMMAND.stopRequested,
+					UPDATE_STATUS));
+
+			return;
+		}
+/* ======================================================================== */
 		if       (label.equals("Goniometer Scan")) {
 			DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
 			

src/main/java/CalibrationHardwareInterface.java

@@ -2532,7 +2532,44 @@ public class CalibrationHardwareInterface {
         private long nanoReferenceTime; // last time the position was checked
         private int []referencePosition=null;
         private double motorsStuckTestTime=5.0; // seconds
+        private boolean motorsInitialized=false;
+        public int tiltMotor=2;  // 0-1-2
+        public int axialMotor=1; // 0-1-2
+        
 
+        /**
+         * Tries to initialize motors if needed. Assumes that non-initialized motors were not tried to be moved 
+         * @param force unconditionally try to initialize
+         * @return
+         */
+        public boolean tryInit(
+        		boolean force,
+        		boolean updateStatus){
+        	int delta=4;
+        	if (!force) {
+        		if (motorsInitialized) return true;
+        		updateMotorsPosition();
+        		if ((Math.abs(curpos[tiltMotor])>2) || (Math.abs(curpos[axialMotor])>2)){
+        			motorsInitialized=true;
+        			return true;
+        		}
+        		System.out.println("Trying to move axial motor to see if it is initialized ...");
+        		motorsInitialized=true; // to avoid loop
+        		int newpos=curpos[axialMotor]+delta;
+        		if (moveMotorSetETA(axialMotor, newpos)) {
+            		if (waitMotor(axialMotor, null, true, updateStatus)) {// no interaction
+            			moveMotorSetETA(axialMotor, newpos-delta); // restore original
+                		if (waitMotor(axialMotor, null, true, updateStatus)) {// no interaction
+                			return true; // should be always true here 
+                		}
+            		}
+        		}
+        		motorsInitialized=false;
+        	}
+        	initMotors();
+        	motorsInitialized=true;
+        	return true;
+        }
 // TODO: Enable simultaneous motors        
         
         public boolean moveMotorSetETA(int motorNumber, int position){
@@ -2550,6 +2587,17 @@ public class CalibrationHardwareInterface {
         		return false;
         		
         	}
+        	if (!motorsInitialized) { // may be called from tryInit(), but with motorsInitialized set to true;
+        		if (!tryInit(false, true)){ // do not force, update status
+            		String msg="Motors failed to initialize";
+                    		System.out.println("Error: "+msg);
+                    		IJ.showMessage("Error",msg);
+                    		//throw new RuntimeException(msg);
+                    		return false;
+        		}
+        	}
+        	// TODO: check init is needed
+        	
         	updateMotorsPosition();
         	this.targetPosition[motorNumber]=position;
         	this.nanoReferenceTime=System.nanoTime();
@@ -2559,7 +2607,12 @@ public class CalibrationHardwareInterface {
 			return true;
         }
 
-        public boolean waitMotor(int motorNumber){
+        public boolean waitMotor(
+        		int motorNumber,
+        		AtomicInteger stopRequested, // or null
+        		boolean quiet,
+        		boolean updateStatus
+        		){
         	if ((motorNumber<0) ||(motorNumber>=this.motorsRange.length) || (this.motorsRange[motorNumber]==null)){
         		String msg="Motor "+motorNumber+" is undefined";
         		IJ.showMessage("Error",msg);
@@ -2567,7 +2620,11 @@ public class CalibrationHardwareInterface {
         	}
         	while (true) {
     			enableMotors(true); // just in case?
-        		updateMotorsPosition(1); // wait one second before testing to decrease re-test frequency 
+        		updateMotorsPosition(1); // wait one second before testing to decrease re-test frequency
+        		if (updateStatus) {
+        			IJ.showStatus("Goniometer: tilt="+curpos[tiltMotor]+" ("+this.targetPosition[tiltMotor]+") "+
+        		", axial="+curpos[axialMotor]+" ("+this.targetPosition[axialMotor]+")");
+        		}
         		int positionError= Math.abs(this.targetPosition[motorNumber]-this.curpos[motorNumber]);
         		if (positionError<this.motorTolerance){
         			updateMotorsPosition(1); // re-test
@@ -2576,13 +2633,29 @@ public class CalibrationHardwareInterface {
         			return true;
         		}
         		long nanoNow=System.nanoTime();
+        		if ((stopRequested!=null) && (stopRequested.get()>1)){
+        			enableMotors(false);
+					if (this.debugLevel>0) System.out.println("User interrupt");
+					stopRequested.set(0);
+					updateMotorsPosition(1);
+        			String msg="Goniometer: tilt="+curpos[tiltMotor]+" ("+this.targetPosition[tiltMotor]+") "+
+        	        		", axial="+curpos[axialMotor]+" ("+this.targetPosition[axialMotor]+")\n"+
+        					"OK to continue, Cancel to abort movement";
+        			if (!IJ.showMessageWithCancel("Goniometer interrupted", msg)) {
+        				return false;
+        			}
+        			enableMotors(true);
+        			this.nanoETA += System.nanoTime()-nanoNow;
+        		}
         		if (nanoNow>this.nanoETA) {
         			enableMotors(false);
+        			if (quiet) return false;
         			String msg="Motor "+motorNumber+" failed to reach destination "+this.targetPosition[motorNumber]+
         			", current position is "+this.curpos[motorNumber]+
         			"\nYou may try to manually fix the problem before hitting OK";
         			System.out.println ("Error:"+msg);
         			IJ.showMessage("Error",msg);
+//        			if (IJ.showMessageWithCancel("Goniometer did not move", msg+"\n OK will try to initialize ");
 // Give chance to manually fix the problem        			
         			updateMotorsPosition();
         			positionError= Math.abs(this.targetPosition[motorNumber]-this.curpos[motorNumber]);
@@ -2596,6 +2669,7 @@ public class CalibrationHardwareInterface {
         		if ((nanoNow-this.nanoReferenceTime)> ((long) (this.motorsStuckTestTime*1E9))){
         			if(Math.abs(this.referencePosition[motorNumber]-this.curpos[motorNumber]) < motorStuckTolerance){
             			enableMotors(false);
+            			if (quiet) return false;
             			String msg="Motor "+motorNumber+" is stuck at "+this.curpos[motorNumber]+". "+
             			this.motorsStuckTestTime+" seconds ago it was at "+this.referencePosition[motorNumber]+
             			", target position is "+this.targetPosition[motorNumber]+
@@ -2619,13 +2693,23 @@ public class CalibrationHardwareInterface {
         }
 
 // first check tolerance, then - if motor is stuck        
-        
+        public int [] getCurrentPositions(){
+        	updateMotorsPosition();
+        	return this.curpos;
+        }
+
         public int [] getTargetPositions(){
         	return this.targetPosition;
         }
         public int[] enableMotors(boolean enable)  {
         	return commandElphel10364Motors("http://"+this.ipAddress+"/10364.php?"+(enable?"enable":"disable"));
         }
+        public int[] initMotors()  {
+        	return commandElphel10364Motors("http://"+this.ipAddress+"/10364.php?init");
+        }
+        public int[] setHome()  {
+        	return commandElphel10364Motors("http://"+this.ipAddress+"/10364.php?m1=0&m2=0&m3=0&reset");
+        }
         
         public int[] updateMotorsPosition()  {
         	return commandElphel10364Motors("http://"+this.ipAddress+"/10364.php");

src/main/java/Distortions.java

@@ -3851,9 +3851,9 @@ List calibration
 		for (int i=0;i<imageSets.length;i++) availableSets[i]= !allNaNInSet[i]; //!Double.isNaN(rmsPerSet[i]);
 		if (removeEmptySets  && (numNaN>0)){ //(this.debugLevel>0)
 			if (this.debugLevel>0) System.out.println("removeOutLayerSets(): Number of empty (rms=NaN) sets="+numNaN+":");
-			int n=0;
+//			int n=0;
 			for (int setNum=0;setNum<imageSets.length;setNum++) if (!availableSets[setNum]){
-				n++;
+//				n++;
 				if (this.debugLevel>0) System.out.println("Set "+setNum);
 	    		for (int imgInSet=0;imgInSet<imageSets[setNum].length;imgInSet++){
 					int numImg=imageSets[setNum][imgInSet];

src/main/java/Goniometer.java

@@ -94,7 +94,122 @@ horizontal axis:
 	}
 
 	//goniometerMotors
+	private enum MOT_ACT {
+		MOVE_SPEC,
+		HOME,
+		TILT_P85,
+		TILT_M85,
+		RCW200,
+		RCCW200,
+		SET_HOME};
+	private final String [] options={
+			"Move to specified position",
+			"Move home",
+			"Tilt to target 85 degrees",
+			"Tilt from target 85 degrees",
+			"Rotate to clockwise 200 degrees",
+			"Rotate to counter-clockwise 200 degrees",
+			"set current position as new home"};
 	
+
+	public boolean manualMove(
+			AtomicInteger stopRequested, // 1 - stop now, 2 - when convenient
+			boolean updateStatus){
+		int tiltMotor=2;  // 0-1-2
+		int axialMotor=1; // 0-1-2
+		boolean needsInit=false;
+		
+		int [] currentMotors=this.goniometerParameters.goniometerMotors.getCurrentPositions();
+		double currentTilt=currentMotors[tiltMotor]/this.goniometerParameters.goniometerMotors.stepsPerDegreeTilt;
+		double currentAxial=currentMotors[axialMotor]/this.goniometerParameters.goniometerMotors.stepsPerDegreeAxial;
+		currentTilt=0.1*Math.round(10.0*currentTilt);
+		currentAxial=0.1*Math.round(10.0*currentAxial);
+		this.goniometerParameters.goniometerMotors.debugLevel=this.debugLevel+1;
+		System.out.println(
+				"Current position:\n"+
+				"Tilt:  "+IJ.d2s(currentTilt,1)+" degrees ("+currentMotors[tiltMotor]+" steps)\n"+
+				"Axial: "+IJ.d2s(currentAxial,1)+" degrees ("+currentMotors[axialMotor]+" steps)\n");
+		
+		GenericDialog gd = new GenericDialog("User interrupt");
+
+		gd.addRadioButtonGroup("Select action", options, 7, 1, options[0]);
+		gd.addNumericField("Goniometer tilt angle",    currentTilt, 1,5,"degrees");
+		gd.addNumericField("Goniometer axial angle",    currentAxial, 1,5,"degrees");
+		gd.addCheckbox("Initialize goniometer motor driver",needsInit);
+		gd.showDialog();
+		if (gd.wasCanceled()) return false;
+		String selectedAction=gd.getNextRadioButton();
+		currentTilt=  gd.getNextNumber();
+		currentAxial= gd.getNextNumber();
+		needsInit=    gd.getNextBoolean();
+		if (needsInit){
+			if (!this.goniometerParameters.goniometerMotors.tryInit(true,updateStatus)){
+				String msg="Failed to initialize goniometer motor driver";
+				System.out.println("Error: "+msg);
+				IJ.showMessage("Error",msg);
+				return false; // failed initialization
+			}
+		}
+		if (selectedAction.equals(options[MOT_ACT.SET_HOME.ordinal()])){
+			if (this.goniometerParameters.goniometerMotors.setHome()==null){
+				String msg="Failed to set new home position";
+				System.out.println("Error: "+msg);
+				IJ.showMessage("Error",msg);
+				return false; // failed set home
+			}
+			return true;
+		}
+		MOT_ACT act=MOT_ACT.MOVE_SPEC;
+		for (MOT_ACT a: MOT_ACT.values()){
+			if (selectedAction.equals(options[a.ordinal()])){
+				act=a;
+				break;
+			}
+		}
+		double targetTilt=currentTilt;
+		double targetAxial=currentAxial;
+		switch (act) {
+		case HOME:
+			targetTilt=0;
+			targetAxial=0;
+			act=MOT_ACT.MOVE_SPEC;
+			break;
+		case TILT_P85:
+			targetTilt=85.0;
+			act=MOT_ACT.MOVE_SPEC;
+			break;
+		case TILT_M85:
+			targetTilt=-85.0;
+			act=MOT_ACT.MOVE_SPEC;
+			break;
+		case RCW200:
+			targetAxial=200.0;
+			act=MOT_ACT.MOVE_SPEC;
+			break;
+		case RCCW200:
+			targetAxial=-200.0;
+			act=MOT_ACT.MOVE_SPEC;
+			break;
+		default:
+			break;
+		}
+		if (act!=MOT_ACT.MOVE_SPEC){
+			String msg="Program BUG, action "+selectedAction+" can not be processed";
+			System.out.println("Error: "+msg);
+			IJ.showMessage("Error",msg);
+			return false;
+		}
+		int tiltMotorPosition= (int) Math.round(targetTilt*this.goniometerParameters.goniometerMotors.stepsPerDegreeTilt);
+		int axialMotorPosition= (int) Math.round(targetAxial*this.goniometerParameters.goniometerMotors.stepsPerDegreeAxial);
+		boolean OK= motorsMove(
+				tiltMotor,
+				axialMotor,
+				tiltMotorPosition,
+				axialMotorPosition,
+				stopRequested,
+				updateStatus);
+		return OK;
+	}
 	public boolean scanAndAcquire(
 			double targetAngleHorizontal,
 			double targetAngleVertical,
@@ -270,7 +385,7 @@ horizontal axis:
 				IJ.showMessage("Error",msg);
 				return false;
 			}
-			OK=this.goniometerParameters.goniometerMotors.waitMotor(tiltMotor);
+			OK=this.goniometerParameters.goniometerMotors.waitMotor(tiltMotor, null, false,updateStatus); // no interrupts during movement
 			if (!OK) {
 				String msg="Motor "+(tiltMotor+1)+" failed to reach "+tiltMotorPosition+".";
 				System.out.println("Error: "+msg);
@@ -297,7 +412,7 @@ horizontal axis:
 					IJ.showMessage("Error",msg);
 					return false;
 				}
-				OK=this.goniometerParameters.goniometerMotors.waitMotor(axialMotor);
+				OK=this.goniometerParameters.goniometerMotors.waitMotor(axialMotor, null, false,updateStatus);
 				if (!OK) {
 					String msg="Motor "+(axialMotor+1)+" failed to reach "+axialMotorPosition+".";
 					System.out.println("Error: "+msg);
@@ -328,6 +443,7 @@ horizontal axis:
 								axialMotor,
 								0, //tiltMotorPosition,
 								0, //axialMotorPosition,
+								null, // no interrupts
 								updateStatus);
 						return false;
 					}
@@ -341,6 +457,7 @@ horizontal axis:
 				axialMotor,
 				0, //tiltMotorPosition,
 				0, //axialMotorPosition,
+				null, // no interrupts
 				updateStatus);
 		if (this.debugLevel>0) System.out.println("Scan finished in "+IJ.d2s(1E-9*(System.nanoTime()-startTime),3)+" seconds.");
 
@@ -352,8 +469,9 @@ horizontal axis:
 			int axialMotor,
 			int tiltMotorPosition,
 			int axialMotorPosition,
+    		AtomicInteger stopRequested, // or null
 			boolean updateStatus){
-		String status="Movin axial motor to "+axialMotorPosition+"...";
+		String status="Moving axial motor to "+axialMotorPosition+"...";
 			if (updateStatus) IJ.showStatus(status);
 			boolean OK= this.goniometerParameters.goniometerMotors.moveMotorSetETA(axialMotor, axialMotorPosition);
 			if (!OK) {
@@ -362,7 +480,7 @@ horizontal axis:
 				IJ.showMessage("Error",msg);
 				return false;
 			}
-			OK=this.goniometerParameters.goniometerMotors.waitMotor(axialMotor);
+			OK=this.goniometerParameters.goniometerMotors.waitMotor(axialMotor, stopRequested, false,updateStatus);
 			if (!OK) {
 				String msg="Motor "+(axialMotor+1)+" failed to reach "+axialMotorPosition+".";
 				System.out.println("Error: "+msg);
@@ -376,7 +494,7 @@ horizontal axis:
 				IJ.showMessage("Error",msg);
 				return false;
 			}
-			OK=this.goniometerParameters.goniometerMotors.waitMotor(tiltMotor);
+			OK=this.goniometerParameters.goniometerMotors.waitMotor(tiltMotor,stopRequested, false, updateStatus);
 			if (!OK) {
 				String msg="Motor "+(tiltMotor+1)+" failed to reach "+tiltMotorPosition+".";
 				System.out.println("Error: "+msg);