173 add sysid routines to elevator (#178)

* Elevator SYSID slayed Co-authored-by: Asa <axxwang13@gmail.com> Co-authored-by: AlexNunemacher <AlexNunemacher@users.noreply.github.com> * Co-authored-by: Asa <axxwang13@gmail.com> Co-authored-by: AlexNunemacher <AlexNunemacher@users.noreply.github.com> * i legit cant figure out why sysid wont work * Co-authored-by: Raymond Dokholyan <urayed@users.noreply.github.com> * fix * final fixing commit --------- Co-authored-by: wibwib <ray@dokholyan.org> Co-authored-by: Asa <axxwang13@gmail.com> Co-authored-by: AlexNunemacher <AlexNunemacher@users.noreply.github.com> Co-authored-by: Matthew Milunic <62996888+mmilunicmobile@users.noreply.github.com> Co-authored-by: mmilunicmobile <mmilunicmobile@gmail.com>
FRC2539 · Feb 8, 2025 · a47afeb · a47afeb
1 parent e106f80
commit a47afeb
Show file tree

Hide file tree

Showing 6 changed files with 128 additions and 14 deletions.
diff --git a/elastic-layout.json b/elastic-layout.json
@@ -343,6 +343,71 @@
                 }
               }
             ]
+          },
+          {
+            "title": "Elevator Characterization",
+            "x": 512.0,
+            "y": 0.0,
+            "width": 384.0,
+            "height": 512.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "Elevator SysId Dynamic Reverse",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Elevator SysId Dynamic Reverse",
+                  "period": 0.06,
+                  "show_type": true
+                }
+              },
+              {
+                "title": "Elevator SysId Dynamic Forward",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Elevator SysId Dynamic Forward",
+                  "period": 0.06,
+                  "show_type": true
+                }
+              },
+              {
+                "title": "Elevator SysId Quasistatic Reverse",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Elevator SysId Quasistatic Reverse",
+                  "period": 0.06,
+                  "show_type": true
+                }
+              },
+              {
+                "title": "Elevator SysId Quasistatic Forward",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 256.0,
+                "height": 128.0,
+                "type": "Command",
+                "properties": {
+                  "topic": "/SmartDashboard/Elevator SysId Quasistatic Forward",
+                  "period": 0.06,
+                  "show_type": true
+                }
+              }
+            ]
           }
         ],
         "containers": []

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -279,6 +279,24 @@ private void configureBindings() {
                                 WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain)
                         .withName("Wheel Radius Characterization Command"));
 
+        SmartDashboard.putData(
+                elevatorSubsystem
+                        .runDynamicElevatorSysId(Direction.kForward)
+                        .withName("Elevator SysId Dynamic Forward"));
+        SmartDashboard.putData(
+                elevatorSubsystem
+                        .runDynamicElevatorSysId(Direction.kReverse)
+                        .withName("Elevator SysId Dynamic Reverse"));
+        SmartDashboard.putData(
+                elevatorSubsystem
+                        .runQStaticElevatorSysId(Direction.kForward)
+                        .withName("Elevator SysId Quasistatic Forward"));
+        SmartDashboard.putData(
+                elevatorSubsystem
+                        .runQStaticElevatorSysId(Direction.kReverse)
+                        .withName("Elevator SysId Quasistatic Reverse"));
+
+        SmartDashboard.putData(elevatorSubsystem);
         // operatorController
         // operatorController.getA().onTrue(stateManager.moveToPosition(Position.L4));
         // operatorController.getB().onTrue(stateManager.moveToPosition(Position.L3));

diff --git a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems.ModeManager;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
@@ -354,6 +355,12 @@ public SuperstructureStateManager(
 
         SmartDashboard.putData("Mech2d", mech);
 
-        setDefaultCommand(moveToPosition(Position.Home));
+        Command defaultcom =
+                Commands.either(
+                        Commands.idle(),
+                        moveToPosition(Position.Home).asProxy(),
+                        () -> !RobotState.isAutonomous());
+        defaultcom.addRequirements(this);
+        setDefaultCommand(defaultcom);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
@@ -4,7 +4,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ArmConstants;
-import frc.robot.constants.WristConstants;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 
@@ -50,11 +49,11 @@ public Command tunablePose() {
     }
 
     public Command setPosition(double position) {
-        if (position > WristConstants.upperLimit) {
-            position = WristConstants.upperLimit;
+        if (position > ArmConstants.upperLimit) {
+            position = ArmConstants.upperLimit;
         }
-        if (position < WristConstants.lowerLimit) {
-            position = WristConstants.lowerLimit;
+        if (position < ArmConstants.lowerLimit) {
+            position = ArmConstants.lowerLimit;
         }
         double nextPosition = position;
 

diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java
@@ -1,30 +1,32 @@
 package frc.robot.subsystems.elevator;
 
+import edu.wpi.first.math.controller.PIDController;
+
 public class ElevatorIOSim implements ElevatorIO {
     private double position = 0;
     private double positionSetpoint = 0;
     private double voltage = 0;
+    private PIDController sussyAhh = new PIDController(5, 0, 0);
+
+    private boolean positionControl = false;
 
     public void updateInputs(ElevatorIOInputs inputs) {
-        final double stepAmount = 1;
-        if (positionSetpoint > position) {
-            position += stepAmount * 0.02;
-            if (position > positionSetpoint) position = positionSetpoint;
-        } else if (positionSetpoint < position) {
-            position -= stepAmount * 0.02;
-            if (position < positionSetpoint) position = positionSetpoint;
+        if (positionControl) {
+            voltage = sussyAhh.calculate(position, positionSetpoint);
         }
+        position += voltage * 0.02;
 
         inputs.position = position;
         inputs.voltage = voltage;
     }
 
     public void setVoltage(double voltage) {
+        positionControl = false;
         this.voltage = voltage;
     }
 
     public void setPosition(double position) {
-
+        positionControl = true;
         this.positionSetpoint = position;
     }
 

diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
@@ -1,14 +1,29 @@
 package frc.robot.subsystems.elevator;
 
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import org.littletonrobotics.junction.Logger;
 
 public class ElevatorSubsystem extends SubsystemBase {
 
     private ElevatorIO piviotIO;
     private ElevatorIOInputsAutoLogged elevatorInputs = new ElevatorIOInputsAutoLogged();
 
+    private SysIdRoutine elevatorSysIdRoutine =
+            new SysIdRoutine(
+                    new SysIdRoutine.Config(
+                            null,
+                            Volts.of(4),
+                            null,
+                            state ->
+                                    Logger.recordOutput(
+                                            "Elevator/SysIdElevator_State", state.toString())),
+                    new SysIdRoutine.Mechanism(
+                            (voltage) -> piviotIO.setVoltage(voltage.in(Volts)), null, this));
+
     public ElevatorSubsystem(ElevatorIO elevatorIO) {
         this.piviotIO = elevatorIO;
         setDefaultCommand(setPosition(0));
@@ -21,6 +36,14 @@ public void periodic() {
         Logger.processInputs("RealOutputs/Elevator", elevatorInputs);
     }
 
+    public Command runQStaticElevatorSysId(SysIdRoutine.Direction direction) {
+        return elevatorSysIdRoutine.quasistatic(direction);
+    }
+
+    public Command runDynamicElevatorSysId(SysIdRoutine.Direction direction) {
+        return elevatorSysIdRoutine.dynamic(direction);
+    }
+
     public Command zeroElevatorCommand() {
         return runOnce(
                 () -> {