From 705c8b9a1b2769b86d3a13324d04e21954a598c0 Mon Sep 17 00:00:00 2001 From: colyic Date: Wed, 29 Nov 2023 17:14:43 -0500 Subject: [PATCH] Finish ElevatorVisualizer Co-authored-by: ambers7 --- .Glass/glass.json | 7 ++ networktables.json | 1 + simgui-ds.json | 92 +++++++++++++++++ simgui.json | 30 ++++++ .../com/stuypulse/robot/RobotContainer.java | 4 + .../stuypulse/robot/constants/Settings.java | 2 + .../robot/util/ElevatorVisualizer.java | 99 +++++++++++++++++++ 7 files changed, 235 insertions(+) create mode 100644 .Glass/glass.json create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json create mode 100644 src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..083db85 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,7 @@ +{ + "NetworkTables": { + "Retained Values": { + "open": false + } + } +} diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/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 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..c437f6d --- /dev/null +++ b/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 + } + } + } +} diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 95fd359..6c15c75 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/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 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() { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 0d68ed6..338113d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/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); diff --git a/src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java new file mode 100644 index 0000000..f0e0bf7 --- /dev/null +++ b/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); + } +} \ No newline at end of file