diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de5ee637..24b8acf2 100644 --- 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 index ad2d171a..08ad3282 100644 --- 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 index 2f9f77ff..20862af1 100644 --- 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 index 5b893b41..73af99ce 100644 --- 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);