button binding back

Author: 111010022-ai

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

@@ -6,7 +6,6 @@
 import choreo.auto.AutoTrajectory;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -143,43 +142,6 @@ public AutoRoutine simpleShoot() {
     return routine;
   }
 
-  public AutoRoutine middleShootOnly() {
-    AutoRoutine routine = autoFactory.newRoutine("middleShootOnly");
-
-    routine
-        .active()
-        .onTrue(
-            Commands.parallel(
-                Commands.sequence(
-                    Commands.waitSeconds(1),
-                    shooter.rawFlywheelCMD(() -> 0.25),
-                    Commands.waitUntil(
-                        () -> {
-                          return shooter.atSpeed();
-                        }),
-                    // Commands.runOnce(drive::stopWithX),//maybe
-                    hopper.shootCMD())));
-
-    return routine;
-  }
-
-  public AutoRoutine simplerShoot() {
-    AutoRoutine routine = autoFactory.newRoutine("");
-
-    routine
-        .active()
-        .onTrue(
-            Commands.sequence(
-                Commands.deadline(
-                    Commands.waitSeconds(12.5),
-                    Commands.parallel(
-                        shooter.rawFlywheelCMD(() -> 3.2),
-                        Commands.sequence(Commands.waitSeconds(1), hopper.shootCMD()))),
-                Commands.parallel(Commands.waitSeconds(1), shooter.stopCMD(), hopper.stopCMD())));
-
-    return routine;
-  }
-
   public AutoRoutine depotAndClimb(boolean addClimb) {
     AutoRoutine routine = autoFactory.newRoutine("DepotAndClimb");
 
@@ -190,9 +152,8 @@ public AutoRoutine depotAndClimb(boolean addClimb) {
         .active()
         .onTrue(
             Commands.sequence(
-                sendState("Auto Started!"),
-                DriveCommands.autoAlign(drive, collect.getInitialPose().orElse(drive.getPose())),
                 collect.resetOdometry(),
+                sendState("Auto Started!"),
                 Commands.parallel(collect.cmd(), deploy.deployCMD())));
 
     collect.atTime("intake").onTrue(Commands.sequence(intake.intakeCMD(), sendState("Intaking!")));
@@ -209,11 +170,12 @@ public AutoRoutine depotAndClimb(boolean addClimb) {
 
     if (addClimb) {
       collect
-          .doneDelayed(10)
+          .doneDelayed(8)
           .onTrue(
               Commands.parallel(
                   sendState("Aligning!"),
-                  DriveCommands.autoClimb(drive, climber),
+                  climb.cmd(),
+                  climber.raiseCMD(),
                   hopper.stopCMD(),
                   shooter.stopCMD(),
                   deploy.undeployCMD()));
@@ -274,18 +236,4 @@ public AutoRoutine midClimbLeft() {
   public AutoRoutine midClimbRight() {
     return null;
   }
-
-  public Command theOnlyAuto() {
-    return Commands.parallel(
-        Commands.sequence(
-            Commands.waitSeconds(1),
-            shooter.rawFlywheelCMD(() -> 0.25),
-            Commands.waitUntil(
-                () -> {
-                  DriverStation.reportWarning("theonlyauto", false);
-                  return shooter.atSpeed();
-                }),
-            // Commands.runOnce(drive::stopWithX),//maybe
-            hopper.shootCMD()));
-  }
 }

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

@@ -38,7 +38,7 @@ public static final class DriveConstants {
     public static final double SLOWMODE = 0.6;
     public static final double POINT_DEADBAND = 0.4;
     public static final Pose2d POSE_RESET =
-        new Pose2d(Meters.of(3.61), Meters.of(3.87), Rotation2d.kZero);//CHANGED
+        new Pose2d(Meters.of(2), Meters.of(2), Rotation2d.kZero);
     public static final int GYRO_ID = 0; // TODO
     public static final Angle ALIGN_SHOOTER_COMP =
         Radians.of(
@@ -66,10 +66,10 @@ public static final class VisionConstants {
         new Transform3d(Units.inchesToMeters(12), 0, 0.5, new Rotation3d(0.0, 0.0, 0.0));
     public static Transform3d robotToCamera1 =
         new Transform3d(
-            -0.132,
-            -0.248,
-            0.0513 + 0.115 + 0.0375,
-            new Rotation3d(0.0, Units.degreesToRadians(-25), Math.PI / 2));
+            Units.inchesToMeters(7),
+            Units.inchesToMeters(12),
+            0.1,
+            new Rotation3d(0.0, -0.6, Math.PI / 2));
     public static Transform3d robotToCamera2 =
         new Transform3d(
             Units.inchesToMeters(7.5),
@@ -129,19 +129,18 @@ public final class HopperConstants {
   }
 
   public static class IntakeConstants {
-    public static final int SPINTAKEID = 42;
-    public static final int SPINTAKEID_KRAKEN = 43;
-    public static final int DEPLOYID = 41;
+    public static final int SPINTAKEID = 41;
+    public static final int DEPLOYID = 42;
 
-    public static final double INTAKE_SPEED = 0.5;
-    public static final double SPITAKE_SPEED = -0.5;
+    public static final double INTAKE_SPEED = 1;
+    public static final double SPITAKE_SPEED = -1;
     public static final double DEPLOY_SPEED = 1;
-    public static final Angle DEPLOY_POSITION = Degrees.of(70);
+    public static final Angle DEPLOY_POSITION = Degrees.of(70); 
     public static final Angle READY_POSITION = Degrees.of(40);
     public static final Angle UP_POSITION = Degrees.of(0);
     public static final Angle DEPLOY_TOLERANCE = Degrees.of(5);
 
-    public static final double kP = 1; // 1.33;
+    public static final double kP = 1;//1.33;
     public static final double kI = 0;
     public static final double kD = 7.84;
     public static final double DEPLOY_RATIO = 25;

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

@@ -30,7 +30,6 @@ public class Robot extends LoggedRobot {
   Timer gcTimer = new Timer();
 
   public Robot() {
-    // super(0.04);
     gcTimer.start();
     // Record metadata
     Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
@@ -50,8 +49,8 @@ public Robot() {
     switch (Constants.currentMode) {
       case REAL:
         // Running on a real robot, log to a USB stick ("/U/logs")
-        // Logger.addDataReceiver(new WPILOGWriter());
-        // Logger.addDataReceiver(new NT4Publisher());
+        Logger.addDataReceiver(new WPILOGWriter());
+        Logger.addDataReceiver(new NT4Publisher());
         break;
 
       case SIM: