Arm visualization uses height+width variable

StuyPulse · Dec 1, 2023 · b128d9d · b128d9d
1 parent fe2be7a
commit b128d9d
Showing 1 changed file with 43 additions and 19 deletions.
diff --git a/src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java
@@ -1,5 +1,7 @@
 package com.stuypulse.robot.util;
 
+import com.stuypulse.stuylib.network.SmartNumber;
+
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -35,48 +37,61 @@ public class ElevatorVisualizer {
     private MechanismRoot2d secondLeftBottomRoot;
     private MechanismRoot2d secondRightBottomRoot;
     private MechanismRoot2d secondTopRoot;
+
+    private SmartNumber height;
+    private SmartNumber width;
+
+    private double leftRootx;
+    private double rightRootx;
 
     // colors
     private Color8Bit white = new Color8Bit(255,255,255);
     private Color8Bit blue = new Color8Bit(0, 0, 255);
     private Color8Bit red = new Color8Bit(255, 0, 0);
 
     public ElevatorVisualizer() {
-        elevator = new Mechanism2d(10,8); //width x height  //30 x 46 
+        width = new SmartNumber("Elevator/width", 10);
+        height = new SmartNumber("Elevator/height", 8);
+        double xincrement = 0.2;
+        double yincrement = 0.5;
+        double xfromorigin = 3; //where we start elevator 
+        leftRootx = xfromorigin;
+        rightRootx = width.getAsDouble()-xfromorigin;
+        elevator = new Mechanism2d(width.getAsDouble(),height.getAsDouble()); //width x height  //10 x 8
 
         // root nodes
 
         //outer shell
-        leftRoot = elevator.getRoot("left root", 3,0);
-        rightRoot = elevator.getRoot("right root", 7,0);
+        leftRoot = elevator.getRoot("left root", leftRootx,0);
+        rightRoot = elevator.getRoot("right root", rightRootx,0);
 
         //first stage 
-        firstLeftBottomRoot = elevator.getRoot("first left bottom root", 3.2,0);
-        firstRightBottomRoot = elevator.getRoot("first right bottom root", 6.8,0);
-        firstTopRoot = elevator.getRoot("first top root", 3.2,6.5);
+        firstLeftBottomRoot = elevator.getRoot("first left bottom root", leftRootx+xincrement,0); //3.2
+        firstRightBottomRoot = elevator.getRoot("first right bottom root", rightRootx-xincrement,0); //6.8
+        firstTopRoot = elevator.getRoot("first top root", leftRootx+xincrement,height.getAsDouble()-yincrement); //3.2,6.5
 
         //second stage
-        secondLeftBottomRoot = elevator.getRoot("second left bottom root", 3.4,.5);
-        secondRightBottomRoot = elevator.getRoot("second right bottom root", 6.6,.5);
-        secondTopRoot = elevator.getRoot("second top root", 3.4,2);
+        secondLeftBottomRoot = elevator.getRoot("second left bottom root", leftRootx+2*xincrement,yincrement); //3.4 .5
+        secondRightBottomRoot = elevator.getRoot("second right bottom root", rightRootx-2*xincrement,yincrement); //6.6 .5
+        secondTopRoot = elevator.getRoot("second top root", leftRootx+2*xincrement,yincrement*4); //3.4 2
 
         // ligaments
 
         //outer shell
-        rightLigament = new MechanismLigament2d("right ligament", 7, 90, 8, red);
-        leftLigament = new MechanismLigament2d("left ligament", 7, 90, 8, red);
+        rightLigament = new MechanismLigament2d("right ligament", rightRootx, 90, 8, red); //7
+        leftLigament = new MechanismLigament2d("left ligament", rightRootx, 90, 8, red); //7
 
         // first stage
-        firstTopLigament = new MechanismLigament2d("elevator top ligament first", 3.6, 0,8,blue);
-        firstBottomLigament = new MechanismLigament2d("elevator bottom ligament first", 3.6, 0,8,blue);
-        firstLeftLigament = new MechanismLigament2d("elevator left ligament first",6.5,90, 8, blue);
-        firstRightLigament = new MechanismLigament2d("elevator right ligament first",6.5, 90, 8, blue);
+        firstTopLigament = new MechanismLigament2d("elevator top ligament first", leftRootx+3*xincrement, 0,8,blue); //3.6
+        firstBottomLigament = new MechanismLigament2d("elevator bottom ligament first", leftRootx+3*xincrement, 0,8,blue); //3.6
+        firstLeftLigament = new MechanismLigament2d("elevator left ligament first",rightRootx-xincrement,90, 8, blue); //6.5
+        firstRightLigament = new MechanismLigament2d("elevator right ligament first",rightRootx-xincrement, 90, 8, blue); //6.5
 
         // second stage
-        secondTopLigament = new MechanismLigament2d("elevator top ligament second", 3.2, 0,8, white);
-        secondBottomLigament = new MechanismLigament2d("elevator bottom ligament second", 3.2, 0,8,white);
-        secondLeftLigament = new MechanismLigament2d("elevator left ligament second", 1.5, 90,8,white);
-        secondRightLigament = new MechanismLigament2d("elevator right ligament second", 1.5, 90,8,white);
+        secondTopLigament = new MechanismLigament2d("elevator top ligament second", leftRootx+xincrement, 0,8, white); //3.2
+        secondBottomLigament = new MechanismLigament2d("elevator bottom ligament second", leftRootx+xincrement, 0,8,white); //3.2
+        secondLeftLigament = new MechanismLigament2d("elevator left ligament second", leftRootx/2, 90,8,white); //1.5
+        secondRightLigament = new MechanismLigament2d("elevator right ligament second", leftRootx/2, 90,8,white); //1.5
 
         //outer shell
         leftRoot.append(leftLigament);
@@ -95,5 +110,14 @@ public ElevatorVisualizer() {
         secondRightBottomRoot.append(secondRightLigament);
 
         SmartDashboard.putData("Elevator", elevator);
+        // SmartDashboard.putNumber("Elevator/width", width);
+        // SmartDashboard.putNumber("Elevator/height", height);
+        SmartDashboard.putNumber("Elevator/starting x", xfromorigin);
     }
+
+    void updateValues(double newWidth, double newHeight) {
+        width = new SmartNumber("Elevator/width", newWidth);
+        height = new SmartNumber("Elevator/height", newHeight);
+    }
+
 }