diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 4e327135..2c8dbb83 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -268,6 +268,9 @@ public static final class ShooterConstants{ public static final double SHOOTER_HEIGHT = 0.1; public static final double ANGLE_TICKS_PER_DEGREE = 2491; public static final double DEGREES_PER_ROTATION = 2491; + public static final double DISTANCE_MULTIPLIER = 0; + public static final double OFFSET_MULTIPLIER = 1; + //PID coefficients for shooter: diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index 5085e3d4..11ac5cdb 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java @@ -3,6 +3,10 @@ import frc.robot.settings.Constants.Field; import frc.robot.settings.Constants.ShooterConstants; +import static frc.robot.settings.Constants.ShooterConstants.DISTANCE_MULTIPLIER; +import static frc.robot.settings.Constants.ShooterConstants.OFFSET_MULTIPLIER; +import static frc.robot.settings.Constants.ShooterConstants.ROBOT_ANGLE_TOLERANCE; + import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -69,7 +73,7 @@ public double calculateSpeakerAngleDifference() { double shootingTime = speakerDist / shootingSpeed; // calculates how long the note will take to reach the target double currentXSpeed = DTChassisSpeeds.vxMetersPerSecond; double currentYSpeed = DTChassisSpeeds.vyMetersPerSecond; - Translation2d targetOffset = new Translation2d(currentXSpeed * shootingTime, currentYSpeed * shootingTime); + Translation2d targetOffset = new Translation2d(currentXSpeed * shootingTime*OFFSET_MULTIPLIER, currentYSpeed * shootingTime*OFFSET_MULTIPLIER); // line above calculates how much our current speed will affect the ending // location of the note if it's in the air for ShootingTime @@ -94,7 +98,7 @@ public double calculateSpeakerAngleDifference() { .sqrt(Math.pow(offsetSpeakerdist, 2) + Math.pow(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT, 2)); double desiredShooterAngle = Math .toDegrees(Math.asin(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT / totalDistToSpeaker)); - + desiredShooterAngle = desiredShooterAngle+(speakerDist*DISTANCE_MULTIPLIER); SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle); double differenceAngle = (desiredShooterAngle - this.getShooterAngle()); diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index d7d12826..c6d218c0 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -19,6 +19,7 @@ import static frc.robot.settings.Constants.DriveConstants.FR_DRIVE_MOTOR_ID; import static frc.robot.settings.Constants.DriveConstants.FR_STEER_ENCODER_ID; import static frc.robot.settings.Constants.DriveConstants.FR_STEER_MOTOR_ID; +import static frc.robot.settings.Constants.ShooterConstants.OFFSET_MULTIPLIER; import java.util.Arrays; import java.util.Collections; @@ -323,7 +324,7 @@ public double calculateSpeakerAngleMoving(){ shootingTime = speakerDist/shootingSpeed; //calculates how long the note will take to reach the target currentXSpeed = this.getChassisSpeeds().vxMetersPerSecond; currentYSpeed = this.getChassisSpeeds().vyMetersPerSecond; - targetOffset = new Translation2d(currentXSpeed*shootingTime, currentYSpeed*shootingTime); + targetOffset = new Translation2d(currentXSpeed*shootingTime*OFFSET_MULTIPLIER, currentYSpeed*shootingTime*OFFSET_MULTIPLIER); //line above calculates how much our current speed will affect the ending location of the note if it's in the air for ShootingTime //next 3 lines set where we actually want to aim, given the offset our shooting will have based on our speed