src/main/java/frc/robot/RobotContainer.java

@@ -14,16 +14,12 @@ import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Manipulator;
-
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
-
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotContainer {
@@ -44,8 +40,6 @@ public class RobotContainer {
 	private CommandXboxController driver;
 	private CommandXboxController operator;
 
-	private SendableChooser<Command> autoChooser;
-
 	public RobotContainer() {
 		arm = new Arm();
 
@@ -64,12 +58,8 @@ public class RobotContainer {
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
-		autoChooser = AutoBuilder.buildAutoChooser();
-
 		configureButtonBindings();
 
-		configureNamedCommands();
-
 		configureShuffleboard();
 	}
 
@@ -96,7 +86,7 @@ public class RobotContainer {
 		);
 
 		elevator.setDefaultCommand(
-			elevator.runAssistedElevator(operator::getLeftY)
+			elevator.runElevator(operator::getLeftY)
 		);
 
 		indexer.setDefaultCommand(
@@ -158,22 +148,10 @@ public class RobotContainer {
 		);
 	}
 
-	private void configureNamedCommands() {
-		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
-	}
-
 	//creates tabs and transforms them on the shuffleboard
     private void configureShuffleboard() {
-		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
     	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
 
-		Shuffleboard.selectTab(OIConstants.kAutoTab);
-
-		autoTab.add("Auto Selection", autoChooser)
-			.withSize(2, 1)
-			.withPosition(0, 0)
-			.withWidget(BuiltInWidgets.kComboBoxChooser);
-
 		sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(0, 0)
@@ -186,7 +164,7 @@ public class RobotContainer {
   }
 
 	public Command getAutonomousCommand() {
-		return autoChooser.getSelected();
+		return new PrintCommand("NO AUTO DEFINED");
 	}
 
 	//teleop routines
@@ -244,14 +222,4 @@ public class RobotContainer {
 			() -> elevatorFirst
 		);
 	}
-
+}
-	/*
-	 * A moveManipulator method that will guarantee a safe movement. 
-	 * Here in case we need want to skip moveManipulator debugging
-	 */
-	@SuppressWarnings("unused")
-	private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
-		return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
-			.andThen(arm.goToSetpoint(armPosition, 2));
-	}
-}

src/main/java/frc/robot/constants/AutoConstants.java

@@ -1,13 +1,5 @@
 package frc.robot.constants;
 
-import java.io.IOException;
-
-import org.json.simple.parser.ParseException;
-
-import com.pathplanner.lib.config.PIDConstants;
-import com.pathplanner.lib.config.RobotConfig;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
-
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
@@ -23,21 +15,4 @@ public class AutoConstants {
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
-
-    // TODO This is a constant being managed like a static rewriteable variable
-    public static RobotConfig kRobotConfig;
-
-    public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
-        new PIDConstants(kPXController, 0, 0),
-        new PIDConstants(kPYController, 0, 0)
-    );
-
-    static {
-        try {
-            kRobotConfig = RobotConfig.fromGUISettings();
-        } catch (IOException | ParseException e) {
-            System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
-            e.printStackTrace();
-        }
-    }
 }

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -43,7 +43,6 @@ public class ElevatorConstants {
     public static final double kElevatorL2AlgaePosition = 0;
     public static final double kElevatorL3AlgaePosition = 0;
     public static final double kElevatorBracePosition = 0;
-    public static final double kElevatorMaxHeight = 0;
 
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = 1;

src/main/java/frc/robot/constants/OIConstants.java

@@ -6,6 +6,5 @@ public class OIConstants {
     
     public static final double kDriveDeadband = 0.05;
 
-    public static final String kAutoTab = "Auto Tab";
+    public static final String kSensorsTab = "SensorsTab";
-    public static final String kSensorsTab = "Sensors Tab";
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -4,14 +4,9 @@
 
 package frc.robot.subsystems;
 
-import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.studica.frc.AHRS;
-import com.studica.frc.AHRS.NavXComType;
-
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -20,10 +15,10 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
 
@@ -35,7 +30,7 @@ public class Drivetrain extends SubsystemBase {
     protected MAXSwerveModule m_rearRight;
 
     // The gyro sensor
-    private AHRS ahrs;
+    private ADIS16470_IMU m_gyro;
 
     // Odometry class for tracking robot pose
     private SwerveDriveOdometry m_odometry;
@@ -66,41 +61,24 @@ public class Drivetrain extends SubsystemBase {
             DrivetrainConstants.kBackRightChassisAngularOffset
         );
 
-        ahrs = new AHRS(NavXComType.kMXP_SPI);
+        m_gyro = new ADIS16470_IMU();
 
         m_odometry = new SwerveDriveOdometry(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(ahrs.getAngle()),
+            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
         });
-
-        AutoBuilder.configure(
-            this::getPose, 
-            this::resetOdometry, 
-            this::getCurrentChassisSpeeds, 
-            this::driveWithChassisSpeeds, 
-            AutoConstants.kPPDriveController, 
-            AutoConstants.kRobotConfig,
-            () -> {
-                Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
-                    if (alliance.isPresent()) {
-                        return alliance.get() == DriverStation.Alliance.Red;
-                    }
-                    return false;
-            }, 
-            this
-        );
     }
 
     @Override
     public void periodic() {
         // Update the odometry in the periodic block
         m_odometry.update(
-            Rotation2d.fromDegrees(getGyroValue()),
+            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -109,23 +87,6 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
-    public ChassisSpeeds getCurrentChassisSpeeds() {
-        return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
-            m_frontLeft.getState(),
-            m_frontRight.getState(),
-            m_rearLeft.getState(),
-            m_rearRight.getState()
-        );
-    }
-
-    public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
-        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
-        SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
-        SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
-
-        setModuleStates(newStates);
-    }
-
     /**
      * Returns the currently-estimated pose of the robot.
      *
@@ -142,7 +103,7 @@ public class Drivetrain extends SubsystemBase {
      */
     public void resetOdometry(Pose2d pose) {
         m_odometry.resetPosition(
-            Rotation2d.fromDegrees(getGyroValue()),
+            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -183,7 +144,7 @@ public class Drivetrain extends SubsystemBase {
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative
                 ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
-                        Rotation2d.fromDegrees(getGyroValue()))
+                        Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
@@ -233,11 +194,7 @@ public class Drivetrain extends SubsystemBase {
 
     /** Zeroes the heading of the robot. */
     public void zeroHeading() {
-        ahrs.reset();;
+        m_gyro.reset();
-    }
-
-    public double getGyroValue() {
-        return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
     }
 
     /**
@@ -246,7 +203,7 @@ public class Drivetrain extends SubsystemBase {
      * @return the robot's heading in degrees, from -180 to 180
      */
     public double getHeading() {
-        return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
+        return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
     }
 
     /**
@@ -255,6 +212,6 @@ public class Drivetrain extends SubsystemBase {
      * @return The turn rate of the robot, in degrees per second
      */
     public double getTurnRate() {
-        return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
+        return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 }

src/main/java/frc/robot/subsystems/Elevator.java

@@ -97,13 +97,8 @@ public class Elevator extends SubsystemBase {
         return motionTarget > ElevatorConstants.kElevatorBracePosition;
     }
 
-    /**
+    //manual command that keeps ouput speed consistent no matter the direction
-     * A manual translation command that will move the elevator using a consistent velocity disregarding direction
+    public Command runElevator(DoubleSupplier speed) {
-     * 
-     * @param speed How fast the elevator moves
-     * @return Sets motor voltage to move the elevator relative to the speed parameter
-     */
-    public Command runAssistedElevator(DoubleSupplier speed) {
         return run(() -> {
             double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
 
@@ -113,29 +108,11 @@ public class Elevator extends SubsystemBase {
             ) + feedForward.calculate(realSpeedTarget);
 
             elevatorMotor1.setVoltage(voltsOut);
-        }).until(
+        }).until(bottomLimitSwitch::get);
-            () -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight);
     }
 
-    /**
-     * A manual translation command that uses feed forward calculation to maintain position
-     * 
-     * @param speed The speed at which the elevator translates
-     * @return Sets motor voltage to translate the elevator and maintain position
-     */
-    public Command runManualElevator(double speed) {
-        return run(() -> {
-            elevatorMotor1.set(speed);
-        });
-    }
     
-    /**
+    //go to setpoint command
-     * Moves the elevator to a target destination (setpoint)
-     * 
-     * @param setpoint Target destination of the subsystem
-     * @param timeout Time to achieve the setpoint before quitting
-     * @return Sets motor voltage to achieve the target destination
-     */
     public Command goToSetpoint(double setpoint, double timeout) {
         return run(() -> {
             double voltsOut = positionController.calculate(
@@ -149,21 +126,7 @@ public class Elevator extends SubsystemBase {
         ).withTimeout(timeout);
     }
 
-    /**
-     * Returns the current encoder position
-     * 
-     * @return Current encoder position
-     */
     public double getEncoderPosition() {
         return encoder.getPosition();
     }
-
+}
-    /**
-     * Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
-     * 
-     * @return The value of bottomLimitSwitch
-     */
-    public boolean getBottomLimitSwitch() {
-        return bottomLimitSwitch.get();
-    }
-}