Browse Source

evve deployed 16.07.2020

Martin Kunz 2 years ago
parent
commit
7e360fc27a

+ 9 - 8
src/main/java/at/acdp/urweb/Main.java

@@ -27,7 +27,7 @@ public class Main {
             var so=new RtdeSetupOutputs()
                     .addVariable("timestamp")
                     .addVariable("robot_mode")
-                    .addVariable("safety_mode")
+                    .addVariable("safety_mode") //
                     .addVariable("safety_status")
                     .addVariable("target_speed_fraction")
                     .addVariable("runtime_state")
@@ -42,15 +42,16 @@ public class Main {
             HTTPServerEndpoint xrs = XR.server(fxrs, FestoXRServer.class);
             server.add(xrs);
             server.start();
+            while(true) {
+                try {
+                    fhm.start(app);
+                }
+                catch (Exception e) {
+                    logger.warn("modbus failed", e);
+                }
+            }
         } catch (Exception e) {
             logger.error("Server exited", e);
         }
-        try {
-            fhm.start(app);
-        } catch (Exception e) {
-            logger.error("failed.", e);
-            picocli.CommandLine.usage(new Params(), System.out);
-            System.exit(1);
-        }
     }
 }

+ 1 - 1
src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -129,7 +129,7 @@ public class RTDEClient implements Runnable {
                 p.read(dis, length-3);
                 lastData=p.getPdata();
                 lastData.entrySet().forEach(entry->{
-                    System.out.println(entry.getKey() + " " + entry.getValue());
+                    logger.info(entry.getKey() + " " + entry.getValue());
                 });
                 break;
 

+ 122 - 109
src/main/java/at/acdp/urweb/web/FestoXRServer.java

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