Commit 80f959fc authored by Andrey Filippov's avatar Andrey Filippov

Adding goniometer motors initialization/control

parent 40645fb8
...@@ -817,6 +817,7 @@ if (MORE_BUTTONS) { ...@@ -817,6 +817,7 @@ if (MORE_BUTTONS) {
panelGoniometer = new Panel(); panelGoniometer = new Panel();
panelGoniometer.setLayout(new GridLayout(1, 0, 5, 5)); panelGoniometer.setLayout(new GridLayout(1, 0, 5, 5));
addButton("Configure Goniometer",panelGoniometer,color_configure); addButton("Configure Goniometer",panelGoniometer,color_configure);
addButton("Goniometer Move",panelGoniometer,color_debug);
addButton("Goniometer Scan",panelGoniometer,color_conf_process); addButton("Goniometer Scan",panelGoniometer,color_conf_process);
addButton("Filter Grids",panelGoniometer,color_bundle); addButton("Filter Grids",panelGoniometer,color_bundle);
addButton("Update Image Set",panelGoniometer); addButton("Update Image Set",panelGoniometer);
...@@ -5806,6 +5807,37 @@ if (MORE_BUTTONS) { ...@@ -5806,6 +5807,37 @@ if (MORE_BUTTONS) {
return; 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")) { if (label.equals("Goniometer Scan")) {
DEBUG_LEVEL=MASTER_DEBUG_LEVEL; DEBUG_LEVEL=MASTER_DEBUG_LEVEL;
......
...@@ -2532,7 +2532,44 @@ public class CalibrationHardwareInterface { ...@@ -2532,7 +2532,44 @@ public class CalibrationHardwareInterface {
private long nanoReferenceTime; // last time the position was checked private long nanoReferenceTime; // last time the position was checked
private int []referencePosition=null; private int []referencePosition=null;
private double motorsStuckTestTime=5.0; // seconds 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 // TODO: Enable simultaneous motors
public boolean moveMotorSetETA(int motorNumber, int position){ public boolean moveMotorSetETA(int motorNumber, int position){
...@@ -2550,6 +2587,17 @@ public class CalibrationHardwareInterface { ...@@ -2550,6 +2587,17 @@ public class CalibrationHardwareInterface {
return false; 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(); updateMotorsPosition();
this.targetPosition[motorNumber]=position; this.targetPosition[motorNumber]=position;
this.nanoReferenceTime=System.nanoTime(); this.nanoReferenceTime=System.nanoTime();
...@@ -2559,7 +2607,12 @@ public class CalibrationHardwareInterface { ...@@ -2559,7 +2607,12 @@ public class CalibrationHardwareInterface {
return true; 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)){ if ((motorNumber<0) ||(motorNumber>=this.motorsRange.length) || (this.motorsRange[motorNumber]==null)){
String msg="Motor "+motorNumber+" is undefined"; String msg="Motor "+motorNumber+" is undefined";
IJ.showMessage("Error",msg); IJ.showMessage("Error",msg);
...@@ -2568,6 +2621,10 @@ public class CalibrationHardwareInterface { ...@@ -2568,6 +2621,10 @@ public class CalibrationHardwareInterface {
while (true) { while (true) {
enableMotors(true); // just in case? 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]); int positionError= Math.abs(this.targetPosition[motorNumber]-this.curpos[motorNumber]);
if (positionError<this.motorTolerance){ if (positionError<this.motorTolerance){
updateMotorsPosition(1); // re-test updateMotorsPosition(1); // re-test
...@@ -2576,13 +2633,29 @@ public class CalibrationHardwareInterface { ...@@ -2576,13 +2633,29 @@ public class CalibrationHardwareInterface {
return true; return true;
} }
long nanoNow=System.nanoTime(); 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) { if (nanoNow>this.nanoETA) {
enableMotors(false); enableMotors(false);
if (quiet) return false;
String msg="Motor "+motorNumber+" failed to reach destination "+this.targetPosition[motorNumber]+ String msg="Motor "+motorNumber+" failed to reach destination "+this.targetPosition[motorNumber]+
", current position is "+this.curpos[motorNumber]+ ", current position is "+this.curpos[motorNumber]+
"\nYou may try to manually fix the problem before hitting OK"; "\nYou may try to manually fix the problem before hitting OK";
System.out.println ("Error:"+msg); System.out.println ("Error:"+msg);
IJ.showMessage("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 // Give chance to manually fix the problem
updateMotorsPosition(); updateMotorsPosition();
positionError= Math.abs(this.targetPosition[motorNumber]-this.curpos[motorNumber]); positionError= Math.abs(this.targetPosition[motorNumber]-this.curpos[motorNumber]);
...@@ -2596,6 +2669,7 @@ public class CalibrationHardwareInterface { ...@@ -2596,6 +2669,7 @@ public class CalibrationHardwareInterface {
if ((nanoNow-this.nanoReferenceTime)> ((long) (this.motorsStuckTestTime*1E9))){ if ((nanoNow-this.nanoReferenceTime)> ((long) (this.motorsStuckTestTime*1E9))){
if(Math.abs(this.referencePosition[motorNumber]-this.curpos[motorNumber]) < motorStuckTolerance){ if(Math.abs(this.referencePosition[motorNumber]-this.curpos[motorNumber]) < motorStuckTolerance){
enableMotors(false); enableMotors(false);
if (quiet) return false;
String msg="Motor "+motorNumber+" is stuck at "+this.curpos[motorNumber]+". "+ String msg="Motor "+motorNumber+" is stuck at "+this.curpos[motorNumber]+". "+
this.motorsStuckTestTime+" seconds ago it was at "+this.referencePosition[motorNumber]+ this.motorsStuckTestTime+" seconds ago it was at "+this.referencePosition[motorNumber]+
", target position is "+this.targetPosition[motorNumber]+ ", target position is "+this.targetPosition[motorNumber]+
...@@ -2619,6 +2693,10 @@ public class CalibrationHardwareInterface { ...@@ -2619,6 +2693,10 @@ public class CalibrationHardwareInterface {
} }
// first check tolerance, then - if motor is stuck // first check tolerance, then - if motor is stuck
public int [] getCurrentPositions(){
updateMotorsPosition();
return this.curpos;
}
public int [] getTargetPositions(){ public int [] getTargetPositions(){
return this.targetPosition; return this.targetPosition;
...@@ -2626,6 +2704,12 @@ public class CalibrationHardwareInterface { ...@@ -2626,6 +2704,12 @@ public class CalibrationHardwareInterface {
public int[] enableMotors(boolean enable) { public int[] enableMotors(boolean enable) {
return commandElphel10364Motors("http://"+this.ipAddress+"/10364.php?"+(enable?"enable":"disable")); 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() { public int[] updateMotorsPosition() {
return commandElphel10364Motors("http://"+this.ipAddress+"/10364.php"); return commandElphel10364Motors("http://"+this.ipAddress+"/10364.php");
......
...@@ -3851,9 +3851,9 @@ List calibration ...@@ -3851,9 +3851,9 @@ List calibration
for (int i=0;i<imageSets.length;i++) availableSets[i]= !allNaNInSet[i]; //!Double.isNaN(rmsPerSet[i]); for (int i=0;i<imageSets.length;i++) availableSets[i]= !allNaNInSet[i]; //!Double.isNaN(rmsPerSet[i]);
if (removeEmptySets && (numNaN>0)){ //(this.debugLevel>0) if (removeEmptySets && (numNaN>0)){ //(this.debugLevel>0)
if (this.debugLevel>0) System.out.println("removeOutLayerSets(): Number of empty (rms=NaN) sets="+numNaN+":"); 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]){ for (int setNum=0;setNum<imageSets.length;setNum++) if (!availableSets[setNum]){
n++; // n++;
if (this.debugLevel>0) System.out.println("Set "+setNum); if (this.debugLevel>0) System.out.println("Set "+setNum);
for (int imgInSet=0;imgInSet<imageSets[setNum].length;imgInSet++){ for (int imgInSet=0;imgInSet<imageSets[setNum].length;imgInSet++){
int numImg=imageSets[setNum][imgInSet]; int numImg=imageSets[setNum][imgInSet];
...@@ -94,7 +94,122 @@ horizontal axis: ...@@ -94,7 +94,122 @@ horizontal axis:
} }
//goniometerMotors //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( public boolean scanAndAcquire(
double targetAngleHorizontal, double targetAngleHorizontal,
double targetAngleVertical, double targetAngleVertical,
...@@ -270,7 +385,7 @@ horizontal axis: ...@@ -270,7 +385,7 @@ horizontal axis:
IJ.showMessage("Error",msg); IJ.showMessage("Error",msg);
return false; return false;
} }
OK=this.goniometerParameters.goniometerMotors.waitMotor(tiltMotor); OK=this.goniometerParameters.goniometerMotors.waitMotor(tiltMotor, null, false,updateStatus); // no interrupts during movement
if (!OK) { if (!OK) {
String msg="Motor "+(tiltMotor+1)+" failed to reach "+tiltMotorPosition+"."; String msg="Motor "+(tiltMotor+1)+" failed to reach "+tiltMotorPosition+".";
System.out.println("Error: "+msg); System.out.println("Error: "+msg);
...@@ -297,7 +412,7 @@ horizontal axis: ...@@ -297,7 +412,7 @@ horizontal axis:
IJ.showMessage("Error",msg); IJ.showMessage("Error",msg);
return false; return false;
} }
OK=this.goniometerParameters.goniometerMotors.waitMotor(axialMotor); OK=this.goniometerParameters.goniometerMotors.waitMotor(axialMotor, null, false,updateStatus);
if (!OK) { if (!OK) {
String msg="Motor "+(axialMotor+1)+" failed to reach "+axialMotorPosition+"."; String msg="Motor "+(axialMotor+1)+" failed to reach "+axialMotorPosition+".";
System.out.println("Error: "+msg); System.out.println("Error: "+msg);
...@@ -328,6 +443,7 @@ horizontal axis: ...@@ -328,6 +443,7 @@ horizontal axis:
axialMotor, axialMotor,
0, //tiltMotorPosition, 0, //tiltMotorPosition,
0, //axialMotorPosition, 0, //axialMotorPosition,
null, // no interrupts
updateStatus); updateStatus);
return false; return false;
} }
...@@ -341,6 +457,7 @@ horizontal axis: ...@@ -341,6 +457,7 @@ horizontal axis:
axialMotor, axialMotor,
0, //tiltMotorPosition, 0, //tiltMotorPosition,
0, //axialMotorPosition, 0, //axialMotorPosition,
null, // no interrupts
updateStatus); updateStatus);
if (this.debugLevel>0) System.out.println("Scan finished in "+IJ.d2s(1E-9*(System.nanoTime()-startTime),3)+" seconds."); 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: ...@@ -352,8 +469,9 @@ horizontal axis:
int axialMotor, int axialMotor,
int tiltMotorPosition, int tiltMotorPosition,
int axialMotorPosition, int axialMotorPosition,
AtomicInteger stopRequested, // or null
boolean updateStatus){ boolean updateStatus){
String status="Movin axial motor to "+axialMotorPosition+"..."; String status="Moving axial motor to "+axialMotorPosition+"...";
if (updateStatus) IJ.showStatus(status); if (updateStatus) IJ.showStatus(status);
boolean OK= this.goniometerParameters.goniometerMotors.moveMotorSetETA(axialMotor, axialMotorPosition); boolean OK= this.goniometerParameters.goniometerMotors.moveMotorSetETA(axialMotor, axialMotorPosition);
if (!OK) { if (!OK) {
...@@ -362,7 +480,7 @@ horizontal axis: ...@@ -362,7 +480,7 @@ horizontal axis:
IJ.showMessage("Error",msg); IJ.showMessage("Error",msg);
return false; return false;
} }
OK=this.goniometerParameters.goniometerMotors.waitMotor(axialMotor); OK=this.goniometerParameters.goniometerMotors.waitMotor(axialMotor, stopRequested, false,updateStatus);
if (!OK) { if (!OK) {
String msg="Motor "+(axialMotor+1)+" failed to reach "+axialMotorPosition+"."; String msg="Motor "+(axialMotor+1)+" failed to reach "+axialMotorPosition+".";
System.out.println("Error: "+msg); System.out.println("Error: "+msg);
...@@ -376,7 +494,7 @@ horizontal axis: ...@@ -376,7 +494,7 @@ horizontal axis:
IJ.showMessage("Error",msg); IJ.showMessage("Error",msg);
return false; return false;
} }
OK=this.goniometerParameters.goniometerMotors.waitMotor(tiltMotor); OK=this.goniometerParameters.goniometerMotors.waitMotor(tiltMotor,stopRequested, false, updateStatus);
if (!OK) { if (!OK) {
String msg="Motor "+(tiltMotor+1)+" failed to reach "+tiltMotorPosition+"."; String msg="Motor "+(tiltMotor+1)+" failed to reach "+tiltMotorPosition+".";
System.out.println("Error: "+msg); System.out.println("Error: "+msg);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment