diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 51e39a21..01f225c1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -422,7 +422,7 @@ public Robot() { : new FlywheelIOSim( TurretSubsystem.getFlywheelConfig(), canivore, - TurretSubsystem.FLYWHEEL_GEAR_RATIO, + TurretSubsystem.NEW_FLYWHEEL_GEAR_RATIO, 11, 12), ROBOT_MODE == RobotMode.REAL diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index a4d3d043..29b29fd5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -59,7 +59,9 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { /** Creates a new TurretSubsystem. */ public static final double HOOD_GEAR_RATIO = 33.8671875; // 58.96875; - public static final double FLYWHEEL_GEAR_RATIO = 20.0 / 18.0; // 0.84615384615; + public static final double OLD_FLYWHEEL_GEAR_RATIO = 20.0 / 18.0; // 0.84615384615; + + public static final double NEW_FLYWHEEL_GEAR_RATIO = 25.0 / 12.0; public static final Rotation2d HOOD_MAX_ANGLE = Rotation2d.fromDegrees(56); public static final Rotation2d HOOD_MIN_ANGLE = Rotation2d.fromDegrees(11.33); @@ -508,12 +510,12 @@ public static TalonFXConfiguration getFlywheelConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = TurretSubsystem.NEW_FLYWHEEL_GEAR_RATIO; // slot 0 is for velocity - config.Slot0.kS = 0.33706; // 0.63933; - config.Slot0.kV = 0.13893; // 0.11582; - config.Slot0.kA = 0.030026; // 0.020809; + config.Slot0.kS = 0.17257; // 0.33706; // 0.63933; + config.Slot0.kV = 0.25853; // 0.13893; // 0.11582; + config.Slot0.kA = 0.031279; // 0.030026; // 0.020809; config.Slot0.kP = 0.67; config.Slot0.kD = 0; diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java index d932f8da..714aa7b0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/comp/R1WispSwerveConstants.java @@ -204,7 +204,7 @@ public Pigeon2Configuration getGyroConfig() { config.MountPose.MountPoseYaw = -175.72914123535156; // -90.44647216796875; // -90.93168640136719; // -86.66709899902344; - config.GyroTrim.GyroScalarZ = -4;//-2.5; + config.GyroTrim.GyroScalarZ = -4; // -2.5; return config; } diff --git a/src/main/java/frc/robot/utils/autoaim/ShotTrees.java b/src/main/java/frc/robot/utils/autoaim/ShotTrees.java index a20026e8..13fcc7a3 100644 --- a/src/main/java/frc/robot/utils/autoaim/ShotTrees.java +++ b/src/main/java/frc/robot/utils/autoaim/ShotTrees.java @@ -42,81 +42,81 @@ public class ShotTrees { 1.716849, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (30 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 0.8)); COMP_HUB_SHOT_TREE.put( 2.017596, new ShotData( Rotation2d.fromDegrees(23 - 13.16), - 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (33 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 0.9)); COMP_HUB_SHOT_TREE.put( 2.423868, new ShotData( Rotation2d.fromDegrees(25 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (35 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 1.1)); COMP_HUB_SHOT_TREE.put( 2.664198, new ShotData( Rotation2d.fromDegrees(26 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (36 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 1.2)); COMP_HUB_SHOT_TREE.put( 2.903207, new ShotData( Rotation2d.fromDegrees(30 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (35 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 1.2)); COMP_HUB_SHOT_TREE.put( 3.156802, new ShotData( Rotation2d.fromDegrees(32 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (35 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 1.23)); COMP_HUB_SHOT_TREE.put( 3.437033, new ShotData( Rotation2d.fromDegrees(34 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (35 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 1.25)); COMP_HUB_SHOT_TREE.put( 3.611052, new ShotData( Rotation2d.fromDegrees(38 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (34 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 1.24)); COMP_HUB_SHOT_TREE.put( 3.773999, new ShotData( Rotation2d.fromDegrees(39 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (34 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 1.21)); COMP_HUB_SHOT_TREE.put( 3.899275, new ShotData( Rotation2d.fromDegrees(40 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (34 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 1.2)); COMP_HUB_SHOT_TREE.put( 4.138058, new ShotData( Rotation2d.fromDegrees(41 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.95 * (34 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1), 1.13)); // budget shots - COMP_HUB_SHOT_TREE.put(4.172776, new ShotData(Rotation2d.fromDegrees(36), 25.5, 1.13)); + COMP_HUB_SHOT_TREE.put(4.172776, new ShotData(Rotation2d.fromDegrees(36), 0.95 * 25.5, 1.13)); - COMP_HUB_SHOT_TREE.put(4.512483, new ShotData(Rotation2d.fromDegrees(34), 25.5, 1.2)); + COMP_HUB_SHOT_TREE.put(4.512483, new ShotData(Rotation2d.fromDegrees(34), 0.95 * 25.5, 1.2)); - COMP_HUB_SHOT_TREE.put(4.974656, new ShotData(Rotation2d.fromDegrees(39), 27, 1.2)); + COMP_HUB_SHOT_TREE.put(4.974656, new ShotData(Rotation2d.fromDegrees(39), 0.95 * 27, 1.2)); - COMP_HUB_SHOT_TREE.put(5.116029, new ShotData(Rotation2d.fromDegrees(39.5), 27.5, 1.1)); + COMP_HUB_SHOT_TREE.put(5.116029, new ShotData(Rotation2d.fromDegrees(39.5), 0.95 * 27.5, 1.1)); - COMP_HUB_SHOT_TREE.put(5.509574, new ShotData(Rotation2d.fromDegrees(40), 28, 1.12)); + COMP_HUB_SHOT_TREE.put(5.509574, new ShotData(Rotation2d.fromDegrees(40), 0.95 * 28, 1.12)); - COMP_HUB_SHOT_TREE.put(5.63499, new ShotData(Rotation2d.fromDegrees(40), 28, 1.12)); + COMP_HUB_SHOT_TREE.put(5.63499, new ShotData(Rotation2d.fromDegrees(40), 0.95 * 28, 1.12)); // COMP_HUB_SHOT_TREE.put( // 4.602258, @@ -175,80 +175,80 @@ public class ShotTrees { Units.feetToMeters(18), new ShotData( Rotation2d.fromDegrees(40 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 36 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.42)); // - 2, 1.42)); FEED_SHOT_TREE.put( Units.feetToMeters(20), new ShotData( Rotation2d.fromDegrees(43 - 13.16), - 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 38 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.36)); // - 2, 1.36)); FEED_SHOT_TREE.put( Units.feetToMeters(22), new ShotData( Rotation2d.fromDegrees(45 - 13.16), - 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 39 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.34)); // - 2, 1.34)); FEED_SHOT_TREE.put( Units.feetToMeters(24), new ShotData( Rotation2d.fromDegrees(47 - 13.16), - 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 40 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.25)); // - 2, 1.25)); FEED_SHOT_TREE.put( Units.feetToMeters(26), new ShotData( Rotation2d.fromDegrees(48 - 13.16), - 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 41 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.28)); // - 2, 1.28)); FEED_SHOT_TREE.put( Units.feetToMeters(28), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 43 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.27)); // - 2, 1.27)); FEED_SHOT_TREE.put( Units.feetToMeters(30), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 44 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.32)); // - 2, 1.32)); FEED_SHOT_TREE.put( Units.feetToMeters(32), new ShotData( Rotation2d.fromDegrees(49 - 13.16), - 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 46 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.4)); // - 2, 1.4)); FEED_SHOT_TREE.put( Units.feetToMeters(34), new ShotData( Rotation2d.fromDegrees(52 - 13.16), - 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 47 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(36), new ShotData( Rotation2d.fromDegrees(53 - 13.16), - 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 51 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.33)); // - 2, 1.33)); FEED_SHOT_TREE.put( Units.feetToMeters(38), new ShotData( Rotation2d.fromDegrees(53 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 55 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(40), new ShotData( Rotation2d.fromDegrees(55 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 55 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.2)); // - 2, 1.2)); FEED_SHOT_TREE.put( Units.feetToMeters(42), new ShotData( Rotation2d.fromDegrees(56 - 13.16), - 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 57 * 0.84615384615 / TurretSubsystem.OLD_FLYWHEEL_GEAR_RATIO + 1, 1.2)); // - 2, 1.2)); }