diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..1745ba0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3105fcd..dac8511 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,10 +8,12 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.ResetGyro; import frc.robot.constants.SWERVE; import frc.robot.constants.USB; import frc.robot.generated.TunerConstants; @@ -50,12 +52,17 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { m_swerveDrive.registerTelemetry(m_telemetry::telemeterize); - + initSmartDashboard(); // Configure the trigger bindings initializeSubSystems(); configureBindings(); } + private void initSmartDashboard() { + + SmartDashboard.putData("ResetGyro", new ResetGyro(m_swerveDrive)); + } + private void initializeSubSystems() { m_swerveDrive.setDefaultCommand( // Drivetrain will execute this command periodically diff --git a/src/main/java/frc/robot/commands/ResetGyro.java b/src/main/java/frc/robot/commands/ResetGyro.java new file mode 100644 index 0000000..af0c5d4 --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetGyro.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Controls; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class ResetGyro extends Command { + /** Creates a new ResetGyro. */ + private final CommandSwerveDrivetrain m_swerveDrive; + + public ResetGyro(CommandSwerveDrivetrain swervedrive) { + m_swerveDrive = swervedrive; + // Use addRequirements() here to declare subsystem dependencies. + + addRequirements(m_swerveDrive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if (Controls.isRedAlliance()) { + m_swerveDrive.resetGyro(0); + } else { + m_swerveDrive.resetGyro(180); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index ade87fc..a7563ae 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -13,10 +13,9 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; import frc.robot.constants.CAN; -import frc.robot.constants.SWERVE; import frc.robot.subsystems.CommandSwerveDrivetrain; -// Generated by the Tuner X Swerve Project Generator +// Generated by the Tuner X Swerve Project Generator for Software Bot/AlphaBot as of 1/6/2025 // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. @@ -25,23 +24,17 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(SWERVE.kTurnKP) - .withKI(SWERVE.kTurnKI) - .withKD(SWERVE.kTurnKD) - .withKS(SWERVE.kTurnKS) - .withKV(SWERVE.kTurnKV) - .withKA(SWERVE.kTurnKA) + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.59) + .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = - new Slot0Configs() - .withKP(SWERVE.kDriveKP) - .withKI(SWERVE.kDriveKI) - .withKD(SWERVE.kDriveKD) - .withKS(SWERVE.kDriveKS) - .withKV(SWERVE.kDriveKV) - .withKA(SWERVE.kDriveKA); + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -59,11 +52,11 @@ public class TunerConstants { // The remote sensor feedback type to use for the steer motors; // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder; // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(SWERVE.kSlipCurrent); + private static final Current kSlipCurrent = Amps.of(120.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -75,7 +68,7 @@ public class TunerConstants { // Swerve azimuth does not require much torque output, so we can set a relatively // low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(SWERVE.kTurnSatorCurrentLimit)) + .withStatorCurrentLimit(Amps.of(60)) .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs @@ -83,30 +76,31 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus(CAN.driveBaseCanbus, "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = - MetersPerSecond.of(SWERVE.kMaxSpeedMetersPerSecond); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.96); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = SWERVE.kCoupleRatio; + private static final double kCoupleRatio = 3.125; + + private static final double kDriveGearRatio = 5.357142857142857; + private static final double kSteerGearRatio = 12.8; + private static final Distance kWheelRadius = Inches.of(2); - private static final double kDriveGearRatio = SWERVE.kDriveMotorGearRatio; - private static final double kSteerGearRatio = SWERVE.kTurnMotorGearRatio; - private static final Distance kWheelRadius = Inches.of(SWERVE.kWheelRadiusInches); + private static final boolean kInvertLeftSide = true; + private static final boolean kInvertRightSide = false; - private static final int kPigeonId = CAN.pigeon; + private static final int kPigeonId = 9; // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(SWERVE.kTurnInertia); - private static final MomentOfInertia kDriveInertia = - KilogramSquareMeters.of(SWERVE.kDriveInertia); + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(SWERVE.kTurnFrictionVoltage); - private static final Voltage kDriveFrictionVoltage = Volts.of(SWERVE.kDriveFrictionVoltage); + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() @@ -144,50 +138,45 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = CAN.frontLeftDriveMotor; private static final int kFrontLeftSteerMotorId = CAN.frontLeftTurnMotor; private static final int kFrontLeftEncoderId = CAN.frontLeftCanCoder; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(SWERVE.kFrontLeftEncoderOffset); - private static final boolean kFrontLeftSteerMotorInverted = SWERVE.kTurnInversions[0]; - private static final boolean kFrontLeftDriveMotorInverted = SWERVE.kDriveInversions[0]; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.263916015625); + private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(SWERVE.kFrontLeftPosition.getX()); - private static final Distance kFrontLeftYPos = Inches.of(SWERVE.kFrontLeftPosition.getY()); + private static final Distance kFrontLeftXPos = Inches.of(13.75); + private static final Distance kFrontLeftYPos = Inches.of(13.75); // Front Right private static final int kFrontRightDriveMotorId = CAN.frontRightDriveMotor; private static final int kFrontRightSteerMotorId = CAN.frontRightTurnMotor; private static final int kFrontRightEncoderId = CAN.frontRightCanCoder; - private static final Angle kFrontRightEncoderOffset = - Rotations.of(SWERVE.kFrontRightEncoderOffset); - private static final boolean kFrontRightSteerMotorInverted = SWERVE.kTurnInversions[1]; - private static final boolean kFrontRightDriveMotorInverted = SWERVE.kDriveInversions[1]; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.485595703125); + private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(SWERVE.kFrontRightPosition.getX()); - private static final Distance kFrontRightYPos = Inches.of(SWERVE.kFrontRightPosition.getY()); + private static final Distance kFrontRightXPos = Inches.of(13.75); + private static final Distance kFrontRightYPos = Inches.of(-13.75); // Back Left private static final int kBackLeftDriveMotorId = CAN.backLeftDriveMotor; private static final int kBackLeftSteerMotorId = CAN.backLeftTurnMotor; private static final int kBackLeftEncoderId = CAN.backLeftCanCoder; - private static final Angle kBackLeftEncoderOffset = Rotations.of(SWERVE.kBackLeftEncoderOffset); - private static final boolean kBackLeftSteerMotorInverted = SWERVE.kTurnInversions[2]; - private static final boolean kBackLeftDriveMotorInverted = SWERVE.kDriveInversions[2]; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.0556640625); + private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(SWERVE.kBackLeftPosition.getX()); - private static final Distance kBackLeftYPos = Inches.of(SWERVE.kBackLeftPosition.getY()); + private static final Distance kBackLeftXPos = Inches.of(-13.75); + private static final Distance kBackLeftYPos = Inches.of(13.75); // Back Right private static final int kBackRightDriveMotorId = CAN.backRightDriveMotor; private static final int kBackRightSteerMotorId = CAN.backRightTurnMotor; private static final int kBackRightEncoderId = CAN.backRightCanCoder; - private static final Angle kBackRightEncoderOffset = Rotations.of(SWERVE.kBackRightEncoderOffset); - private static final boolean kBackRightSteerMotorInverted = SWERVE.kTurnInversions[3]; - private static final boolean kBackRightDriveMotorInverted = SWERVE.kDriveInversions[3]; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.201171875); + private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(SWERVE.kBackRightPosition.getX()); - private static final Distance kBackRightYPos = Inches.of(SWERVE.kBackRightPosition.getY()); + private static final Distance kBackRightXPos = Inches.of(-13.75); + private static final Distance kBackRightYPos = Inches.of(-13.75); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> @@ -199,7 +188,7 @@ public class TunerConstants { kFrontLeftEncoderOffset, kFrontLeftXPos, kFrontLeftYPos, - kFrontLeftDriveMotorInverted, + kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted); public static final SwerveModuleConstants< @@ -212,7 +201,7 @@ public class TunerConstants { kFrontRightEncoderOffset, kFrontRightXPos, kFrontRightYPos, - kFrontRightDriveMotorInverted, + kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted); public static final SwerveModuleConstants< @@ -225,7 +214,7 @@ public class TunerConstants { kBackLeftEncoderOffset, kBackLeftXPos, kBackLeftYPos, - kBackLeftDriveMotorInverted, + kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted); public static final SwerveModuleConstants< @@ -238,7 +227,7 @@ public class TunerConstants { kBackRightEncoderOffset, kBackRightXPos, kBackRightYPos, - kBackRightDriveMotorInverted, + kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index e52292d..eac59d7 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -128,7 +128,7 @@ public CommandSwerveDrivetrain( if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); + // configureAutoBuilder(); } /** @@ -150,7 +150,7 @@ public CommandSwerveDrivetrain( if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); + // configureAutoBuilder(); } /** @@ -183,7 +183,7 @@ public CommandSwerveDrivetrain( if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); + // configureAutoBuilder(); } // TODO: Re-implement @@ -252,6 +252,10 @@ public CommandSwerveDrivetrain( // }); // } + public void resetGyro(double angle) { + getPigeon2().setYaw(angle); + } + private void configureAutoBuilder() { try { var config = RobotConfig.fromGUISettings(); diff --git a/src/main/java/frc/robot/subsystems/Controls.java b/src/main/java/frc/robot/subsystems/Controls.java new file mode 100644 index 0000000..28b97f9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Controls.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Controls extends SubsystemBase { + private CommandSwerveDrivetrain m_swervedrive; + private static DriverStation.Alliance m_allianceColor = DriverStation.Alliance.Red; + + /** Creates a new Controls. */ + public Controls() {} + + public static DriverStation.Alliance getAllianceColor() { + return m_allianceColor; + } + + public static boolean isRedAlliance() { + return (m_allianceColor == DriverStation.Alliance.Red); + } + + public static boolean isBlueAlliance() { + return (m_allianceColor == DriverStation.Alliance.Blue); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}