implemented variable angle shooter, untuned

awtybots · Apr 4, 2024 · b5175aa · b5175aa
1 parent 6736f56
commit b5175aa
Show file tree

Hide file tree

Showing 27 changed files with 159 additions and 89 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -44,6 +44,7 @@
     "Autons",
     "bezier",
     "deadband",
+    "Desmos",
     "Feedforward",
     "Holonomic",
     "keybinds",

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -183,18 +183,6 @@ public RobotContainer() {
 
     NamedCommands.registerCommand("ShootFarPosition", ShootFar.run(sArm).withTimeout(2.0));
 
-    // NamedCommands.registerCommand(
-    //     "FloorPickupPosition", new FloorPickupCommand(sArm).withTimeout(2.0));
-
-    // NamedCommands.registerCommand(
-    //     "ShootClosePosition", new ShootCloseCommand(sArm).withTimeout(2.0));
-
-    // NamedCommands.registerCommand(
-    //     "ShootMediumPosition", new ShootMediumCommand(sArm).withTimeout(2.0));
-
-    // NamedCommands.registerCommand("ShootFarPosition", new
-    // ShootFarCommand(sArm).withTimeout(2.0));
-
     NamedCommands.registerCommand(
         "PreRunShooter", new PreRunShooter(sFlywheel, sIntake).withTimeout(4.0));
 
@@ -210,9 +198,8 @@ public RobotContainer() {
     NamedCommands.registerCommand( // The name is inaccurate
         "ShootMediumGroup", new ShootNote(sIntake, sFlywheel, sArm).withTimeout(3.0));
 
-    // In testing
-    NamedCommands.registerCommand(
-        "SpeakerShot", SpeakerShot.run(1, sArm).withTimeout(5.0)); // TODO Replace SpeakerDistance
+    // TEST
+    NamedCommands.registerCommand("SpeakerShot", SpeakerShot.run(sArm, sDrive).withTimeout(5.0));
 
     SendableChooser<Command> chooser = new SendableChooser<>();
 

diff --git a/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java b/src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java
@@ -1,4 +1,4 @@
-// Copyright 2016-2024 FRC 6328, FRC 5829
+// Copyright 2016-2024 FRC 6328, FRC 5172, FRC 5829
 //
 // This program is free software; you can redistribute it and/or
 // modify it under the terms of the GNU General Public License

diff --git a/src/main/java/frc/robot/commands/Positions/AmpShot.java b/src/main/java/frc/robot/commands/Positions/AmpShot.java
@@ -15,21 +15,23 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class AmpShot {
 
   public static Command run(Arm arm) {
     return Commands.run(
         () -> {
           // Position preset settings
-          double ARMANGLE = 2.093;
+          double ArmAngle = 2.093;
 
-          arm.runTargetAngle(ARMANGLE);
+          arm.runTargetAngle(Optional.of(ArmAngle));
         },
         arm);
   }
 }
-//  TODO check to add the `.until(
+//  TODO check if .until needed:
+//          .until(
 //             () -> {
 //               return arm.getIsFinished();
 //             });`
diff --git a/src/main/java/frc/robot/commands/Positions/AmpShotCommand.java b/src/main/java/frc/robot/commands/Positions/AmpShotCommand.java
@@ -14,12 +14,13 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class AmpShotCommand extends Command {
 
   private Arm arm;
 
-  double ARMANGLE = 2.093;
+  double ArmAngle = 2.093;
 
   public AmpShotCommand(Arm arm) {
     this.arm = arm;
@@ -33,7 +34,7 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    arm.runTargetAngle(ARMANGLE);
+    arm.runTargetAngle(Optional.of(ArmAngle));
   }
 
   // Called once the command ends or is interrupted.
@@ -42,6 +43,6 @@ public void end(boolean interrupted) {}
 
   @Override
   public boolean isFinished() {
-    return arm.getIsFinished();
+    return arm.isRoughlyAtSetpoint();
   }
 }
diff --git a/src/main/java/frc/robot/commands/Positions/Climb.java b/src/main/java/frc/robot/commands/Positions/Climb.java
@@ -15,21 +15,23 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class Climb {
 
   public static Command run(Arm arm) {
     return Commands.run(
         () -> {
           // Position preset settings
-          double ARMANGLE = 2.53;
+          double ArmAngle = 2.53;
 
-          arm.runTargetAngle(ARMANGLE);
+          arm.runTargetAngle(Optional.of(ArmAngle));
         },
         arm);
   }
 }
-//  TODO check to add the `.until(
+//  TODO check if .until needed:
+//          .until(
 //             () -> {
 //               return arm.getIsFinished();
 //             });`
diff --git a/src/main/java/frc/robot/commands/Positions/FloorPickup.java b/src/main/java/frc/robot/commands/Positions/FloorPickup.java
@@ -15,21 +15,22 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class FloorPickup {
 
   public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ARMANGLE = 0.09;
+              double ArmAngle = 0.09;
 
-              arm.runTargetAngle(ARMANGLE);
+              arm.runTargetAngle(Optional.of(ArmAngle));
             },
             arm)
         .until(
             () -> {
-              return arm.getIsFinished();
+              return arm.isRoughlyAtSetpoint();
             });
   }
 }
diff --git a/src/main/java/frc/robot/commands/Positions/FloorPickupCommand.java b/src/main/java/frc/robot/commands/Positions/FloorPickupCommand.java
@@ -14,12 +14,13 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class FloorPickupCommand extends Command {
 
   private Arm arm;
 
-  double ARMANGLE = 0.135;
+  double ArmAngle = 0.135;
 
   public FloorPickupCommand(Arm arm) {
     this.arm = arm;
@@ -33,7 +34,7 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    arm.runTargetAngle(ARMANGLE);
+    arm.runTargetAngle(Optional.of(ArmAngle));
   }
 
   // Called once the command ends or is interrupted.
@@ -42,6 +43,6 @@ public void end(boolean interrupted) {}
 
   @Override
   public boolean isFinished() {
-    return arm.getIsFinished();
+    return arm.isRoughlyAtSetpoint();
   }
 }
diff --git a/src/main/java/frc/robot/commands/Positions/ShootClose.java b/src/main/java/frc/robot/commands/Positions/ShootClose.java
@@ -15,21 +15,22 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class ShootClose { // bumper pressed against the wall
 
   public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ARMANGLE = 0.747;
+              double ArmAngle = 0.747;
 
-              arm.runTargetAngle(ARMANGLE);
+              arm.runTargetAngle(Optional.of(ArmAngle));
             },
             arm)
         .until(
             () -> {
-              return arm.getIsFinished();
+              return arm.isRoughlyAtSetpoint();
             });
   }
 }
diff --git a/src/main/java/frc/robot/commands/Positions/ShootCloseCommand.java b/src/main/java/frc/robot/commands/Positions/ShootCloseCommand.java
@@ -14,12 +14,13 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class ShootCloseCommand extends Command {
 
   private Arm arm;
 
-  double ARMANGLE = 0.687;
+  double ArmAngle = 0.687;
 
   public ShootCloseCommand(Arm arm) {
     this.arm = arm;
@@ -33,7 +34,7 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    arm.runTargetAngle(ARMANGLE);
+    arm.runTargetAngle(Optional.of(ArmAngle));
   }
 
   // Called once the command ends or is interrupted.
@@ -42,6 +43,6 @@ public void end(boolean interrupted) {}
 
   @Override
   public boolean isFinished() {
-    return arm.getIsFinished();
+    return arm.isRoughlyAtSetpoint();
   }
 }
diff --git a/src/main/java/frc/robot/commands/Positions/ShootFar.java b/src/main/java/frc/robot/commands/Positions/ShootFar.java
@@ -15,21 +15,21 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class ShootFar { // two robots away from wall
 
   public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ARMANGLE = 0.799;
-
-              arm.runTargetAngle(ARMANGLE);
+              double ArmAngle = 0.799;
+              arm.runTargetAngle(Optional.of(ArmAngle));
             },
             arm)
         .until(
             () -> {
-              return arm.getIsFinished();
+              return arm.isRoughlyAtSetpoint();
             });
   }
 }
diff --git a/src/main/java/frc/robot/commands/Positions/ShootFarCommand.java b/src/main/java/frc/robot/commands/Positions/ShootFarCommand.java
@@ -14,12 +14,13 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class ShootFarCommand extends Command {
 
   private Arm arm;
 
-  double ARMANGLE = 0.799;
+  double ArmAngle = 0.799;
 
   public ShootFarCommand(Arm arm) {
     this.arm = arm;
@@ -33,7 +34,7 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    arm.runTargetAngle(ARMANGLE);
+    arm.runTargetAngle(Optional.of(ArmAngle));
   }
 
   // Called once the command ends or is interrupted.
@@ -42,6 +43,6 @@ public void end(boolean interrupted) {}
 
   @Override
   public boolean isFinished() {
-    return arm.getIsFinished();
+    return arm.isRoughlyAtSetpoint();
   }
 }
diff --git a/src/main/java/frc/robot/commands/Positions/ShootMedium.java b/src/main/java/frc/robot/commands/Positions/ShootMedium.java
@@ -15,21 +15,22 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class ShootMedium { // one robot length away
 
   public static Command run(Arm arm) {
     return Commands.run(
             () -> {
               // Position preset settings
-              double ARMANGLE = 0.933;
+              double ArmAngle = 0.933;
 
-              arm.runTargetAngle(ARMANGLE);
+              arm.runTargetAngle(Optional.of(ArmAngle));
             },
             arm)
         .until(
             () -> {
-              return arm.getIsFinished();
+              return arm.isRoughlyAtSetpoint();
             });
   }
 }
diff --git a/src/main/java/frc/robot/commands/Positions/ShootMediumCommand.java b/src/main/java/frc/robot/commands/Positions/ShootMediumCommand.java
@@ -14,12 +14,13 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.arm.Arm;
+import java.util.Optional;
 
 public class ShootMediumCommand extends Command {
 
   private Arm arm;
 
-  double ARMANGLE = 0.933;
+  double ArmAngle = 0.933;
 
   public ShootMediumCommand(Arm arm) {
     this.arm = arm;
@@ -33,7 +34,7 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    arm.runTargetAngle(ARMANGLE);
+    arm.runTargetAngle(Optional.of(ArmAngle));
   }
 
   // Called once the command ends or is interrupted.
@@ -42,6 +43,6 @@ public void end(boolean interrupted) {}
 
   @Override
   public boolean isFinished() {
-    return arm.getIsFinished();
+    return arm.isRoughlyAtSetpoint();
   }
 }