Merge branch 'feature-backwards-pathplanning' into dev

FRC-4509-MechBulls · Feb 13, 2024 · cfeef45 · cfeef45
2 parents 6c1675f + 7ee616e
commit cfeef45
Show file tree

Hide file tree

Showing 12 changed files with 232 additions and 69 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -1,6 +1,6 @@
 {
-  "robotWidth": 0.9,
-  "robotLength": 0.9,
+  "robotWidth": 0.92,
+  "robotLength": 1.2,
   "holonomicMode": true,
   "pathFolders": [
     "4 amp auto",

diff --git a/src/main/deploy/pathplanner/paths/amp-align.path b/src/main/deploy/pathplanner/paths/amp-align.path
@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.823242000682939,
-        "y": 6.593871363056098
+        "x": 1.8379276830110018,
+        "y": 6.60855704537885
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.808556318359875,
-        "y": 6.740728186286739
+        "x": 1.8379276830110018,
+        "y": 6.756146326136938
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.823242000682939,
-        "y": 7.621869125670586
+        "x": 1.8379276830110018,
+        "y": 7.695297537285593
       },
       "prevControl": {
-        "x": 1.823242000682939,
-        "y": 6.8582136448712525
+        "x": 1.8379276830160005,
+        "y": 7.151927291331908
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "ampauto0"
     }
   ],
   "rotationTargets": [],
@@ -40,13 +40,13 @@
   "goalEndState": {
     "velocity": 0.0,
     "rotation": 90.0,
-    "rotateFast": false
+    "rotateFast": true
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
     "rotation": 90.0,
-    "velocity": 3.0
+    "velocity": 2.0
   },
   "useDefaultConstraints": true
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -4,14 +4,10 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.util.Units;
 
-import java.util.ArrayList;
-
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -42,7 +38,8 @@ public static class EFCConstants{
   }
 
   public static class ArmConstants{
-    public static final double holdingRad = Units.degreesToRadians(20.0);
+    public static final double duckingRad = Units.degreesToRadians(20.0);
+    public static final double holdingRadSafe = Units.degreesToRadians(70.0);
     public static final double ampRad = Units.degreesToRadians(100);
 
     public static final double sourceRad = Units.degreesToRadians(95.0);
@@ -77,6 +74,8 @@ public static class OperatorConstants {
 
   public static final class DriveConstants{
 
+    public static final double alignmentkP = 1.5;
+
     /*Physical Characteristics*/
     public static final double TRACK_WIDTH = Units.inchesToMeters(23.625); //need to find
     // Distance between right and left wheels

diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java
@@ -65,6 +65,8 @@ public static final class Speaker {
             new Pose2d(0.0, fieldWidth - Units.inchesToMeters(104.0), new Rotation2d());
   }
 
+  public static final Translation2d sourceCenterRough = new Translation2d(15.65,0.552);
+
   // corners (blue alliance origin)
   public static Translation3d topRightSpeaker =
           new Translation3d(

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,19 +4,13 @@
 
 package frc.robot;
 
-import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.commands.PathPlannerAuto;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.commands.Autos;
-import frc.robot.commands.drive.Alignments;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.ClimbSubsystem;
 import frc.robot.subsystems.drive.SwerveSubsystem;
@@ -31,11 +25,13 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
 
-  StateControllerSub stateController = new StateControllerSub(); //this MUST be created before any pathplanner commands
 
   CommandXboxController driver = new CommandXboxController(0);
   CommandXboxController operator = new CommandXboxController(1);
 
+  StateControllerSub stateController = new StateControllerSub(()->driver.getLeftTriggerAxis()>0.5 || driver.getRightTriggerAxis()>0.5); //this MUST be created before any pathplanner commands
+
+
   VisionSubsystem visionSub = new VisionSubsystem();
 
   SendableChooser<Command> autoChooser = new SendableChooser<>();
@@ -70,9 +66,9 @@ private void configureBindings() {
     operator.x().onTrue(new InstantCommand(stateController::ejectPressed));
     operator.y().onTrue(new InstantCommand(stateController::readyToShootPressed));
 
-    operator.leftBumper().onTrue(new InstantCommand(stateController::stowPressed));
-    operator.rightBumper().onTrue(new InstantCommand(stateController::raiseClimbPressed));
-    operator.rightTrigger(0.5).onTrue(new InstantCommand(stateController::climbPressed));
+   // operator.leftBumper().onTrue(new InstantCommand(stateController::stowPressed));
+   // operator.rightBumper().onTrue(new InstantCommand(stateController::raiseClimbPressed));
+   // operator.rightTrigger(0.5).onTrue(new InstantCommand(stateController::climbPressed));
 
     operator.povUp().onTrue(new InstantCommand(stateController::sourcePressed));
     operator.povRight().onTrue(new InstantCommand(stateController::ampPressed));
@@ -82,6 +78,19 @@ private void configureBindings() {
     operator.leftTrigger(0.5).onTrue(new InstantCommand(stateController::shootPressed));
 
 
+    driver.leftTrigger(0.5).onTrue(new InstantCommand(stateController::scheduleAlignmentCommand,swerveSubsystem));
+    driver.leftTrigger(0.5).onFalse(swerveSubsystem.getDefaultCommand());
+
+    driver.x().onTrue(new InstantCommand(stateController::shootPressed));
+
+
+   // driver.leftTrigger(0.5).onTrue(new InstantCommand(()->stateController.setDuckMode(true)));
+    //driver.leftTrigger(0.5).onFalse(new InstantCommand(()->stateController.setDuckMode(false)));
+
+   // driver.rightTrigger(0.5).onTrue(new InstantCommand(()->stateController.setDuckMode(true)));
+   // driver.rightTrigger(0.5).onFalse(new InstantCommand(()->stateController.setDuckMode(false)));
+
+    driver.a().onTrue(new InstantCommand(stateController::toggleAlignWhenClose));
 
  // operator.a().onTrue(new InstantCommand(()->stateController.setArmState(StateControllerSub.ArmState.INTAKE)));