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

package at.acdp.urweb.web;

import at.acdp.urweb.Main;
import at.acdp.urweb.fhpp.Status;
import at.acdp.urweb.rtde.RTDEClient;
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;
    }

    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")
    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(20);
            Main.fhm.direktAuftrag(relPos, speed, 1, true, false);
            Thread.sleep(20);

            while (true) {
                boolean reached = checkPos(targetPos);
                if (reached) {
                    logger.info("Reached {}", targetPos);
                    return "OK";
                }
                Thread.sleep(100);
            }
        } catch (ExecutionException | InterruptedException e) {
            logger.warn("rel failed", e);
            return "rel failed: " + e.toString();
        }
    }

    @XRMethod(value = "abs", help = "abs")
    public String abs(int targetPos, int speed) {
        try {
            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
                    Thread.sleep(20);
                    Main.fhm.direktAuftrag(targetPos, speed, 0, 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
                    Thread.sleep(20);
                    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";
                }
                //TODO: if(timeout)
                round++;
            }
        } catch (ExecutionException | InterruptedException e) {
            logger.warn("abs failed", e);
            return "abs failed: " + e.toString();
        }
    }

    private void restart(int targetPos, int speed) throws InterruptedException {
        Main.fhm.bereitschaft();
        Thread.sleep(20);
        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");
        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";
    }
}