added multipliers to change our target angles for the speaker proport…

…ionally based on our distance and speed. by default they do nothing
2491-NoMythic · Feb 3, 2024 · 8cc35cd · 8cc35cd
1 parent b4b0e1a
commit 8cc35cd
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 3 deletions.
diff --git 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
@@ -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
@@ -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