Finish ElevatorVisualizer

Co-authored-by: ambers7 <[email protected]>

.Glass/glass.json

@@ -0,0 +1,7 @@
+{
+  "NetworkTables": {
+    "Retained Values": {
+      "open": false
+    }
+  }
+}

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,30 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
+      "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Elevator": "Mechanism2d"
+    },
+    "windows": {
+      "/SmartDashboard/Elevator": {
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "Elevator": {
+          "left root": {
+            "open": true
+          },
+          "open": true
+        },
+        "open": true
+      }
+    }
+  }
+}

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -7,9 +7,11 @@
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.util.ElevatorVisualizer;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
+import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -21,6 +23,7 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
+    ElevatorVisualizer elevator = new ElevatorVisualizer();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
@@ -53,6 +56,7 @@ public void configureAutons() {
         autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton());
 
         SmartDashboard.putData("Autonomous", autonChooser);
+        // SmartDashboard.putData("elevator", elevator);
     }
 
     public Command getAutonomousCommand() {

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -18,6 +18,8 @@ public interface Elevator {
         double MIN_HEIGHT = 0.0;
         double MAX_HEIGHT = 0.0;
 
+        double MAX_HEIGHT_ERROR = 0.0;
+
         SmartNumber VEL_LIMIT = new SmartNumber("Velocity Limit", 0);
         SmartNumber ACC_LIMIT = new SmartNumber("Acceleration Limit",0);
 

src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java

@@ -0,0 +1,99 @@
+package com.stuypulse.robot.util;
+
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+public class ElevatorVisualizer {
+    private final Mechanism2d elevator;
+
+    // ligaments
+    private MechanismLigament2d leftLigament;
+    private MechanismLigament2d rightLigament;
+
+    private MechanismLigament2d firstTopLigament;
+    private MechanismLigament2d firstBottomLigament;
+    private MechanismLigament2d firstLeftLigament;
+    private MechanismLigament2d firstRightLigament;
+
+    private MechanismLigament2d secondTopLigament;
+    private MechanismLigament2d secondBottomLigament;
+    private MechanismLigament2d secondLeftLigament;
+    private MechanismLigament2d secondRightLigament;
+
+    // roots
+    private MechanismRoot2d leftRoot;
+    private MechanismRoot2d rightRoot;
+
+    private MechanismRoot2d firstRightBottomRoot;
+    private MechanismRoot2d firstLeftBottomRoot;
+    private MechanismRoot2d firstTopRoot;
+
+    private MechanismRoot2d secondLeftBottomRoot;
+    private MechanismRoot2d secondRightBottomRoot;
+    private MechanismRoot2d secondTopRoot;
+
+    // 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 
+
+        // root nodes
+
+        //outer shell
+        leftRoot = elevator.getRoot("left root", 3,0);
+        rightRoot = elevator.getRoot("right root", 7,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);
+
+        //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);
+
+        // ligaments
+
+        //outer shell
+        rightLigament = new MechanismLigament2d("right ligament", 7, 90, 8, red);
+        leftLigament = new MechanismLigament2d("left ligament", 7, 90, 8, red);
+
+        // 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);
+
+        // 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);
+
+        //outer shell
+        leftRoot.append(leftLigament);
+        rightRoot.append(rightLigament);
+
+        //first shell 
+        firstLeftBottomRoot.append(firstBottomLigament); //bottom horizontal
+        firstLeftBottomRoot.append(firstLeftLigament); //left vertical
+        firstTopRoot.append(firstTopLigament); //top horiontal
+        firstRightBottomRoot.append(firstRightLigament); // right vertical
+
+        // second shell
+        secondLeftBottomRoot.append(secondBottomLigament);
+        secondLeftBottomRoot.append(secondLeftLigament);
+        secondTopRoot.append(secondTopLigament);
+        secondRightBottomRoot.append(secondRightLigament);
+
+        SmartDashboard.putData("Elevator", elevator);
+    }
+}