123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199 |
- 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;
- import java.util.HashMap;
- import java.util.Map;
- import java.util.concurrent.ExecutionException;
- public class FestoXRServer {
- private final static Logger logger = LoggerFactory.getLogger(FestoXRServer.class);
- private final RTDEClient rtde;
- public FestoXRServer(RTDEClient r) {
- this.rtde=r;
- }
- @XRMethod(value = "example.helloWorld", help = "Returns 'Helo ' + argument")
- public String hello(Object s) {
- return "Hello '" + s + "'";
- }
- @XRMethod(value = "ref2", help = "ref")
- public String ref(String a, String b) {
- return "ref";
- }
- @XRMethod(value = "rel", help = "rel")
- public String rel(int relPos, int speed) {
- try {
- logger.info("rel {}", 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) {
- logger.info("Controller not enabled");
- return "Controller not enabled";
- }
- 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) {
- logger.info("Reached {}", targetPos);
- return "OK";
- }
- Thread.sleep(100);
- }
- } catch (ExecutionException|InterruptedException e) {
- logger.warn("abs failed", e);
- 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();
- Main.fhm.direktAuftrag(targetPos, speed, 0, false, false);
- Thread.sleep(100);
- Main.fhm.direktAuftrag(targetPos, speed, 1, false, false);
- }
- 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) {
- logger.warn("abs failed", e);
- return "abs failed: "+e.toString();
- }
- }
- @XRMethod(value = "getpos", help = "getpos")
- public int getpos() {
- logger.info("GetPos");
- try {
- return Main.fhm.readStatus().get().istPosition;
- } catch (InterruptedException e) {
- e.printStackTrace();
- } catch (ExecutionException e) {
- e.printStackTrace();
- }
- return -1;
- }
- @XRMethod(value = "ready", help = "ready")
- public String ready() {
- try {
- logger.info("Ready called");
- Main.fhm.bereitschaft();
- 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) {
- 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;
- }
- }
- }
|