fixed reversed trajectories

Jarhead20 · Feb 1, 2023 · baa352a · baa352a
1 parent ae3ba4a
commit baa352a
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 14 deletions.
diff --git a/src/main/java/jarhead/ButtonPanel.java b/src/main/java/jarhead/ButtonPanel.java
@@ -141,7 +141,7 @@ public void export(){
         if(getCurrentManager().size() > 0){
             main.infoPanel.editPanel.saveValues();
             main.infoPanel.markerPanel.saveValues();
-            Node node = getCurrentManager().get(0);
+            Node node = getCurrentManager().getNodes().get(0);
             double x = main.toInches(node.x);
             double y = main.toInches(node.y);
             if(!robot.importPath.matches("")){
@@ -167,14 +167,22 @@ public void export(){
             markers.sort((n1, n2) -> ((Double) n1.displacement).compareTo(n2.displacement));
             for (int i = 0; i < markers.size(); i++) {
                 Marker marker = markers.get(i);
-                System.out.println(marker.displacement);
-                sb.append(String.format(".UNSTABLE_addTemporalMarkerOffset((%.2f) -> {%s})%n", marker.displacement, marker.code));
+                sb.append(String.format(".UNSTABLE_addTemporalMarkerOffset(%.2f,() -> {%s})%n", marker.displacement, marker.code));
             }
-
-            for (int i = 1; i < getCurrentManager().size(); i++) {
+            boolean prev = false;
+            for (int i = 0; i < getCurrentManager().size(); i++) {
                 node = getCurrentManager().get(i);
+                if(node.equals(getCurrentManager().getNodes().get(0))) {
+                    if(node.reversed != prev){
+                        sb.append(String.format(".setReversed(%s)%n", node.reversed));
+                        prev = node.reversed;
+                    }
+                    continue;
+                }
                 x = main.toInches(node.x);
                 y = main.toInches(node.y);
+
+
                 switch (node.getType()){
                     case splineTo:
                         sb.append(String.format(".splineTo(new Vector2d(%.2f, %.2f), Math.toRadians(%.2f))%n", x, -y, (node.splineHeading +90)));
@@ -200,10 +208,16 @@ public void export(){
                     case lineToConstantHeading:
                         sb.append(String.format(".lineToConstantHeading(new Vector2d(%.2f, %.2f))%n", x, -y, (node.splineHeading +90)));
                         break;
+                    case addTemporalMarker:
+                        break;
                     default:
                         sb.append("couldn't find type");
                         break;
                 }
+                if(node.reversed != prev){
+                    sb.append(String.format(".setReversed(%s)%n", node.reversed));
+                    prev = node.reversed;
+                }
             }
             sb.append(String.format(".build();%n"));
             if(main.exportPanel.addPoseEstimate) sb.append(String.format("drive.setPoseEstimate(%s.start());", getCurrentManager().name));

diff --git a/src/main/java/jarhead/DrawPanel.java b/src/main/java/jarhead/DrawPanel.java
@@ -306,12 +306,14 @@ public void paintComponent (Graphics g){
     private TrajectorySequence generateTrajectory(NodeManager manager, Node exlude){
         Node node = exlude.shrink(main.scale);
         TrajectorySequenceBuilder builder = new TrajectorySequenceBuilder(new Pose2d(node.x, node.y, Math.toRadians(-node.robotHeading - 90)), Math.toRadians(-node.splineHeading - 90), new MecanumVelocityConstraint(60.0, 10), new ProfileAccelerationConstraint(60), 60, 60);
+        builder.setReversed(exlude.reversed);
         for (int i = 0; i < manager.size(); i++) {
             if(exlude.equals(manager.get(i))) continue; //stops empty path segment error
 
             node = manager.get(i).shrink(main.scale);
 
             try{
+
                 switch (node.getType()){
                     case splineTo:
                         builder.splineTo(new Vector2d(node.x, node.y), Math.toRadians(-node.splineHeading-90));
@@ -341,6 +343,7 @@ private TrajectorySequence generateTrajectory(NodeManager manager, Node exlude){
                         Marker marker = (Marker) manager.get(i);
                         builder.UNSTABLE_addTemporalMarkerOffset(marker.displacement, () -> {});
                 }
+                builder.setReversed(node.reversed);
             } catch (Exception e) {
                 main.undo(false);
                 i--;
@@ -380,14 +383,15 @@ private void renderArrows(Graphics g, NodeManager nodeM, int ovalscale, Color co
         Graphics2D g2d = (Graphics2D) g.create();
         BufferedImage bufferedImage = new BufferedImage(preRenderedSplines.getWidth(), preRenderedSplines.getHeight(), BufferedImage.TYPE_4BYTE_ABGR);
         Graphics2D g2 = bufferedImage.createGraphics();
-        for (int i = 0; i < nodeM.size(); i++) {
-            Node node = nodeM.get(i);
+        List<Node> nodes = nodeM.getNodes();
+        for (int i = 0; i < nodes.size(); i++) {
+            Node node = nodes.get(i);
             tx.setToIdentity();
             tx.translate(node.x, node.y);
-            if(nodeM.reversed)
-                tx.rotate(Math.toRadians(-node.robotHeading));
-            else
+            if(!node.reversed)
                 tx.rotate(Math.toRadians(-node.robotHeading +180));
+            else
+                tx.rotate(Math.toRadians(-node.robotHeading));
             tx.scale(main.scale, main.scale);
 
             g2.setTransform(tx);
@@ -533,6 +537,7 @@ private void mPressed(MouseEvent e) {
                         index = i;
                         mouse.splineHeading = close.splineHeading;
                         mouse.robotHeading = close.robotHeading;
+                        mouse.reversed = close.reversed;
                         mid = false;
                     }
                 }
@@ -685,8 +690,9 @@ private void keyInput(KeyEvent e){
             }
         }
         if(e.getKeyCode() == KeyEvent.VK_R) {
-            getCurrentManager().reversed = !getCurrentManager().reversed;
-            getCurrentManager().get(0).splineHeading += 180;
+            if(main.currentN != -1){
+                getCurrentManager().get(main.currentN).reversed = !getCurrentManager().get(main.currentN).reversed;
+            }
         }
         if(e.isControlDown() && e.getKeyCode() == KeyEvent.VK_Z){
             main.undo();

diff --git a/src/main/java/jarhead/Node.java b/src/main/java/jarhead/Node.java
@@ -7,6 +7,7 @@ public class Node {
     public double robotHeading;
     public int index = -1;
     public int state = 1;
+    public boolean reversed = false;
 
     private Type type = Type.splineTo;
 
@@ -20,7 +21,6 @@ public enum Type {
         lineToLinearHeading,
         lineToConstantHeading,
         addTemporalMarker,
-        setReversed
     }
     Node(){
 
@@ -70,6 +70,7 @@ public Node copy(){
         node.state = this.state;
         node.type = this.type;
         node.robotHeading = this.robotHeading;
+        node.reversed = this.reversed;
         return node;
     }
 

diff --git a/src/main/java/jarhead/NodeManager.java b/src/main/java/jarhead/NodeManager.java
@@ -13,7 +13,6 @@ public class NodeManager {
     public int editIndex = -1;
     private ArrayList nodes;
     public String name;
-    public boolean reversed = false;
     private int id;
 
     NodeManager(ArrayList<Node> nodes, int id){