working shooting

Signed-off-by: a1cd <[email protected]>
DurhamAcademy · Feb 29, 2024 · 611504b · 611504b
1 parent 357aec2
commit 611504b
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -219,6 +219,12 @@ private void configureButtonBindings() {
                         .whileTrue(
                                 IntakeCommands.flushIntake(intake).alongWith(FeederCommands.flushFeeder(feeder))
                         );
+        driverController
+                .leftTrigger()
+                .whileTrue(
+                        IntakeCommands.intakeCommand(intake)
+                                .alongWith(FeederCommands.feedToBeamBreak(feeder)))
+                .onFalse(FeederCommands.feedToBeamBreak(feeder).withTimeout(5));
 
 
         // ---- SHOOTER COMMANDS ----

diff --git a/src/main/java/frc/robot/commands/FeederCommands.java b/src/main/java/frc/robot/commands/FeederCommands.java
@@ -1,6 +1,5 @@
 package frc.robot.commands;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.robot.subsystems.feeder.Feeder;
@@ -21,9 +20,7 @@ public static Command feedToShooter(Feeder feeder) {
 
     public static Command flushFeeder(Feeder feeder){
         return new RunCommand(
-                () -> {
-                    feeder.runVolts(-0.75);
-                },
+                () -> feeder.runVolts(-4),
                 feeder);
     }
 

diff --git a/src/main/java/frc/robot/commands/ShooterCommands.java b/src/main/java/frc/robot/commands/ShooterCommands.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.shooter.Shooter;
+import org.littletonrobotics.junction.Logger;
 
 import java.util.function.DoubleSupplier;
 
@@ -22,10 +23,14 @@ private static double getDistance(Pose3d pose3d) {
     }
 
     private static void populateITM() { //im making separate methods for this because I am not sure how much adjustments you would have to make
-        distanceToAngle.put(0.0, 0.0);
-        distanceToAngle.put(1000.0, 0.0);
-        distanceToRPM.put(0.0, 3000.0);
-        distanceToRPM.put(1000.0, 3000.0);
+        distanceToAngle.put(0.0, .703);
+        distanceToAngle.put(.894, .703);
+        distanceToAngle.put(3.506, .423);
+        distanceToAngle.put(1000.0, .703);
+        distanceToRPM.put(0.0, 3500.0);
+        distanceToRPM.put(0.894, 3500.0);
+        distanceToRPM.put(3.506, 5000.0);
+        distanceToRPM.put(1000.0, 4000.0);
     }
 
     public static Command autoAim(Shooter shooter, Drive drive, DoubleSupplier supplier) {
@@ -35,9 +40,10 @@ public static Command autoAim(Shooter shooter, Drive drive, DoubleSupplier suppl
             Pose3d shooter1 = new Pose3d(drive.getPose()).plus(shooterOffset);
             Pose3d pose3d = speakerPos.relativeTo(shooter1);
             double distance = getDistance(pose3d);
+            Logger.recordOutput("distanceFromGoal", distance);
             double atan = Math.atan(pose3d.getZ() / distance);
-            shooter.setTargetShooterAngle(Rotation2d.fromRadians(atan + supplier.getAsDouble()));
-            shooter.shooterRunVelocity(4500);
+            shooter.setTargetShooterAngle(Rotation2d.fromRadians(distanceToAngle.get(distance)));
+            shooter.shooterRunVelocity(distanceToRPM.get(distance));
         }, shooter).handleInterrupt(() -> {
             shooter.shooterRunVelocity(0.0);
         });

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -45,7 +45,7 @@ public Intake(IntakeIO io) {
                     7.0,
                 0.0,
                 0.0,
-                    new Constraints(RadiansPerSecond.of(1), RadiansPerSecond.per(Second).of(1.5)));
+                    new Constraints(RadiansPerSecond.of(4), RadiansPerSecond.per(Second).of(2)));
         break;
       case REPLAY:
         armFF = new ArmFeedforward(0.1, .15, 1.95);