Fix Elevator Commands

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

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Launch Main",
+      "request": "launch",
+      "mainClass": "com.stuypulse.robot.Main",
+      "projectName": "ColyiAmber"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

simgui.json

@@ -23,17 +23,11 @@
     "transitory": {
       "SmartDashboard": {
         "Elevator": {
-          "first top root": {
-            "open": true
-          },
-          "right root": {
-            "open": true
-          },
+          "open": true,
           "second left bottom root": {
             "elevator bottom ligament second": {
               "open": true
-            },
-            "open": true
+            }
           }
         },
         "open": true

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

@@ -5,14 +5,17 @@
 
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.commands.ElevatorDrive;
+import com.stuypulse.robot.commands.ElevatorToBottom;
+import com.stuypulse.robot.commands.ElevatorToHeight;
+import com.stuypulse.robot.commands.ElevatorToTop;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.subsystems.Elevator;
-import com.stuypulse.robot.util.ElevatorVisualizer;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
+import com.stuypulse.stuylib.input.gamepads.Xbox;
 
-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,7 +24,7 @@ public class RobotContainer {
 
     // Gamepads
     public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER);
-    public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
+    public final Gamepad operator = new Xbox(Ports.Gamepad.OPERATOR);
 
     // Subsystem
     Elevator elevator = Elevator.getInstance();
@@ -35,13 +38,17 @@ public RobotContainer() {
         configureDefaultCommands();
         configureButtonBindings();
         configureAutons();
+
+        SmartDashboard.putData("Gamepads/Operator", driver);
     }
 
     /****************/
     /*** DEFAULTS ***/
     /****************/
 
-    private void configureDefaultCommands() {}
+    private void configureDefaultCommands() {
+        elevator.setDefaultCommand(new ElevatorDrive(driver));
+    }
 
     /***************/
     /*** BUTTONS ***/
@@ -55,6 +62,9 @@ private void configureButtonBindings() {}
 
     public void configureAutons() {
         autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton());
+        autonChooser.addOption("Elevator To Bottom", new ElevatorToBottom());
+        autonChooser.addOption("Elevator To Top", new ElevatorToTop());
+        autonChooser.addOption("Elevator To Height",new ElevatorToHeight(1.2));
 
         SmartDashboard.putData("Autonomous", autonChooser);
     }

src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java

@@ -1,17 +1,19 @@
 package com.stuypulse.robot.commands;
 
-import com.stuypulse.robot.subsystems.ElevatorImpl;
+import com.stuypulse.robot.subsystems.Elevator;
+
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.streams.IStream;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class ElevatorDrive extends CommandBase {
-    private final ElevatorImpl elevator;
+    private final Elevator elevator;
     private IStream velocity;
 
-    public ElevatorDrive(ElevatorImpl elevator, Gamepad gamepad) {
-        this.elevator = elevator;
+    public ElevatorDrive(Gamepad gamepad) {
+        elevator = Elevator.getInstance();
 
         velocity = IStream.create(gamepad::getLeftY); // left Y is elevator in controls 
 
@@ -20,7 +22,8 @@ public ElevatorDrive(ElevatorImpl elevator, Gamepad gamepad) {
 
     @Override
     public void execute() {
-        elevator.addTargetHeight(velocity.get());
+        elevator.setTargetHeight(velocity.get());
+        SmartDashboard.putNumber("Gamepad velocity", velocity.getAsDouble());
     }
 
     @Override

src/main/java/com/stuypulse/robot/commands/ElevatorToBottom.java

@@ -1,11 +1,9 @@
 package com.stuypulse.robot.commands;
 
-import com.stuypulse.robot.subsystems.ElevatorImpl;
-
 import static com.stuypulse.robot.constants.Settings.Elevator.*;
 
 public class ElevatorToBottom extends ElevatorToHeight {
-    public ElevatorToBottom(ElevatorImpl elevator) {
-        super(elevator, MIN_HEIGHT);
+    public ElevatorToBottom() {
+        super(MIN_HEIGHT);
     }
 }

src/main/java/com/stuypulse/robot/commands/ElevatorToHeight.java

@@ -1,19 +1,19 @@
 package com.stuypulse.robot.commands;
 
-import com.stuypulse.robot.subsystems.ElevatorImpl;
-
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 import static com.stuypulse.robot.constants.Settings.Elevator.*;
 
+import com.stuypulse.robot.subsystems.Elevator;
+
 public class ElevatorToHeight extends CommandBase {
-    private final ElevatorImpl elevator;
+    private final Elevator elevator;
     private final double height; 
 
     private boolean instant;
 
-    public ElevatorToHeight(ElevatorImpl elevator, double height) {
-        this.elevator = elevator;
+    public ElevatorToHeight(double height) {
+        elevator = Elevator.getInstance();
         this.height = height;
 
         instant = true;

src/main/java/com/stuypulse/robot/commands/ElevatorToTop.java

@@ -1,11 +1,9 @@
 package com.stuypulse.robot.commands;
 
-import com.stuypulse.robot.subsystems.ElevatorImpl;
-
 import static com.stuypulse.robot.constants.Settings.Elevator.*;
 
 public class ElevatorToTop extends ElevatorToHeight {
-    public ElevatorToTop(ElevatorImpl elevator) {
-        super(elevator, MAX_HEIGHT);
+    public ElevatorToTop() {
+        super(MAX_HEIGHT);
     }
 }

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

@@ -17,9 +17,10 @@
  */
 public interface Settings {
     public interface Elevator {
+        double DT = 0.3; //time between each simulation update
         double GEARING = 1;
         double CARRIAGE_MASS = 15; // kg
-        double DRUM_RADIUS = Units.inchesToMeters(1); // meters
+        double DRUM_RADIUS = 0.025; // meters
 
         double MIN_HEIGHT = 0.1;
         double MAX_HEIGHT = 1.5;

src/main/java/com/stuypulse/robot/subsystems/Elevator.java

@@ -12,6 +12,7 @@
 import com.stuypulse.stuylib.streams.filters.MotionProfile;
 
 import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public abstract class Elevator extends SubsystemBase {
@@ -63,6 +64,6 @@ public final boolean isReady(double error) {
 
     @Override
     public void periodic() {
-        elevatorVisualizer.setHeight(getHeight());
+        elevatorVisualizer.setHeight(targetHeight.getAsDouble());
     }
 }

src/main/java/com/stuypulse/robot/subsystems/SimElevator.java

@@ -26,4 +26,12 @@ public double getVelocity() {
     public double getHeight() {
         return sim.getPositionMeters();
     }
+
+    @Override
+    public void simulationPeriodic() {
+        sim.update(DT);
+
+        height = sim.getPositionMeters();
+        velocity = sim.getVelocityMetersPerSecond();
+    }
 }