Stalls

src/main/java/frc/robot/Constants.java

@@ -24,6 +24,7 @@ public final class Constants {
     public static class Vision {
         public static final String kRightCameraName = "right_Camera";
         public static final String kLeftCameraName = "left_Camera";
+        public static final String kTestCameraName = "noteCamera";
         // Cam mounted facing forward, half a meter forward of center, half a meter up
         // from center.
         public static final Transform3d kRightRobotToCam = new Transform3d(new Translation3d(-0.16, -0.075, 0.45),

src/main/java/frc/robot/RobotContainer.java

@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.a_robotCommandGroups.AlignTag;
 import frc.robot.a_robotCommandGroups.ReadyShootSpeaker;
 import frc.robot.a_robotCommandGroups.ShootBase;
 import frc.robot.a_robotCommandGroups.ShootSpeaker;
@@ -20,6 +21,7 @@
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.subsystems.takeFeed.TakeFeedSubsystem;
 import frc.util.PathPlanner.PathPlannerHelper;
+import frc.robot.basicCommands.SwerveCommands.TurnToNote;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -34,6 +36,7 @@ public class RobotContainer {
     /* Controllers */
     private final CommandXboxController driver = new CommandXboxController(0);
     private final CommandXboxController operator = new CommandXboxController(1);
+    private final CommandXboxController stalls = new CommandXboxController(2);
 
     // Driver Triggers
     Trigger driverYTrigger = driver.y();
@@ -55,6 +58,11 @@ public class RobotContainer {
     Trigger operatorBTrigger = operator.b();
     Trigger operatorStart = operator.start();
 
+    // stalls Triggers
+    Trigger stallsRightBumperTrigger = stalls.rightBumper();
+    Trigger stallsYTrigger = stalls.y();
+
+
     PathPlannerHelper pathPlannerHelper = PathPlannerHelper.getInstace();
 
     /* Subsystems */
@@ -106,5 +114,10 @@ private void configureButtonBindings() {
         operatorYTrigger.onTrue(new ReadyShootSpeaker());
         operatorBTrigger.onTrue(new InstantCommand(() -> ShooterSubsystem.getInstance().coast(), ShooterSubsystem.getInstance()));
         TakeFeedSubsystem.getInstance().setDefaultCommand(new TakeFeedJoystickSetSpeed(()-> - operator.getHID().getLeftY()));
+
+        //stalls Buttons
+        stallsRightBumperTrigger.whileTrue(new AlignTag());
+        stallsYTrigger.onTrue(new TurnToNote());
+
     }
 }

src/main/java/frc/robot/a_robotCommandGroups/AlignTag.java

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.a_robotCommandGroups;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.basicCommands.ShooterArmCommands.ShooterArmSpeakerAngle;
+import frc.robot.basicCommands.ShooterArmCommands.ShooterArmTag;
+import frc.robot.basicCommands.SwerveCommands.AutoAlignToTagWhileTrue;
+import frc.robot.basicCommands.SwerveCommands.TurnToTag;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class AlignTag extends ParallelCommandGroup {
+  /** Creates a new AlignTag. */
+  public AlignTag() {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(
+      new TurnToTag(),
+      new ShooterArmTag()
+    );
+  }
+}

src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmTag.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.basicCommands.ShooterArmCommands;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.shooterArm.ShooterArmSubsystem;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+
+public class ShooterArmTag extends Command {
+
+  private final ShooterArmSubsystem shooterArm = ShooterArmSubsystem.getInstance();
+  double angle;
+
+  public ShooterArmTag() {
+    addRequirements(shooterArm);
+  }
+
+  @Override
+  public void initialize() {
+    SmartDashboard.putNumber("angle", shooterArm.speakerInterpolate());
+    shooterArm.moveArmTo(shooterArm.speakerInterpolate());}
+
+
+  @Override
+  public void execute() {
+    SmartDashboard.putNumber("angle", shooterArm.speakerInterpolate());
+    shooterArm.moveArmTo(shooterArm.speakerInterpolate());
+  } 
+}

src/main/java/frc/robot/basicCommands/SwerveCommands/AutoAlignToTagWhileTrue.java

@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.basicCommands.SwerveCommands;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.FieldConstants.Speaker;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.subsystems.takeFeed.TakeFeedSubsystem;
+import frc.util.AllianceFlipUtil;
+import frc.util.vision.Vision;
+
+public class AutoAlignToTagWhileTrue extends Command {
+  /** Creates a new TurnToDegree. */
+  SwerveSubsystem swerve = SwerveSubsystem.getInstance();
+
+  private Vision vision = Vision.getInstance();
+  double angleFromTarget = vision.GetAngleFromTarget().getDegrees();
+
+  Rotation2d heading = new Rotation2d();
+
+  public AutoAlignToTagWhileTrue() {
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+
+    Pose2d robot = swerve.getPose();
+
+    double deltaX = Math.abs(robot.getX() - AllianceFlipUtil.apply(Speaker.centerSpeakerOpening.getX()));
+    double deltaY = Speaker.centerSpeakerOpening.getY() - robot.getY();
+    double tempHeading = Units.radiansToDegrees(Math.atan2(deltaY, deltaX));
+    if(DriverStation.getAlliance().get() == Alliance.Red)
+      heading = new Rotation2d(Units.degreesToRadians(tempHeading + 180));
+    else
+      heading = new Rotation2d(Units.degreesToRadians(360-tempHeading));
+
+    SmartDashboard.putNumber("heading", heading.getDegrees());
+    swerve.setHeading(heading);
+    swerve.drive(new Translation2d(),0,true,false);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    Pose2d robot = swerve.getPose();
+
+    double deltaX = Math.abs(robot.getX() - AllianceFlipUtil.apply(Speaker.centerSpeakerOpening.getX()));
+    double deltaY = Speaker.centerSpeakerOpening.getY() - robot.getY();
+    double tempHeading = Units.radiansToDegrees(Math.atan2(deltaY, deltaX));
+    if(DriverStation.getAlliance().get() == Alliance.Red)
+      heading = new Rotation2d(Units.degreesToRadians(tempHeading + 180));
+    else
+      heading = new Rotation2d(Units.degreesToRadians(360-tempHeading));
+
+    SmartDashboard.putNumber("heading", heading.getDegrees());
+    swerve.setHeading(heading);
+    swerve.drive(new Translation2d(),0,true,true);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    swerve.disableHeading();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return swerve.headingPid.atSetpoint();
+  }
+}

src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToNote.java

@@ -0,0 +1,46 @@
+package frc.robot.basicCommands.SwerveCommands;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.subsystems.swerve.SwerveConstants.swerveConstants;
+import frc.util.vision.Vision;
+
+/*
+ * Turn to note is one of our best codes so far. It is written by one of the 
+ * best programmer we have! mister Eilon Nave Huppert.
+ * this code is programmed to detectd notes with the help of our Vision class and turn to position
+ * where our great XXXfeederXXX intake face them.
+ */
+
+public class TurnToNote extends Command {
+  SwerveSubsystem swerve = SwerveSubsystem.getInstance();
+  Vision vision = Vision.getInstance();
+  public PIDController notepid = swerveConstants.notePid;
+
+
+  public TurnToNote() {
+    addRequirements(swerve);
+    notepid.setTolerance(3);
+  }
+
+  @Override
+  public void initialize() {
+  }
+
+  @Override
+  public void execute() {
+    double angelFromNote = vision.GetAngelFromNote();
+    swerve.drive(new Translation2d(),notepid.calculate(angelFromNote,0),true,false);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    swerve.drive(new Translation2d(),0,true,false);
+  }
+
+  @Override
+  public boolean isFinished() {
+     return notepid.atSetpoint();
+  }
+}

src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToTag.java

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.basicCommands.SwerveCommands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.subsystems.swerve.SwerveConstants.swerveConstants;
+import frc.util.vision.Vision;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class TurnToTag extends Command {
+  SwerveSubsystem swerve = SwerveSubsystem.getInstance();
+  Vision vision = Vision.getInstance();
+  public PIDController notepid = swerveConstants.notePid;
+  public TurnToTag() {
+    addRequirements(swerve);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  @Override
+  public void execute() {
+    double angelFromTag = vision.GetAngelFromTag();
+    swerve.drive(new Translation2d(),notepid.calculate(angelFromTag,0),true,false);
+  }
+}

src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java

@@ -26,6 +26,8 @@ public class SwerveConstants {
     public static final class swerveConstants {
         public static final double minimumErrorAligning = 0; 
         public static final PIDController aligningPID = new PIDController(0.14, 0, 0.0);
+        public static final PIDController notePid = new PIDController(0.05, 0, 0);
+
 
         public static final int pigeonID = 11;
         public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW-

src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java

@@ -197,9 +197,9 @@ public void periodic() {
                     var estPose = est.estimatedPose.toPose2d();
                     // Change our trust in the measurement based on the tags we can see
                     var estStdDevs = vision.getEstimationStdDevsRight(estPose);
-
+                    
                     poseEstimation.addVisionMeasurement(
-                            new Pose2d(estPose.getTranslation(), getYaw()), est.timestampSeconds, estStdDevs);
+                            estPose, est.timestampSeconds, estStdDevs);
                 });
 
         vision.getLeftEstimatedGlobalPose().ifPresent(
@@ -209,7 +209,7 @@ public void periodic() {
                     var estStdDevs = vision.getEstimationStdDevsLeft(estPose);
 
                     poseEstimation.addVisionMeasurement(
-                            new Pose2d(estPose.getTranslation(), getYaw()), est.timestampSeconds, estStdDevs);
+                            estPose, est.timestampSeconds, estStdDevs);
                 });
 
         field2d.setRobotPose(poseEstimation.getEstimatedPosition());

src/main/java/frc/util/vision/Vision.java

@@ -41,8 +41,10 @@
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.PhotonUtils;
+import edu.wpi.first.units.Angle;
 
 public class Vision {
+    private final PhotonCamera noteCamera;
     private final PhotonCamera rightCamera;
     private final PhotonPoseEstimator rightPhotonEstimator;
     public double rightLastEstTimestamp = 0;
@@ -62,6 +64,7 @@ public static Vision getInstance() {
     }
 
     private Vision(){
+        noteCamera = new PhotonCamera(kTestCameraName);
         rightCamera = new PhotonCamera(kRightCameraName);
         rightPhotonEstimator =
                 new PhotonPoseEstimator(
@@ -139,7 +142,7 @@ public Matrix<N3, N1> getEstimationStdDevsRight(Pose2d estimatedPose) {
         if (numTags > 1)
             estStdDevs = kRightMultiTagStdDevs;
         // Increase std devs based on (average) distance
-        if (numTags == 1 && avgDist > 7)//TODO: test on field
+        if (numTags == 1 && avgDist > 7)
             estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
         else
             estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
@@ -189,4 +192,28 @@ public Rotation2d GetAngleFromTarget() {
     public Rotation2d GetAngleFromTarget(Pose2d targetPose) {
         return PhotonUtils.getYawToPose(SwerveSubsystem.getInstance().getPose(), targetPose);
     }
+    public double GetAngelFromNote(){
+        var result = noteCamera.getLatestResult();
+        double angle = 0;
+
+        if (result.hasTargets()) { // Check if the camera see the note.
+            angle = result.getBestTarget().getYaw(); // Provides the best angel.
+        }
+        return angle;
+    }
+    public double GetAngelFromTag(){
+        var result = leftCamera.getLatestResult();
+        double angle = 0;
+
+        if (result.hasTargets()) { // Check if the camera see the note.
+            angle = result.getBestTarget().getYaw(); // Provides the best angel.
+        }
+        return angle;
+    }
+
+    // Check if the camera see the note.
+    public boolean seeTarget(){
+        var result = noteCamera.getLatestResult();
+        return result.hasTargets();
+    }
 }