uraxisdaemon/src/main/java/at/acdp/urweb/web/FestoXRServer.java

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;
        }
    }
}