自动瞄准+射球已封装 增加conveyor独立控制

src/main/deploy/pathplanner/autos/8 Balls Auto Middle Start.auto

@@ -7,7 +7,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "redBackAndShoot"
+            "pathName": "Back To Shoot"
           }
         },
         {

src/main/deploy/pathplanner/autos/8 Balls and Climb Middle Start.auto

@@ -7,7 +7,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "redBackAndShoot"
+            "pathName": "Back To Shoot"
           }
         },
         {
@@ -19,13 +19,19 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Climb Path step1"
+            "pathName": "Shoot to Climb Step1"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "Climb Path step2"
+            "pathName": "Climb Step2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Climb Step3"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Copy of Depot Auto.auto

@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Back To Shoot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Shoot to Red Depot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_timelimit"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_timelimit"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_up"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Depot to Shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Depot Auto.auto

@@ -17,7 +17,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "redDepotPath"
+                  "pathName": "To Red Depot"
                 }
               }
             ]
@@ -42,7 +42,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "depotIntakePath"
+                  "pathName": "Depot Intake"
                 }
               }
             ]
@@ -67,7 +67,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "depotShootPath"
+                  "pathName": "Depot to Shoot"
                 }
               }
             ]

src/main/deploy/pathplanner/autos/Line.auto

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Climb Path step2"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": false,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/LineGoAndBack.auto

@@ -1,37 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Climb Path step2"
-          }
-        },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.0
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "LineBack"
-          }
-        },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.0
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": false,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/Rotate.auto

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Rotate"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": false,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/RotateAndLine.auto

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "RotateAndLine"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": false,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/paths/Back To Shoot.path

@@ -4,24 +4,24 @@
     {
       "anchor": {
         "x": 13.01022182786158,
-        "y": 4.119503105590061
+        "y": 4.0
       },
       "prevControl": null,
       "nextControl": {
         "x": 13.865906250551847,
-        "y": 4.119503105590061
+        "y": 4.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 13.59,
+        "x": 13.5,
-        "y": 4.12
+        "y": 4.0
       },
       "prevControl": {
-        "x": 13.321071353115471,
+        "x": 13.231071353115471,
-        "y": 4.12
+        "y": 4.0
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Middle",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0

src/main/deploy/pathplanner/paths/Climb Step2.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 15.650000000000002,
+        "x": 15.610835339716738,
-        "y": 3.365
+        "y": 3.3590426144037364
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 3.365
       },
       "prevControl": {
-        "x": 15.935706120733446,
+        "x": 15.743555432745389,
-        "y": 3.365
+        "y": 3.3550226257587115
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Middle",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0

src/main/deploy/pathplanner/paths/Climb Step3.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 15.467,
+        "y": 3.365
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 15.716589593303949,
+        "y": 3.3506809597259686
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.467,
+        "y": 3.465
+      },
+      "prevControl": {
+        "x": 15.191301517038063,
+        "y": 3.4933990509259307
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.25,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Red Middle",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Depot Intake.path

@@ -45,7 +45,7 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Middle",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0

src/main/deploy/pathplanner/paths/Depot to Shoot.path

@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 16.145956799790877,
+        "x": 16.024211489072027,
-        "y": 1.9416406158462824
+        "y": 1.797481861769637
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 4.0
       },
       "prevControl": {
-        "x": 13.28182215800934,
+        "x": 13.313026105373341,
-        "y": 4.126069541450674
+        "y": 4.165954098256533
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Middle",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0

src/main/deploy/pathplanner/paths/LineBack.path

@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 14.314,
-        "y": 6.668017118402283
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 14.299415121255347,
-        "y": 7.128787446504993
-      },
-      "isLocked": false,
-      "linkedName": "Point1"
-    },
-    {
-      "anchor": {
-        "x": 14.314,
-        "y": 4.15
-      },
-      "prevControl": {
-        "x": 14.299415121255347,
-        "y": 4.597831669044222
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": "StartingPointRed2"
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 180.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 180.0
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/paths/Rotate.path

@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 14.314,
-        "y": 4.15
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 14.064,
-        "y": 4.15
-      },
-      "isLocked": false,
-      "linkedName": "StartingPointRed1"
-    },
-    {
-      "anchor": {
-        "x": 14.314,
-        "y": 4.15
-      },
-      "prevControl": {
-        "x": 14.650181888846735,
-        "y": 4.15
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 180.0
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/paths/RotateAndLine.path

@@ -1,54 +0,0 @@
-{
-  "version": "2025.0",
-  "waypoints": [
-    {
-      "anchor": {
-        "x": 14.314,
-        "y": 4.15
-      },
-      "prevControl": null,
-      "nextControl": {
-        "x": 14.312353780313838,
-        "y": 4.74015691868759
-      },
-      "isLocked": false,
-      "linkedName": "StartingPointRed1"
-    },
-    {
-      "anchor": {
-        "x": 14.314,
-        "y": 6.357489300998574
-      },
-      "prevControl": {
-        "x": 14.312353780313838,
-        "y": 6.098716119828817
-      },
-      "nextControl": null,
-      "isLocked": false,
-      "linkedName": null
-    }
-  ],
-  "rotationTargets": [],
-  "constraintZones": [],
-  "pointTowardsZones": [],
-  "eventMarkers": [],
-  "globalConstraints": {
-    "maxVelocity": 1.5,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 360.0,
-    "maxAngularAcceleration": 720.0,
-    "nominalVoltage": 12.0,
-    "unlimited": false
-  },
-  "goalEndState": {
-    "velocity": 0,
-    "rotation": 0.0
-  },
-  "reversed": false,
-  "folder": null,
-  "idealStartingState": {
-    "velocity": 0,
-    "rotation": 180.0
-  },
-  "useDefaultConstraints": true
-}

src/main/deploy/pathplanner/paths/Shoot to Climb Step1.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 13.59,
+        "x": 13.5,
-        "y": 4.12
+        "y": 4.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.266378040870812,
+        "x": 13.951314803780512,
-        "y": 3.6432882001482505
+        "y": 3.7446544288446453
       },
       "isLocked": true,
       "linkedName": null
@@ -20,12 +20,12 @@
         "y": 3.3650694444444444
       },
       "prevControl": {
-        "x": 14.336543591648573,
+        "x": 14.32971291116739,
-        "y": 3.4619706510920394
+        "y": 3.4437767342805516
       },
       "nextControl": {
-        "x": 14.797456408351428,
+        "x": 14.804287088832611,
-        "y": 3.2681682377968495
+        "y": 3.2863621546083372
       },
       "isLocked": true,
       "linkedName": null
@@ -36,8 +36,8 @@
         "y": 3.365
       },
       "prevControl": {
-        "x": 15.652649792454762,
+        "x": 15.593954129696199,
-        "y": 3.401302545741686
+        "y": 3.387182687738287
       },
       "nextControl": null,
       "isLocked": false,
@@ -61,7 +61,7 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Middle",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0

src/main/deploy/pathplanner/paths/Shoot to Red Depot.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 13.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 13.72461981440462,
+        "y": 3.8902460070118927
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.400452528841061,
+        "y": 2.1
+      },
+      "prevControl": {
+        "x": 15.201612527520673,
+        "y": 2.25153433233069
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Red Middle",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/To Red Depot.path

@@ -45,7 +45,7 @@
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Middle",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0

src/main/deploy/pathplanner/settings.json

@@ -2,7 +2,9 @@
   "robotWidth": 0.85,
   "robotLength": 0.85,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "Red Middle"
+  ],
   "autoFolders": [],
   "defaultMaxVel": 1.5,
   "defaultMaxAccel": 3.0,

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

@@ -149,6 +149,7 @@ public final class Constants {
 
     public static double climbUpTicks = 94; // TODO: 实测后调整
     public static double climbDownTicks = 0; // TODO: 实测后调整
+    public static double climbDownAutoTicks = -5; // TODO: 实测后调整，自动爬时可能不需要完全降到底
     public static final double climbVelocity = -200;
     public static final double holdToleranceRot = 0.5; 
     public static final double deltaDownTicks = 1;

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

@@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -76,7 +77,8 @@ public class RobotContainer {
         climberSubsystem,
         IntakerSubsystem,
         drivetrain,
-        shooterSubsystem
+        shooterSubsystem,
+        driveControls
     );
 
     SignalLogger.enableAutoLogging(false);
@@ -162,7 +164,8 @@ public class RobotContainer {
     controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
     controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
     controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
-    controllerupper.b().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
+    controllerupper.b().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(10), shooterSubsystem));
+    controllerupper.a().whileTrue(new RunCommand(() -> shooterSubsystem.setConveyorSpeedByRPS(-25), shooterSubsystem));
   }
   /* ====================== */
   /*        LED 绑定          */

src/main/java/frc/robot/controls/Autocommand.java

@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.RobotStatusManager;
 import frc.robot.subsystems.IntakerSubsystem;
@@ -26,7 +27,9 @@ import frc.robot.Constants.RobotStatus;
             ClimberSubsystem climber,
             IntakerSubsystem intaker,
             CommandSwerveDrivetrain drivetrain,
-            ShooterSubsystem shooter
+            ShooterSubsystem shooter,
+            DriveControls driveControls,
+            ShooterControls shooterControls
     ) {
         NamedCommands.registerCommand("climb_up", climb_up(climber));
         NamedCommands.registerCommand("climb_down", climb_down(climber));
@@ -37,20 +40,22 @@ import frc.robot.Constants.RobotStatus;
         NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
         NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
         NamedCommands.registerCommand("shoot", shoot(shooter));
+        NamedCommands.registerCommand("auto_aim_and_shoot", autoAimAndShoot(driveControls, shooterControls));
     }
 
         public static Command climb_up(ClimberSubsystem climber) {
             return new RunCommand(() -> {
                 climber.climbUp();  // 控制爬升器上升
-            }, climber).withTimeout(1.0)  // 
+            }, climber).withTimeout(1.0).andThen(new RunCommand(() -> {
-            .andThen(new InstantCommand(() -> {
+                climber.climbUpNonStopforAuto();
+            }, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> {
                 climber.holdPosition();  // 停止并锁定当前爬升器位置
             }, climber));
         }
         public static Command climb_down(ClimberSubsystem climber) {
             return new RunCommand(() -> {
-                climber.climbDown();  // 控制爬升器下降
+                climber.climbDownAuto();  // 控制爬升器下降
-            }, climber).withTimeout(1.0)  //
+            }, climber).withTimeout(2.0)  //
             .andThen(new InstantCommand(() -> {
                 climber.holdPosition();  // 停止并锁定当前爬升器位置
             }, climber));
@@ -125,9 +130,14 @@ import frc.robot.Constants.RobotStatus;
                 //shooter.setBackboardPosition(0);  // 恢复背板位置
                 shooter.setConveyorSpeedByRPS(0);  // 停止输送
             }, shooter)));
-    
-
         }
+        
+        public static Command autoAimAndShoot(DriveControls driveControls, ShooterControls shooterControl) {
+            return driveControls.autoAimCommand()
+            .andThen(shooterControl.autoShootToHubCommand());
+        }
+
+
     }
 
 

src/main/java/frc/robot/controls/DriveControls.java

@@ -12,6 +12,7 @@ import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
 import frc.robot.RobotStatusManager;
@@ -414,6 +415,22 @@ public class DriveControls {
             drivetrain.stop();
         }
     }
+    public Command autoAimCommand() {
+        return drivetrain.applyRequest(() -> {
+        double[] distanceAndRotation = calculateDistanceAndRotationToHub();
+        double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
+        return fieldCentric
+            .withVelocityX(0.0)
+            .withVelocityY(0.0)
+            .withRotationalRate(vomega);
+        }, () -> DriveMode.FIELD_CENTRIC)
+        .until(() -> Math.abs(calculateDistanceAndRotationToHub()[1]) < 1.5)
+        .withTimeout(1.0)
+        .andThen(new InstantCommand(() -> {
+            drivetrain.stop();
+        }, drivetrain));
+    }
+
 
 
 }

src/main/java/frc/robot/controls/ShooterControls.java

@@ -1,9 +1,13 @@
 package frc.robot.controls;
 
+import java.util.Set;
+
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
 import frc.robot.RobotStatusManager;
@@ -147,8 +151,50 @@ public class ShooterControls {
         flywheelSpeedOffset = 0.0;
         backboardPositionOffset = 0.0;
     }
-    public void startconveyor(){
-        shooterSubsystem.setConveyorSpeedByRPS(10);
 
+    public Command autoShootToHubCommand() {
+        return Commands.defer(() -> {
+            double x = shooterSubsystem.getDistanceToHub();
+            x = Math.max(1.2, Math.min(4.5, x));
+
+            double flywheelTargetSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
+            double backboardTargetPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
+            double conveyorTargetSpeed = Constants.Shooter.conveyorSpeed;
+
+            return new RunCommand(() -> {
+                shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
+                shooterSubsystem.setConveyorSpeedByRPS(0.0);
+                shooterSubsystem.setBackboardPosition(backboardTargetPosition);
+                if (shooterSubsystem.isBackboardAtTarget()) {
+                    shooterSubsystem.backboardMotor.set(0);
+                } else {
+                    shooterSubsystem.outputBackboard();
+                }
+            }, shooterSubsystem)
+            .until(() ->
+                shooterSubsystem.isBackboardAtTarget()
+                && shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
+            )
+            .withTimeout(1.0)
+            .andThen(new RunCommand(() -> {
+                shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
+                shooterSubsystem.setBackboardPosition(backboardTargetPosition);
+                if (shooterSubsystem.isBackboardAtTarget()) {
+                    shooterSubsystem.backboardMotor.set(0);
+                } else {
+                    shooterSubsystem.outputBackboard();
+                }
+                if (shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9) {
+                    shooterSubsystem.setConveyorSpeedByRPS(conveyorTargetSpeed);
+                } else {
+                    shooterSubsystem.setConveyorSpeedByRPS(0.0);
+                }
+            }, shooterSubsystem).withTimeout(3.0))
+            .andThen(new InstantCommand(() -> {
+                shooterSubsystem.setFlywheelSpeedByRPS(0.0);
+                shooterSubsystem.setConveyorSpeedByRPS(0.0);
+                shooterSubsystem.backboardMotor.set(0.0);
+            }, shooterSubsystem));
+        }, Set.of(shooterSubsystem));
     }
 }

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

@@ -70,6 +70,10 @@ public class ClimberSubsystem extends SubsystemBase {
         targetPositionTicks = Constants.Climber.climbDownTicks;  
         climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
     }
+    public void climbDownAuto() {
+        targetPositionTicks = Constants.Climber.climbDownAutoTicks;  
+        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
+    }
 
     public void climbDownNonStop(){
         climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity));
@@ -78,6 +82,9 @@ public class ClimberSubsystem extends SubsystemBase {
     public void climbUpNonStop(){
         climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity));
     }
+    public void climbUpNonStopforAuto(){
+        climberMotor.set(0.07);
+    }
 
     public void captureHoldPosition() {
         holdPositionTicks = getPositionTicks();