Final photon

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

@@ -79,8 +79,8 @@ public static final class Intake {
 
         public static final class Vision {
             //Target
-            public static final double kYawTarget = -8; //Degrees
-            public static final double kDistanceTarget = 1.8; //Meters
+            public static final double kYawTarget = -9; //Degrees
+            public static final double kDistanceTarget = 1.9; //Meters
             public static final AprilTagFieldLayout kAprilTagFieldLayout = AprilTagFieldLayout
                     .loadField(AprilTagFields.kDefaultField);
 
@@ -93,6 +93,15 @@ public static final class Vision {
                 Units.inchesToMeters(158.84),
                 new Rotation2d(0.0));
 
+            public static final Pose2d blueClimbPos = new Pose2d(
+                    Units.inchesToMeters(163.86),
+                    Units.inchesToMeters(21.75 + 11.46),
+                    new Rotation2d(0.0));
+            public static final Pose2d redClimbPos = new Pose2d(
+                Units.inchesToMeters(649.57),
+                Units.inchesToMeters(152.78 - 11.46),
+                new Rotation2d(0.0));
+
             public static final Transform3d kCameraToRobot = new Transform3d(
                     -0.1016, // forward from robot center
                     0.0, // left/right camera is centered
@@ -105,7 +114,7 @@ public static final class Agitator {
             // Ports
             public static final int kAgitatorPort = 7;//PWM
             // Speeds
-            public static final double kMaxAgitatorSpeed = 1.0; // Agitator Speed = 30%
+            public static final double kMaxAgitatorSpeed = 0.7; // Agitator Speed = 30%
         }
     }
 

src/main/java/frc/robot/Robot.java

@@ -8,6 +8,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj.DriverStation;
 
 /**
@@ -19,6 +21,7 @@
  */
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
+  private Command m_driveForwardCommand;
   private final RobotContainer m_robotContainer;
 
   // Added fields
@@ -82,16 +85,22 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+    m_driveForwardCommand = m_robotContainer.DriveForwardCommand();
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
-      CommandScheduler.getInstance().schedule(m_autonomousCommand);
+      CommandScheduler.getInstance().schedule(Commands.sequence(
+        m_driveForwardCommand, 
+        new WaitCommand(2.0), // Wait 2 Seconds
+        m_autonomousCommand.repeatedly())
+        );
     }
   }
 
   /** This function is called periodically during autonomous. */
   @Override
   public void autonomousPeriodic() {
+
   }
 
   @Override
@@ -102,6 +111,7 @@ public void teleopInit() {
     // this line or comment it out.
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
+      m_driveForwardCommand.cancel();
     }
 
   }

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

@@ -43,8 +43,8 @@ public class RobotContainer {
   private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
   private final AgitatorSubsystem m_AgitatorSubsystem = new AgitatorSubsystem();
   private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
-  private final PhotonVision m_photonVision = new PhotonVision(m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem);
-
+  private final PhotonVision m_photonVision = new PhotonVision(m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem, m_ClimberSubsystem);
+ 
   // Replace with CommandPS4Controller or CommandJoystick if needed
   private final CommandXboxController m_driverController = new CommandXboxController(Driver.kJoystickID);
   private final CommandXboxController m_operatorController = new CommandXboxController(Operator.kJoystickID);
@@ -57,6 +57,7 @@ public class RobotContainer {
    */
   public RobotContainer() {
 
+
     autoChooser = AutoBuilder.buildAutoChooser();
 
     // Configure the trigger bindings
@@ -107,9 +108,8 @@ private double applyDeadbandAndScale(double value) {
 
   // Register named commands for use in PathPlanner autonomous paths
   private void registerNamedCommands() {
-    NamedCommands.registerCommand("Shoot_1", m_ShooterSubsystem.shoot_1_Command());
     NamedCommands.registerCommand("Shooter_ON", m_ShooterSubsystem.StartShoot());
-    NamedCommands.registerCommand("Shooter_OFF", m_ShooterSubsystem.StopShoot());
+    NamedCommands.registerCommand("Shoot_1", m_ShooterSubsystem.StartShoot());
     NamedCommands.registerCommand("Climb_UP", m_ClimberSubsystem.UpClimb());
     NamedCommands.registerCommand("Climb_DOWN", m_ClimberSubsystem.DownClimb());
     NamedCommands.registerCommand("Intake_ON", m_IntakeSubsystem.StartIntake());
@@ -122,12 +122,22 @@ private void configureBindings() {
     // Driver Controls
     // Shooter control
     m_driverController.rightBumper()
-        .whileTrue(m_photonVision.AimShoot());
+        .onTrue(m_photonVision.AimShoot());
 
+    m_operatorController.leftBumper()
+        .onTrue(m_photonVision.AimClimb());
+
     // Start Shooter (constant speed)
     m_driverController.rightTrigger()
-        .whileTrue(Commands.parallel(m_ShooterSubsystem.StartShoot(), m_AgitatorSubsystem.StartAgitator(), m_IntakeSubsystem.StartIntake()));
-      //  .onFalse(Commands.parallel(m_ShooterSubsystem.StopShoot(), m_AgitatorSubsystem.StopAgitator(), m_IntakeSubsystem.StopIntake()));
+        .onTrue(Commands.parallel(m_ShooterSubsystem.StartShoot(), m_AgitatorSubsystem.StartAgitator(), m_IntakeSubsystem.StartIntake()));
+
+
+        // m_operatorController.b()
+        //  .whileTrue(m_photonVision.AimClimb())
+        //  .onFalse(m_photonVision.resetClimb());
+
+m_driverController.rightBumper().negate().and(m_driverController.rightTrigger().negate())
+  .onTrue(Commands.parallel(m_ShooterSubsystem.StopShoot(), m_AgitatorSubsystem.StopAgitator(), m_IntakeSubsystem.StopIntake()));
 
     // Climber control
     m_operatorController.a()
@@ -172,6 +182,10 @@ private void configureBindings() {
 
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    return autoChooser.getSelected();
+    return m_photonVision.AimShoot();
+  }
+    public Command DriveForwardCommand() {
+    // An example command will be run in autonomous
+    return m_driveSubsystem.DriveForward();
   }
 }

src/main/java/frc/robot/commands/Autos.java

@@ -4,14 +4,15 @@
 
 package frc.robot.commands;
 
-import frc.robot.subsystems.AgitatorSubsystem;
+import frc.robot.subsystems.PhotonVision;
+import frc.robot.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 
 public final class Autos {
   /** Example static factory for an autonomous command. */
-  public static Command exampleAuto(AgitatorSubsystem subsystem) {
-    return Commands.sequence(subsystem.StartAgitator(), new ExampleCommand(subsystem));
+  public static Command exampleAuto(PhotonVision vision, DriveSubsystem drive) {
+    return Commands.sequence(vision.AimShoot());
   }
 
   private Autos() {

src/main/java/frc/robot/subsystems/AgitatorSubsystem.java

@@ -61,6 +61,9 @@ public Command StartAgitator() {
   }
 
 
+  public void StartAgitatorVoid() {
+  AgitatorMotor.set(-Constants.Subsystems.Agitator.kMaxAgitatorSpeed);  }
+
 
   public Command StopAgitator() {
     return this.run(() -> {

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -3,13 +3,13 @@
 import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 
-
 // For CAN
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
@@ -24,17 +24,17 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.*;
+import frc.robot.subsystems.PhotonVision;
 
 import java.util.Optional;
 
 import com.ctre.phoenix6.hardware.Pigeon2; //Gyro
 
 public class DriveSubsystem extends SubsystemBase {
-
 
-    private final Field2d field = new Field2d();
 
 
+    private final Field2d field = new Field2d();
 
     private final Pigeon2 pigeon = new Pigeon2(Constants.Subsystems.Drive.kGyroPort);
 
@@ -50,8 +50,8 @@ public class DriveSubsystem extends SubsystemBase {
     private final RelativeEncoder rightEncoder = rightMaster.getEncoder();
     private final RelativeEncoder leftEncoder = leftMaster.getEncoder();
 
-
-
+    boolean back = false;
+    boolean done = false;
     double leftDis = 0;
     double leftPos = 0;
     double rightPos = 0;
@@ -65,27 +65,21 @@ public class DriveSubsystem extends SubsystemBase {
 
     private Pose2d currentPose = new Pose2d();
 
-      private final DifferentialDrivePoseEstimator m_poseEstimator =
-      new DifferentialDrivePoseEstimator(
-          Constants.Subsystems.Drive.kinematics,
-          pigeon.getRotation2d(),
-          leftEncoder.getPosition() / 8.45 * wheelCircumference, 
-          rightEncoder.getPosition() / 8.45 * wheelCircumference,
-          new Pose2d(),
-          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
-          VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
-
-
+    private final DifferentialDrivePoseEstimator m_poseEstimator = new DifferentialDrivePoseEstimator(
+            Constants.Subsystems.Drive.kinematics,
+            pigeon.getRotation2d(),
+            leftEncoder.getPosition() / 8.45 * wheelCircumference,
+            rightEncoder.getPosition() / 8.45 * wheelCircumference,
+            new Pose2d(),
+            VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+            VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
 
     public DriveSubsystem() {
 
 
-                SmartDashboard.putData("Field", field);
-            leftMaster.setInverted(true);
-            leftFollower.setInverted(true);
-
-
-
+        SmartDashboard.putData("Field", field);
+        leftMaster.setInverted(true);
+        leftFollower.setInverted(true);
 
         pigeon.setYaw(0.0);
 
@@ -127,6 +121,33 @@ public void arcadeDrive(double fwd, double rot) {
         // }
     }
 
+
+
+    public Command DriveForward() {
+        leftDis = 0;
+        rightDis = 0;
+        done = false;
+
+        return new RunCommand(() -> {
+            if (leftDis < 0.3 && !back) {
+                drive.arcadeDrive(-1, 0);
+                SmartDashboard.putNumber("Dis", leftDis);
+            } else if (leftDis > -0.3) {
+                  back = true;
+                drive.arcadeDrive(1, 0);
+
+            } else {
+
+                drive.arcadeDrive(0, 0);
+                done = true;
+
+
+            }
+
+        }, this).until(() -> done);
+
+    }
+
     public void tankDrive(double left, double right) {
         drive.tankDrive(left, right);
     }
@@ -162,7 +183,7 @@ public void driveRobotRelative(ChassisSpeeds speeds) {
                 speeds.vxMetersPerSecond,
                 speeds.omegaRadiansPerSecond);
     }
-    
+
     public Command resetPigeon() {
         return this.run(() -> {
 
@@ -172,9 +193,14 @@ public Command resetPigeon() {
 
     }
 
+    // public Command driveForward(){
+    // if ()
+    // // drive.arcadeDrive(-0.4, 0);
+
+    // }
 
     public Pose2d get2dPose() {
-    Pose2d pose = m_poseEstimator.getEstimatedPosition();
+        Pose2d pose = m_poseEstimator.getEstimatedPosition();
         return pose;
     }
 
@@ -185,14 +211,14 @@ public void addVisionMeasurement(Pose2d visionPose, double timestampSeconds) {
     @Override
     public void periodic() {
         // Put periodic subsystem code here (telemetry, safety checks)
-    m_poseEstimator.update(
-        pigeon.getRotation2d(), leftDis, rightDis);
+        m_poseEstimator.update(
+                pigeon.getRotation2d(), leftDis, rightDis);
 
         double leftPosition = leftPos - leftEncoder.getPosition() / 8.45 * wheelCircumference;
         double rightPosition = rightPos - rightEncoder.getPosition() / 8.45 * wheelCircumference;
         leftPos = leftEncoder.getPosition() / 8.45 * wheelCircumference;
         rightPos = rightEncoder.getPosition() / 8.45 * wheelCircumference;
-     
+
         leftDis = leftDis + leftPosition;
         rightDis = rightDis + rightPosition;
         SmartDashboard.putNumber("Dis", rightDis);
@@ -203,8 +229,8 @@ public void periodic() {
 
         SmartDashboard.putNumber("Pigeon Yaw", pigeon.getYaw().getValueAsDouble());
 
-         Pose2d pose = m_poseEstimator.getEstimatedPosition();
+        Pose2d pose = m_poseEstimator.getEstimatedPosition();
 
-         field.setRobotPose(pose);
+        field.setRobotPose(pose);
     }
 }