From 3735e3e5088943ad07b8f5b03eb3a22ac6aa7367 Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Sat, 20 Jul 2024 10:44:50 -0700 Subject: [PATCH] stuff alex told me to push --- .../deploy/pathplanner/autos/Right Forward.auto | 4 ++-- .../java/org/carlmontrobotics/RobotContainer.java | 13 +++++++------ .../carlmontrobotics/commands/AlignToApriltag.java | 11 ++++++++++- .../commands/AutoMATICALLYGetNote.java | 2 +- .../java/org/carlmontrobotics/commands/Intake.java | 2 +- .../carlmontrobotics/commands/TeleopEffector.java | 4 +++- 6 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Right Forward.auto b/src/main/deploy/pathplanner/autos/Right Forward.auto index afbb601a..8976eac3 100644 --- a/src/main/deploy/pathplanner/autos/Right Forward.auto +++ b/src/main/deploy/pathplanner/autos/Right Forward.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.6842044526319189, - "y": 6.6998765551505 + "x": 0.7815863793056181, + "y": 4.450354048988052 }, "rotation": -67.01128319791938 }, diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index abb37f7f..764eb191 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -54,6 +54,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -221,13 +222,13 @@ private void setBindingsManipulator() { .onTrue(new Eject(intakeShooter)); //new JoystickButton(manipulatorController, A_BUTTON) - //.onTrue(new RampMaxRPMDriving(intakeShooter)); - axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( - new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), - new PassToOuttake(intakeShooter))); - + // .onTrue(new RampMaxRPMDriving(intakeShooter)); // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( - // new PassToOuttake(intakeShooter)); + // new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), + // new PassToOuttake(intakeShooter))); + + axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) + .whileTrue(new PassToOuttake(intakeShooter)); new JoystickButton(manipulatorController, RAMP_OUTTAKE) .whileTrue(new RampMaxRPM(intakeShooter)); diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java index 68ed68f5..939f4018 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -21,6 +22,8 @@ public class AlignToApriltag extends Command { public final TeleopDrive teleopDrive; public final Drivetrain drivetrain; private Limelight limelight; + private Timer time = new Timer(); + private double startTime; public final PIDController rotationPID = new PIDController( thetaPIDController[0], thetaPIDController[1], @@ -44,6 +47,11 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight, double Rotati rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); SendableRegistry.addChild(this, rotationPID); + + time.reset(); + time.start(); + startTime = time.getFPGATimestamp(); + addRequirements(drivetrain); } @@ -110,6 +118,7 @@ public boolean isFinished() { // return false; // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); // SmartDashboard.putNumber("Error", rotationPID.getPositionError()); - return rotationPID.atSetpoint(); + return rotationPID.atSetpoint() + || (time.getFPGATimestamp() - startTime) > 3; } } diff --git a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java index e019a042..aabd73d4 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java +++ b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java @@ -62,7 +62,7 @@ public void execute() { forwardDistErrMeters = Math.max( forwardDistErrMeters - * 1.5, + * 1.75, MIN_MOVEMENT_METERSPSEC); if (LimelightHelpers.getTV(INTAKE_LL_NAME)) { diff --git a/src/main/java/org/carlmontrobotics/commands/Intake.java b/src/main/java/org/carlmontrobotics/commands/Intake.java index 5c0b3cea..64db298c 100644 --- a/src/main/java/org/carlmontrobotics/commands/Intake.java +++ b/src/main/java/org/carlmontrobotics/commands/Intake.java @@ -12,7 +12,7 @@ public class Intake extends Command { // intake until sees game peice or 4sec has passed private final IntakeShooter intake; private int index = 0; - private double increaseSpeed = .007; + private double increaseSpeed = .01; private double initSpeed = .5; private double slowSpeed = .1; diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopEffector.java b/src/main/java/org/carlmontrobotics/commands/TeleopEffector.java index 74e4fa21..1be88213 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopEffector.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopEffector.java @@ -59,8 +59,10 @@ public void execute() { } if (intake.intakeDetectsNote()) { - manipulatorController.setRumble(RumbleType.kBothRumble, 0.4); + // manipulatorController.setRumble(RumbleType.kBothRumble, 0.4); driverController.setRumble(RumbleType.kBothRumble, 0.4); + } else if (intake.outtakeDetectsNote()) { + manipulatorController.setRumble(RumbleType.kBothRumble, 0.4); } else { manipulatorController.setRumble(RumbleType.kBothRumble, 0); driverController.setRumble(RumbleType.kBothRumble, 0);