From 716dd022e75fd1037e64a1a435a9bc165c843926 Mon Sep 17 00:00:00 2001 From: 3729 Drive Station Date: Sat, 2 Mar 2024 18:07:21 -0700 Subject: [PATCH] plug --- src/main/java/frc/robot/Autos.java | 9 +++++++++ src/main/java/frc/robot/Constants.java | 17 +++++++++-------- .../frc/robot/commands/ScoringCommands.java | 2 +- 3 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 4132b53..afc783e 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -105,6 +105,15 @@ public Autos( autoChooser.addOption("drive qr", driveSubsystem.driveQuasistaticSysIDCommand(Direction.kReverse)); autoChooser.addOption("drive df", driveSubsystem.driveDynamicSysIDCommand(Direction.kForward)); autoChooser.addOption("drive dr", driveSubsystem.driveDynamicSysIDCommand(Direction.kReverse)); + + autoChooser.addOption("Shooter qf", shooterSubsystem.sysIdQuasistatic(Direction.kForward)); + autoChooser.addOption("Shooter qr", shooterSubsystem.sysIdQuasistatic(Direction.kReverse)); + autoChooser.addOption("Shooter df", shooterSubsystem.sysIdDynamic(Direction.kForward)); + autoChooser.addOption("Shooter dr", shooterSubsystem.sysIdDynamic(Direction.kReverse)); + + + + } public SendableChooser getAutoChooser() { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b7028f6..0bca89e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Unit; import frc.robot.telemetry.tunable.gains.TunableArmElevatorFFGains; import frc.robot.telemetry.tunable.gains.TunableFFGains; import frc.robot.telemetry.tunable.gains.TunablePIDGains; @@ -184,20 +185,20 @@ private SwerveConstants() {} STEER_POSITION_PID_GAINS, STEER_VELOCITY_FF_GAINS); - public static final SwerveModuleConfiguration FRONT_LEFT_MODULE_CONFIGURATION = - new SwerveModuleConfiguration( - 12, 8, 17, true, false, 2.43, true, SHARED_SWERVE_MODULE_CONFIGURATION); public static final SwerveModuleConfiguration FRONT_RIGHT_MODULE_CONFIGURATION = new SwerveModuleConfiguration( - 13, 5, 18, true, false, 0.34, true, SHARED_SWERVE_MODULE_CONFIGURATION); - - public static final SwerveModuleConfiguration BACK_LEFT_MODULE_CONFIGURATION = + 12, 8, 17, true, false, 2.462, true, SHARED_SWERVE_MODULE_CONFIGURATION); + public static final SwerveModuleConfiguration FRONT_LEFT_MODULE_CONFIGURATION = new SwerveModuleConfiguration( - 14, 10, 19, true, false, 0.54, true, SHARED_SWERVE_MODULE_CONFIGURATION); + 13, 5, 18, false, false, 0.3497+Units.degreesToRadians(180), true, SHARED_SWERVE_MODULE_CONFIGURATION); public static final SwerveModuleConfiguration BACK_RIGHT_MODULE_CONFIGURATION = new SwerveModuleConfiguration( - 15, 3, 20, true, false, 0.73, true, SHARED_SWERVE_MODULE_CONFIGURATION); + 14, 10, 19, true, false, 0.486, true, SHARED_SWERVE_MODULE_CONFIGURATION); + + public static final SwerveModuleConfiguration BACK_LEFT_MODULE_CONFIGURATION = + new SwerveModuleConfiguration( + 15, 3, 20, true,false, 0.712, true, SHARED_SWERVE_MODULE_CONFIGURATION); } public static class AutoConstants { diff --git a/src/main/java/frc/robot/commands/ScoringCommands.java b/src/main/java/frc/robot/commands/ScoringCommands.java index 71ae257..4f1636c 100644 --- a/src/main/java/frc/robot/commands/ScoringCommands.java +++ b/src/main/java/frc/robot/commands/ScoringCommands.java @@ -16,7 +16,7 @@ public static Command shooterInToleranceCommand(ShooterSubsystem shooterSubsyste public static Command shootSetpointCloseSpeakerCommand(ShooterSubsystem shooterSubsystem) { // return shooterSubsystem.runVelocityCommand(Units.rotationsPerMinuteToRadiansPerSecond(10000.0)); - return shooterSubsystem.setVoltageCommand(10); + return shooterSubsystem.runVelocityCommand(1000.0); }