diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 87933eb..c03fa0e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,8 +42,8 @@ public static class CAN { public static class Speeds { public static final double SWERVE_DRIVE_COEFF = 0.3; - public static final double SWERVE_DRIVE_MAX_SPEED = 2.88; // TODO: Get this value - public static final double SWERVE_DRIVE_MAX_ANGULAR_SPEED = Math.PI; // Todo: Get this value + public static final double SWERVE_DRIVE_MAX_SPEED = 8.78; // TODO: Get this value + public static final double SWERVE_DRIVE_MAX_ANGULAR_SPEED = Math.PI * 2; // Todo: Get this value } public static class Physical { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8c52be5..4b43a96 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,11 +73,9 @@ public RobotContainer() { // to configure button bindings private void configureBindings() { _driveController.R1().onTrue(new ToggleSwerveOrient(_swerveDrive)); - _driveController.L1().onTrue(new ResetGyro(_swerveDrive)); - _driveController.cross().onTrue(new ResetPose(_swerveDrive)); + _driveController.square().onTrue(new ResetPose(_swerveDrive)); _driveController.circle().whileTrue(new Shooter(_shooterSubsystem)); - - _driveController.square().whileTrue(new BrakeSwerve(_swerveDrive, 0)); + _driveController.cross().whileTrue(new BrakeSwerve(_swerveDrive, 0)); } /** @return The Command to schedule for auton. */ diff --git a/src/main/java/frc/robot/commands/swerve/TeleopDrive.java b/src/main/java/frc/robot/commands/swerve/TeleopDrive.java index 368214f..302641b 100644 --- a/src/main/java/frc/robot/commands/swerve/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/swerve/TeleopDrive.java @@ -53,8 +53,8 @@ public void execute() { // drive the swerve chassis subsystem _swerveDrive.driveChassis(new ChassisSpeeds( - xSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED, - ySpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED, + xSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, + ySpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, rotationSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED )); } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 7f93845..b7dbc88 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -168,8 +168,10 @@ public void periodic() { _field.setRobotPose(_pose); SmartDashboard.putData("FIELD", _field); - _robotSpeed = Math.sqrt(Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2) + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2)); - SmartDashboard.putNumber("DRIVE SPEED (m/s)", _robotSpeed); + _robotSpeed = Math.sqrt(Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2) + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2)); + + SmartDashboard.putNumber("ANGULAR SPEED", getRobotRelativeSpeeds().omegaRadiansPerSecond / Math.PI); + SmartDashboard.putNumber("DRIVE SPEED (m/s)", _robotSpeed); } /** diff --git a/src/main/java/frc/robot/utils/SwerveModule.java b/src/main/java/frc/robot/utils/SwerveModule.java index 2c7fb98..105790b 100644 --- a/src/main/java/frc/robot/utils/SwerveModule.java +++ b/src/main/java/frc/robot/utils/SwerveModule.java @@ -110,7 +110,7 @@ public void setState(SwerveModuleState state) { double rotation_volts = -MathUtil.clamp(_rotationController.calculate(getAngle(), state.angle.getDegrees()), -1.5, 1.5); double drive_pid = _driveController.calculate(getDriveVelocity(), speed); - double drive_output = (speed / Constants.Speeds.SWERVE_DRIVE_MAX_SPEED) * Constants.Speeds.SWERVE_DRIVE_COEFF; + double drive_output = (speed / Constants.Speeds.SWERVE_DRIVE_MAX_SPEED); drive_output += drive_pid; rotate(rotation_volts / RobotController.getBatteryVoltage()); diff --git a/src/main/java/frc/robot/utils/TalonFXConfig.java b/src/main/java/frc/robot/utils/TalonFXConfig.java index f80937e..1c66b2e 100644 --- a/src/main/java/frc/robot/utils/TalonFXConfig.java +++ b/src/main/java/frc/robot/utils/TalonFXConfig.java @@ -37,7 +37,7 @@ public static TalonFXConfiguration configureFalcon(TalonFX falcon, boolean inver config.MotorOutput.DutyCycleNeutralDeadband = 0.01; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = invert ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;