diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 122243f..47a5b8f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,12 +14,16 @@ 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 { @@ -40,6 +44,8 @@ public class RobotContainer { private CommandXboxController driver; private CommandXboxController operator; + private SendableChooser autoChooser; + public RobotContainer() { arm = new Arm(); @@ -58,8 +64,12 @@ public class RobotContainer { driver = new CommandXboxController(OIConstants.kDriverControllerPort); operator = new CommandXboxController(OIConstants.kOperatorControllerPort); + autoChooser = AutoBuilder.buildAutoChooser(); + configureButtonBindings(); + configureNamedCommands(); + configureShuffleboard(); } @@ -86,7 +96,7 @@ public class RobotContainer { ); elevator.setDefaultCommand( - elevator.runElevator(operator::getLeftY) + elevator.runAssistedElevator(operator::getLeftY) ); indexer.setDefaultCommand( @@ -148,10 +158,22 @@ 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) @@ -164,7 +186,7 @@ public class RobotContainer { } public Command getAutonomousCommand() { - return new PrintCommand("NO AUTO DEFINED"); + return autoChooser.getSelected(); } //teleop routines @@ -222,4 +244,14 @@ 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)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 9a99510..0bfbc31 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -1,5 +1,13 @@ 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 { @@ -15,4 +23,21 @@ 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(); + } + } } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 21fab1b..2367a0b 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -43,6 +43,7 @@ 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; diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java index e1d484b..7e22167 100644 --- a/src/main/java/frc/robot/constants/OIConstants.java +++ b/src/main/java/frc/robot/constants/OIConstants.java @@ -6,5 +6,6 @@ public class OIConstants { public static final double kDriveDeadband = 0.05; - public static final String kSensorsTab = "SensorsTab"; + public static final String kAutoTab = "Auto Tab"; + public static final String kSensorsTab = "Sensors Tab"; } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 693d5e4..0e4689e 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -4,9 +4,14 @@ 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; @@ -15,10 +20,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.ADIS16470_IMU; -import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; +import edu.wpi.first.wpilibj.DriverStation; 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; @@ -30,7 +35,7 @@ public class Drivetrain extends SubsystemBase { protected MAXSwerveModule m_rearRight; // The gyro sensor - private ADIS16470_IMU m_gyro; + private AHRS ahrs; // Odometry class for tracking robot pose private SwerveDriveOdometry m_odometry; @@ -61,11 +66,41 @@ public class Drivetrain extends SubsystemBase { DrivetrainConstants.kBackRightChassisAngularOffset ); - m_gyro = new ADIS16470_IMU(); + ahrs = new AHRS(NavXComType.kMXP_SPI); m_odometry = new SwerveDriveOdometry( DrivetrainConstants.kDriveKinematics, - Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), + Rotation2d.fromDegrees(ahrs.getAngle()), + 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 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()), new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), @@ -74,17 +109,21 @@ public class Drivetrain extends SubsystemBase { }); } - @Override - public void periodic() { - // Update the odometry in the periodic block - m_odometry.update( - Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), - new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - }); + 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); } /** @@ -103,7 +142,7 @@ public class Drivetrain extends SubsystemBase { */ public void resetOdometry(Pose2d pose) { m_odometry.resetPosition( - Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), + Rotation2d.fromDegrees(getGyroValue()), new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), @@ -144,7 +183,7 @@ public class Drivetrain extends SubsystemBase { var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, - Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ))) + Rotation2d.fromDegrees(getGyroValue())) : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); @@ -194,7 +233,11 @@ public class Drivetrain extends SubsystemBase { /** Zeroes the heading of the robot. */ public void zeroHeading() { - m_gyro.reset(); + ahrs.reset();; + } + + public double getGyroValue() { + return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1); } /** @@ -203,7 +246,7 @@ public class Drivetrain extends SubsystemBase { * @return the robot's heading in degrees, from -180 to 180 */ public double getHeading() { - return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees(); + return Rotation2d.fromDegrees(getGyroValue()).getDegrees(); } /** @@ -212,6 +255,6 @@ public class Drivetrain extends SubsystemBase { * @return The turn rate of the robot, in degrees per second */ public double getTurnRate() { - return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); + return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 7c7fb7d..4c7f2b3 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -97,8 +97,13 @@ public class Elevator extends SubsystemBase { return motionTarget > ElevatorConstants.kElevatorBracePosition; } - //manual command that keeps ouput speed consistent no matter the direction - public Command runElevator(DoubleSupplier speed) { + /** + * A manual translation command that will move the elevator using a consistent velocity disregarding direction + * + * @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; @@ -108,11 +113,29 @@ public class Elevator extends SubsystemBase { ) + feedForward.calculate(realSpeedTarget); elevatorMotor1.setVoltage(voltsOut); - }).until(bottomLimitSwitch::get); + }).until( + () -> 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( @@ -126,7 +149,21 @@ 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(); + } +} \ No newline at end of file