im stupid

201-991-Broncobotics · Feb 27, 2024 · 8899a1a · 8899a1a
1 parent 61dec5d
commit 8899a1a
Show file tree

Hide file tree

Showing 8 changed files with 101 additions and 39 deletions.
diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto
@@ -2,25 +2,25 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 2.0,
-      "y": 7.0
+      "x": 1.0362868269501009,
+      "y": 7.068721083188694
     },
-    "rotation": 180.0
+    "rotation": 0.14258439223781336
   },
   "command": {
     "type": "sequential",
     "data": {
       "commands": [
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "Run Flywheel"
+            "pathName": "Example Path"
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Example Path"
+            "name": "SmartShoot"
           }
         }
       ]

diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
@@ -3,41 +3,41 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.0,
-        "y": 7.0
+        "x": 1.0362868269501009,
+        "y": 7.068721083188694
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.013282910175676,
-        "y": 6.5274984191074985
+        "x": 2.299762772087753,
+        "y": 6.983958234481863
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.166769560390973,
-        "y": 5.0964860911203305
+        "x": 2.9348040006843905,
+        "y": 5.5225949841367274
       },
       "prevControl": {
-        "x": 4.166769560390973,
-        "y": 6.0964860911203305
+        "x": 1.5206207320861609,
+        "y": 5.513338482809104
       },
       "nextControl": {
-        "x": 6.166769560390973,
-        "y": 4.0964860911203305
+        "x": 3.8390146654425257,
+        "y": 5.528513472500037
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 7.0,
-        "y": 1.0
+        "x": 5.183512243471246,
+        "y": 6.245063007523296
       },
       "prevControl": {
-        "x": 6.75,
-        "y": 2.5
+        "x": 4.267503572984577,
+        "y": 5.864973759895436
       },
       "nextControl": null,
       "isLocked": false,
@@ -46,7 +46,42 @@
   ],
   "rotationTargets": [],
   "constraintZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [
+    {
+      "name": "Shoot",
+      "waypointRelativePos": 0.0,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "SmartShoot"
+              }
+            }
+          ]
+        }
+      }
+    },
+    {
+      "name": "Intake",
+      "waypointRelativePos": 0.75,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "SmartIntake"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
@@ -55,7 +90,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0,
+    "rotation": 13.361566187350306,
     "rotateFast": false
   },
   "reversed": false,

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -14,6 +14,7 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -22,9 +23,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.commands.DriveCommands;
-import frc.robot.commands.FeedForwardCharacterization;
-import frc.robot.commands.PivotCommands;
+import frc.robot.commands.*;
 import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.hang.*;
 import frc.robot.subsystems.intake.*;
@@ -131,6 +130,7 @@ public RobotContainer() {
 
     // Configure the button bindings
     configureButtonBindings();
+    configureNamedCommands();
   }
 
   /**
@@ -150,7 +150,7 @@ private void configureButtonBindings() {
     var stopWithXButton = new Trigger(() -> driver.getRawButton(2));
     stopWithXButton.onTrue(Commands.runOnce(drive::stopWithX, drive));
 
-    operator.a().toggleOnTrue(Commands.runOnce(intake::on, intake));
+    operator.a().toggleOnTrue(new SmartIntake(shooter, intake));
     operator.b().toggleOnTrue(Commands.runOnce(intake::off, intake));
     operator.rightBumper().toggleOnTrue(Commands.runOnce(shooter::shootersOn, shooter));
     operator.leftBumper().toggleOnTrue(Commands.runOnce(shooter::shootersOff, shooter));
@@ -165,6 +165,17 @@ private void configureButtonBindings() {
       operator.getRightTriggerAxis() - operator.getLeftTriggerAxis()));
   }
 
+  private void configureNamedCommands() {
+    NamedCommands.registerCommand("SmartIntake", new SmartIntake(shooter, intake));
+    NamedCommands.registerCommand("PivotToIntake", PivotCommands.goToIntake(pivot));
+    // todo: make this like SmartIntake
+    NamedCommands.registerCommand("SmartShoot", Commands.startEnd(
+      () -> {shooter.shootersOn(); shooter.conveyorOn();}, 
+      () -> {shooter.shootersOff(); shooter.conveyorOff();},
+      shooter
+    ).withTimeout(1.0));
+  }
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *

diff --git a/src/main/java/frc/robot/commands/PivotCommands.java b/src/main/java/frc/robot/commands/PivotCommands.java
@@ -4,6 +4,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.PivotConstants;
 import frc.robot.subsystems.pivot.Pivot;
 import java.util.function.DoubleSupplier;
 
@@ -21,4 +22,22 @@ public static Command basicOperatorControl(Pivot pivot, DoubleSupplier rotationS
         },
         pivot);
   }
+
+  public static Command goToPosition(Pivot pivot, Rotation2d position) {
+    return new Command() {
+      @Override
+      public void initialize() {
+        pivot.setTargetPosition(position);
+      }
+
+      @Override
+      public boolean isFinished() {
+        return pivot.getCurrentPosition().minus(position).getDegrees() < 1;
+      }
+    };
+  }
+
+  public static Command goToIntake(Pivot pivot) {
+    return goToPosition(pivot, PivotConstants.intakePosition);
+  }
 }
diff --git a/...a/frc/robot/commands/IntakeToShooter.java → .../java/frc/robot/commands/SmartIntake.java b/...a/frc/robot/commands/IntakeToShooter.java → .../java/frc/robot/commands/SmartIntake.java
@@ -3,13 +3,10 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.IntakeConstants;
-import frc.robot.Constants.PivotConstants;
 import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.pivot.Pivot;
 import frc.robot.subsystems.shooter.Shooter;
 
-public class IntakeToShooter extends Command {
-  Pivot pivot;
+public class SmartIntake extends Command {
   Shooter shooter;
   Intake intake;
   // basically, the way we want this command to work is when the intake shoots to high voltage, then goes back down, we know we're done intaking. so, we keep track of the intake current on the previous periodic run
@@ -18,8 +15,7 @@ public class IntakeToShooter extends Command {
   double endTime;
 
 
-  public IntakeToShooter(Pivot pivot, Shooter shooter, Intake intake) {
-    this.pivot = pivot;
+  public SmartIntake(Shooter shooter, Intake intake) {
     this.shooter = shooter;
     this.intake = intake;
   }
@@ -35,7 +31,6 @@ public void execute() {
 
   @Override
   public void initialize() {
-    pivot.setTargetPosition(PivotConstants.intakePosition);
     intake.on();
     shooter.conveyorOn();
     // not sure if we need this here, but whatever

diff --git a/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/pivot/Pivot.java
@@ -37,6 +37,10 @@ public Rotation2d getCurrentPosition() {
     return inputs.pivotCurentAngle;
   }
 
+  public Rotation2d getTargetPosition() {
+    return inputs.pivotTargetAngle;
+  }
+
   @Override
   public void periodic() {
     io.updateInputs(inputs);

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -4,8 +4,6 @@
 
 import org.littletonrobotics.junction.Logger;
 
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Shooter extends SubsystemBase {

diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
   "fileName": "PathplannerLib.json",
   "name": "PathplannerLib",
-  "version": "2024.1.2",
+  "version": "2024.2.3",
   "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
   "frcYear": "2024",
   "mavenUrls": [
@@ -12,15 +12,15 @@
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-java",
-      "version": "2024.1.2"
+      "version": "2024.2.3"
     }
   ],
   "jniDependencies": [],
   "cppDependencies": [
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-cpp",
-      "version": "2024.1.2",
+      "version": "2024.2.3",
       "libName": "PathplannerLib",
       "headerClassifier": "headers",
       "sharedLibrary": false,