stuff alex told me to push

DeepBlueRobotics · Jul 20, 2024 · 3735e3e · 3735e3e
1 parent 68502ef
commit 3735e3e
Show file tree

Hide file tree

Showing 6 changed files with 24 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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);