|
@@ -1,9 +1,8 @@
|
|
|
package at.acdp.urweb.web;
|
|
|
+
|
|
|
import at.acdp.urweb.Main;
|
|
|
-import at.acdp.urweb.PosThread;
|
|
|
import at.acdp.urweb.fhpp.Status;
|
|
|
import at.acdp.urweb.rtde.RTDEClient;
|
|
|
-import ch.qos.logback.core.pattern.PostCompileProcessor;
|
|
|
import com.nmote.xr.XRMethod;
|
|
|
import org.slf4j.Logger;
|
|
|
import org.slf4j.LoggerFactory;
|
|
@@ -17,7 +16,70 @@ public class FestoXRServer {
|
|
|
private final RTDEClient rtde;
|
|
|
|
|
|
public FestoXRServer(RTDEClient r) {
|
|
|
- this.rtde=r;
|
|
|
+ this.rtde = r;
|
|
|
+ }
|
|
|
+
|
|
|
+ private static boolean checkPos(int pos) {
|
|
|
+ try {
|
|
|
+ Status s = Main.fhm.readStatus().get();
|
|
|
+ int curPos = s.istPosition;
|
|
|
+
|
|
|
+ logger.info("curpos {} ({})", curPos, pos);
|
|
|
+ if (curPos == pos) {
|
|
|
+ logger.info("reached {}", pos);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ if (s.istMoment == 0 && Math.abs(curPos - pos) < 5) {
|
|
|
+ logger.info("reached {} (moment=0)", pos);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ Thread.sleep(500);
|
|
|
+ } catch (ExecutionException | InterruptedException e) {
|
|
|
+ logger.warn("checkPos failed", e);
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ @XRMethod(value = "status", help = "Returns status")
|
|
|
+ public static Map<String, Object> status() {
|
|
|
+ var x = Main.fhm.readStatus();
|
|
|
+ try {
|
|
|
+ if (x == null)
|
|
|
+ return null;
|
|
|
+ var ret = x.get();
|
|
|
+ Map<String, Object> m = new HashMap<>();
|
|
|
+ m.put("opm1", ret.opm1);
|
|
|
+ m.put("opm2", ret.opm2);
|
|
|
+ m.put("fct", ret.fct);
|
|
|
+ m.put("rdyen", ret.rdyen);
|
|
|
+ m.put("fault", ret.fault);
|
|
|
+ m.put("warn", ret.warn);
|
|
|
+ m.put("open", ret.open);
|
|
|
+ m.put("enabled", ret.enabled);
|
|
|
+
|
|
|
+ m.put("ref", ret.ref);
|
|
|
+ m.put("still", ret.still);
|
|
|
+ m.put("dev", ret.dev);
|
|
|
+ m.put("mov", ret.mov);
|
|
|
+ m.put("teach", ret.teach);
|
|
|
+ m.put("mc", ret.mc);
|
|
|
+ m.put("ack", ret.ack);
|
|
|
+ m.put("halt", ret.halt);
|
|
|
+
|
|
|
+ m.put("func", ret.func);
|
|
|
+ m.put("fgrp", ret.fgrp);
|
|
|
+ m.put("fnum", ret.fnum);
|
|
|
+ m.put("com", ret.com);
|
|
|
+ m.put("abs", ret.abs);
|
|
|
+ m.put("istMoment", ret.istMoment);
|
|
|
+ m.put("istPosition", ret.istPosition);
|
|
|
+ m.put("halt", ret.halt);
|
|
|
+
|
|
|
+ return m;
|
|
|
+ } catch (Exception e) {
|
|
|
+ e.printStackTrace();
|
|
|
+ return null;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
@XRMethod(value = "example.helloWorld", help = "Returns 'Helo ' + argument")
|
|
@@ -34,72 +96,86 @@ public class FestoXRServer {
|
|
|
public String rel(int relPos, int speed) {
|
|
|
try {
|
|
|
logger.info("rel {}", relPos);
|
|
|
- int curPos=Main.fhm.readStatus().get().istPosition;
|
|
|
- int targetPos=curPos+relPos;
|
|
|
+ int curPos = Main.fhm.readStatus().get().istPosition;
|
|
|
+ int targetPos = curPos + relPos;
|
|
|
logger.info("Relative by {} from {} to {} speed {}", relPos, curPos, targetPos, speed);
|
|
|
Main.fhm.bereitschaft();
|
|
|
- if(!Main.fhm.readStatus().get().enabled) {
|
|
|
+ if (!Main.fhm.readStatus().get().enabled) {
|
|
|
logger.info("Controller not enabled");
|
|
|
return "Controller not enabled";
|
|
|
}
|
|
|
- Main.fhm.direktAuftrag(relPos, speed,0, true, false);
|
|
|
+ Main.fhm.direktAuftrag(relPos, speed, 0, true, false);
|
|
|
Thread.sleep(10);
|
|
|
Main.fhm.direktAuftrag(relPos, speed, 1, true, false);
|
|
|
Thread.sleep(10);
|
|
|
|
|
|
- while(true) {
|
|
|
- boolean reached=checkPos(targetPos);
|
|
|
- if(reached) {
|
|
|
+ while (true) {
|
|
|
+ boolean reached = checkPos(targetPos);
|
|
|
+ if (reached) {
|
|
|
logger.info("Reached {}", targetPos);
|
|
|
return "OK";
|
|
|
}
|
|
|
Thread.sleep(100);
|
|
|
}
|
|
|
- } catch (ExecutionException|InterruptedException e) {
|
|
|
+ } catch (ExecutionException | InterruptedException e) {
|
|
|
logger.warn("abs failed", e);
|
|
|
- return "abs failed: "+e.toString() ;
|
|
|
+ return "abs failed: " + e.toString();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@XRMethod(value = "abs", help = "abs")
|
|
|
public String abs(int targetPos, int speed) {
|
|
|
try {
|
|
|
- int round=1;
|
|
|
- boolean stopped=false;
|
|
|
- while(true) {
|
|
|
- logger.info("Absolute to {} speed {} try {}, stopped {}", targetPos, speed, round++, stopped);
|
|
|
- if(!stopped) {
|
|
|
- Main.fhm.bereitschaft();
|
|
|
+ int round = 1;
|
|
|
+ boolean paused = false;
|
|
|
+ restart(targetPos, speed);
|
|
|
+ while (true) {
|
|
|
+ logger.info("Absolute to {} speed {} try {}, stopped {}", targetPos, speed, round, paused);
|
|
|
+ boolean reached = checkPos(targetPos);
|
|
|
+ Status s = Main.fhm.readStatus().get();
|
|
|
+ if (reached) {
|
|
|
+ logger.info("Reached {}", targetPos);
|
|
|
+ return "OK";
|
|
|
+ }
|
|
|
+ int rs = Integer.parseInt(rtde.getLastData().get("runtime_state"));
|
|
|
+ //0=stopping, 1=stopped, 2=running, 3=paueing, 4=paused, 5=resuming, 6=retracting
|
|
|
+ if(paused==true && (rs== 2||rs==5)) {
|
|
|
+ restart(targetPos, speed);
|
|
|
+ paused=false;
|
|
|
+ }
|
|
|
+ if (paused==false&&(rs == 3 || rs == 4)) { //paused -> stop axis
|
|
|
Main.fhm.direktAuftrag(targetPos, speed, 0, false, false);
|
|
|
- Thread.sleep(100);
|
|
|
- Main.fhm.direktAuftrag(targetPos, speed, 1, false, false);
|
|
|
+ Thread.sleep(20);
|
|
|
+ Main.fhm.direktAuftrag(targetPos, speed, 0, false, true);
|
|
|
+ Thread.sleep(20);
|
|
|
+ paused=true;
|
|
|
+
|
|
|
+ }
|
|
|
+ else if (rs == 0 || rs == 1) { //stop -> abort
|
|
|
+ Main.fhm.direktAuftrag(targetPos, speed, 0, false, false);
|
|
|
+ Thread.sleep(20);
|
|
|
+ Main.fhm.direktAuftrag(targetPos, speed, 0, false, true);
|
|
|
+ Thread.sleep(20);
|
|
|
+ logger.info("rs=" + rs + " re");
|
|
|
+ return "OK";
|
|
|
}
|
|
|
- while (true) {
|
|
|
- boolean reached = checkPos(targetPos);
|
|
|
- Status s = Main.fhm.readStatus().get();
|
|
|
- if (reached) {
|
|
|
- logger.info("Reached {}", targetPos);
|
|
|
- return "OK";
|
|
|
- }
|
|
|
- int rs=Integer.parseInt(rtde.getLastData().get("runtime_state"));
|
|
|
- if(rs!=2 && rs!=5) {
|
|
|
- Main.fhm.direktAuftrag(targetPos, speed, 0, false, false);
|
|
|
- Thread.sleep(10);
|
|
|
- Main.fhm.direktAuftrag(targetPos, speed, 0, false, true);
|
|
|
- Thread.sleep(100);
|
|
|
- stopped=true;
|
|
|
- break;
|
|
|
- } else if(stopped==true){
|
|
|
- stopped=false;
|
|
|
- break;
|
|
|
- }
|
|
|
- }}
|
|
|
- } catch (ExecutionException|InterruptedException e) {
|
|
|
+ //TODO: if(timeout)
|
|
|
+ round++;
|
|
|
+ }
|
|
|
+ } catch (ExecutionException | InterruptedException e) {
|
|
|
logger.warn("abs failed", e);
|
|
|
- return "abs failed: "+e.toString();
|
|
|
+ return "abs failed: " + e.toString();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ private void restart(int targetPos, int speed) throws InterruptedException {
|
|
|
+ Main.fhm.bereitschaft();
|
|
|
+ Main.fhm.direktAuftrag(targetPos, speed, 0, false, false);
|
|
|
+ Thread.sleep(20);
|
|
|
+ Main.fhm.direktAuftrag(targetPos, speed, 1, false, false);
|
|
|
+ Thread.sleep(20);
|
|
|
+ }
|
|
|
+
|
|
|
@XRMethod(value = "getpos", help = "getpos")
|
|
|
public int getpos() {
|
|
|
logger.info("GetPos");
|
|
@@ -118,81 +194,18 @@ public class FestoXRServer {
|
|
|
try {
|
|
|
logger.info("Ready called");
|
|
|
Main.fhm.bereitschaft();
|
|
|
- for(int i=0;i<5;i++) {
|
|
|
- Status s=Main.fhm.readStatus().get();
|
|
|
- if(s.enabled) {
|
|
|
+ for (int i = 0; i < 5; i++) {
|
|
|
+ Status s = Main.fhm.readStatus().get();
|
|
|
+ if (s.enabled) {
|
|
|
return "OK";
|
|
|
}
|
|
|
logger.info("Controller not enabled.." + i);
|
|
|
Thread.sleep(100);
|
|
|
}
|
|
|
|
|
|
- } catch (InterruptedException|ExecutionException e) {
|
|
|
+ } catch (InterruptedException | ExecutionException e) {
|
|
|
logger.warn("ready failed", e);
|
|
|
}
|
|
|
return "Controller not enabled";
|
|
|
}
|
|
|
-
|
|
|
- private static boolean checkPos(int pos) {
|
|
|
- try {
|
|
|
- Status s= Main.fhm.readStatus().get();
|
|
|
- int curPos = s.istPosition;
|
|
|
-
|
|
|
- logger.info("curpos {} ({})", curPos, pos);
|
|
|
- if(curPos==pos) {
|
|
|
- logger.info("reached {}", pos);
|
|
|
- return true;
|
|
|
- }
|
|
|
- if(s.istMoment==0 && Math.abs(curPos-pos) <5) {
|
|
|
- logger.info("reached {} (moment=0)", pos);
|
|
|
- return true;
|
|
|
- }
|
|
|
- Thread.sleep(500);
|
|
|
- } catch (ExecutionException|InterruptedException e) {
|
|
|
- logger.warn("checkPos failed", e);
|
|
|
- }
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- @XRMethod(value = "status", help = "Returns status")
|
|
|
- public static Map<String, Object> status() {
|
|
|
- var x=Main.fhm.readStatus();
|
|
|
- try {
|
|
|
- if(x==null)
|
|
|
- return null;
|
|
|
- var ret= x.get();
|
|
|
- Map<String, Object> m=new HashMap<>();
|
|
|
- m.put("opm1", ret.opm1);
|
|
|
- m.put("opm2", ret.opm2);
|
|
|
- m.put("fct", ret.fct);
|
|
|
- m.put("rdyen", ret.rdyen);
|
|
|
- m.put("fault", ret.fault);
|
|
|
- m.put("warn", ret.warn);
|
|
|
- m.put("open", ret.open);
|
|
|
- m.put("enabled", ret.enabled);
|
|
|
-
|
|
|
- m.put("ref", ret.ref);
|
|
|
- m.put("still", ret.still);
|
|
|
- m.put("dev", ret.dev);
|
|
|
- m.put("mov", ret.mov);
|
|
|
- m.put("teach", ret.teach);
|
|
|
- m.put("mc", ret.mc);
|
|
|
- m.put("ack", ret.ack);
|
|
|
- m.put("halt", ret.halt);
|
|
|
-
|
|
|
- m.put("func", ret.func);
|
|
|
- m.put("fgrp", ret.fgrp);
|
|
|
- m.put("fnum", ret.fnum);
|
|
|
- m.put("com", ret.com);
|
|
|
- m.put("abs", ret.abs);
|
|
|
- m.put("istMoment", ret.istMoment);
|
|
|
- m.put("istPosition", ret.istPosition);
|
|
|
- m.put("halt", ret.halt);
|
|
|
-
|
|
|
- return m;
|
|
|
- } catch (Exception e) {
|
|
|
- e.printStackTrace();
|
|
|
- return null;
|
|
|
- }
|
|
|
- }
|
|
|
}
|