diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index afe262c..66ce319 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,7 +31,7 @@ public final class Constants { public static final double loopPeriodSecs = 0.02; - public static final Mode currentMode = Mode.SIM; + public static final Mode currentMode = Mode.REAL; public static enum Mode { /** Running on a real robot. */ @@ -57,8 +57,8 @@ public static class Drive { public static final int backRightDrive = 7; public static final int backRightTurn = 8; - public static final boolean frontLeftDriveInvert = true; - public static final boolean frontRightDriveInvert = false; + public static final boolean frontLeftDriveInvert = false; + public static final boolean frontRightDriveInvert = true; public static final boolean backLeftDriveInvert = false; // true? public static final boolean backRightDriveInvert = true; // false? @@ -72,7 +72,7 @@ public static class Drive { public static final int backLeftEncoder = 1; public static final int backRightEncoder = 0; - public static final double frontLeftOffset = -0.5107599496841431; + public static final double frontLeftOffset = 0.807; public static final double frontRightOffset = -0.5107599496841431; public static final double backLeftOffset = 1.8345500230789185; public static final double backRightOffset = 2.437732458114624; @@ -126,8 +126,8 @@ public static class DriveConstants { public static final double odometeryFrequency = 250; public static final double updateFrequency = 100; - // public static final double maxLinearVelocity = Units.feetToMeters(20.4); - public static final double maxLinearVelocity = Units.feetToMeters(1.4); + public static final double maxLinearVelocity = Units.feetToMeters(20.4); + // public static final double maxLinearVelocity = Units.feetToMeters(1.4); public static final double maxLinearAccel = 8.0; public static final double maxAngularVelocity = 10; @@ -139,7 +139,7 @@ public static class DriveConstants { public static double kVDriveReal = 1.93; public static double kADriveReal = 0.25; - public static double kPTurnReal = 1.5; // 1.5? + public static double kPTurnReal = 3; // 1.5? public static double kDTurnReal = 0.0; public static double kPDriveSim = 0.3; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 95c0ec3..12612dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -177,6 +177,8 @@ private void configureButtonBindings() { // right trigger -> climb up m_driver.rightTrigger(0.1).onTrue(m_climber.setDutyCycle(1)).onFalse(m_climber.setDutyCycle(0)); + m_driver.y().whileTrue(m_drive.zeroGyro()); + // Operator Controller // D-Pad Up for intake down, rollers forward, until note in feeder beambreak @@ -251,7 +253,7 @@ private void configureButtonBindings() { .a() .whileTrue( Commands.parallel( - m_pivot.setPivotTarget(() -> Units.degreesToRadians(34)), + m_pivot.setPivotTarget(() -> Units.degreesToRadians(40)), m_shooter.setRPM(() -> -1500, 1.0), m_feeder.setRPM(() -> -1500), m_intake.setRollerRPM(() -> -1000)) @@ -282,7 +284,7 @@ public void robotPeriodic() { } private static double teleopAxisAdjustment(double x) { - return MathUtil.applyDeadband(Math.abs(Math.pow(x, 2)) * Math.signum(x), 0.02); + return MathUtil.applyDeadband(x, 0.02); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b8f5cc2..ff8755b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -278,6 +278,13 @@ public Rotation2d getRotation() { return gyroIO.getYaw(); } + public Command zeroGyro() { + return this.run( + () -> { + gyroIO.setYaw(0); + }); + } + @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java index 47ca959..5fc8620 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java @@ -73,7 +73,7 @@ public class ModuleIOReal implements ModuleIO { private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; private final StatusSignal driveCurrent; - // private int multiplier; + private int multiplier; private final VelocityVoltage driveCurrentVelocity = new VelocityVoltage(0.0).withEnableFOC(false); @@ -93,7 +93,7 @@ public ModuleIOReal(int index) { absoluteEncoderOffset = new Rotation2d(RobotMap.Drive.frontLeftOffset); // MUST BE CALIBRATED name = "FrontLeft"; - // multiplier = -1; + multiplier = 1; break; case 1: driveTalon = new TalonFX(RobotMap.Drive.frontRightDrive); @@ -104,7 +104,7 @@ public ModuleIOReal(int index) { absoluteEncoderOffset = new Rotation2d(RobotMap.Drive.frontRightOffset); // MUST BE CALIBRATED name = "FrontRight"; - // multiplier = 1; + multiplier = 1; break; case 2: driveTalon = new TalonFX(RobotMap.Drive.backLeftDrive); @@ -114,7 +114,7 @@ public ModuleIOReal(int index) { absoluteEncoder = new AnalogEncoder(RobotMap.Drive.backLeftEncoder); absoluteEncoderOffset = new Rotation2d(RobotMap.Drive.backLeftOffset); // MUST BE CALIBRATED name = "BackLeft"; - // multiplier = 1; + multiplier = 1; break; case 3: driveTalon = new TalonFX(RobotMap.Drive.backRightDrive); @@ -125,7 +125,7 @@ public ModuleIOReal(int index) { absoluteEncoderOffset = new Rotation2d(RobotMap.Drive.backRightOffset); // MUST BE CALIBRATED name = "BackRight"; - // multiplier = 1; + multiplier = 1; break; default: throw new RuntimeException("Invalid module index"); @@ -163,10 +163,7 @@ public ModuleIOReal(int index) { turnRelativeEncoder = turnSparkMax.getEncoder(); - turnRelativeEncoder.setPositionConversionFactor(DriveConstants.turnConversion); absoluteEncoder.setDistancePerRotation(2 * Math.PI); - - turnRelativeEncoder.setVelocityConversionFactor(DriveConstants.turnConversion * 60); turnPID = turnSparkMax.getPIDController(); turnSparkMax.restoreFactoryDefaults(); @@ -292,10 +289,11 @@ public void runDriveVelocitySetpoint( && MathUtil.isNear(0.0, driveVelocity.getValueAsDouble(), 0.1)) { runDriveVoltage(0.0); } else { - driveTalon.setControl( - driveCurrentVelocity - .withVelocity(metersPerSecond) - .withFeedForward(metersPerSecondSquared * driveConfig.Slot0.kA)); + runDriveVoltage( + Math.signum(metersPerSecond) + * Math.pow((metersPerSecond / DriveConstants.maxLinearVelocity), 2) + * 12.0 + * multiplier); } }