Revert "added SysID"

This reverts commit 3aa63e4.
DeepBlueRobotics · Mar 2, 2024 · 65c7640 · 65c7640
1 parent 3aa63e4
commit 65c7640
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 68 deletions.
diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -16,7 +16,7 @@
 
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController.Axis;
-import edu.wpi.first.wpilibj.XboxController.Button;
+
 //commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -27,7 +27,6 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
 public class RobotContainer {
   //defaultCommands: elevator, dt
@@ -61,10 +60,6 @@ private void setDefaultCommands() {
     arm.setDefaultCommand(new ArmTeleop(arm, () -> inputProcessing(getStickValue(manipulatorController, Axis.kLeftY))));
   }
   private void setBindingsDriver() {
-    new JoystickButton(driverController, Button.kY.value).whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    new JoystickButton(driverController, Button.kA.value).whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-    new JoystickButton(driverController, Button.kB.value).whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    new JoystickButton(driverController, Button.kX.value).whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kReverse));
     // 4 cardinal directions on arrowpad
 		// slowmode toggle on trigger
 		// 3 cardinal directions on letterpad

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -1,10 +1,6 @@
 package org.carlmontrobotics.subsystems;
 
 import static org.carlmontrobotics.Constants.Arm.*;
-import static edu.wpi.first.units.MutableMeasure.mutable;
-import static edu.wpi.first.units.Units.Rotations;
-import static edu.wpi.first.units.Units.RotationsPerSecond;
-import static edu.wpi.first.units.Units.Volts;
 
 import org.carlmontrobotics.commands.ArmTeleop;
 import org.carlmontrobotics.lib199.MotorConfig;
@@ -24,19 +20,12 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.Angle;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.MutableMeasure;
-import edu.wpi.first.units.Velocity;
-import edu.wpi.first.units.Voltage;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
 // TODO: FIGURE OUT ANGLES
 // Arm angle is measured from horizontal on the intake side of the robot and bounded between __ and __
@@ -60,23 +49,13 @@ public class Arm extends SubsystemBase {
     private static TrapezoidProfile.State goalState;
     private static TrapezoidProfile.State setpointState;
 
-    // SysID
-    private final MutableMeasure<Voltage> voltage = mutable(Volts.of(0));
-    private final MutableMeasure<Velocity<Angle>> velocity = mutable(RotationsPerSecond.of(0));
-    private final MutableMeasure<Angle> distance = mutable(Rotations.of(0));
-
-    private final static boolean runningSysID = true;
-
     public Arm() {
         masterArmMotor.setInverted(motorInverted[MASTER]);
         masterArmMotor.setIdleMode(IdleMode.kBrake);
         followArmMotor.setInverted(motorInverted[FOLLOWER]);
         followArmMotor.setIdleMode(IdleMode.kBrake);
-        // comment out when running sysid
-        if (!runningSysID) {
-            armEncoder.setPositionConversionFactor(rotationToRad);
-            armEncoder.setVelocityConversionFactor(rotationToRad);
-        }
+        armEncoder.setPositionConversionFactor(rotationToRad);
+        armEncoder.setVelocityConversionFactor(rotationToRad);
         armEncoder.setInverted(encoderInverted);
         followArmMotor.follow(masterArmMotor);
 
@@ -100,12 +79,10 @@ public Arm() {
 
     @Override
     public void periodic() {
-        if (!runningSysID) {
-            if (DriverStation.isDisabled())
-                resetGoal();
-            driveArm();
-            autoCancelArmCommand();
-        }
+        if (DriverStation.isDisabled())
+            resetGoal();
+        driveArm();
+        autoCancelArmCommand();
     }
 
     public void autoCancelArmCommand() {
@@ -184,37 +161,4 @@ public double getArmClampedGoal(double goal) {
         return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONTINUITY_RAD, ARM_DISCONTINUITY_RAD + 2 * Math.PI),
                 ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD);
     }
-
-    // SysID
-    private final SysIdRoutine routine = new SysIdRoutine(
-            new SysIdRoutine.Config(),
-            new SysIdRoutine.Mechanism(
-                    this::driveMotor,
-                    this::logMotor,
-                    this));
-
-    private void driveMotor(Measure<Voltage> volts) {
-        masterArmMotor.setVoltage(volts.in(Volts));
-    }
-
-    private void logMotor(SysIdRoutineLog log) {
-        log.motor("arm-motor")
-                .voltage(voltage.mut_replace(
-                        masterArmMotor.getBusVoltage() * masterArmMotor.getAppliedOutput(),
-                        Volts))
-                .angularVelocity(velocity.mut_replace(
-                        armEncoder.getVelocity() / 60,
-                        RotationsPerSecond))
-                .angularPosition(distance.mut_replace(
-                        masterArmMotor.getEncoder().getPosition(),
-                        Rotations));
-    }
-
-    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
-        return routine.quasistatic(direction);
-    }
-
-    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
-        return routine.dynamic(direction);
-    }
 }