Merge branch 'new-stuff' of https://github.com/NSS-Robotics/FRC8884-2023

into new-stuff
NSS-Robotics · Apr 21, 2023 · d1e1617 · d1e1617
2 parents 039821b + e48155e
commit d1e1617
Show file tree

Hide file tree

Showing 13 changed files with 580 additions and 55 deletions.
diff --git a/gradlew.bat b/gradlew.bat
@@ -71,7 +71,7 @@ set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
 
 
 @rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.vfs.watch=false" "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
 
 :end
 @rem End local scope for the variables with windows NT shell

diff --git a/src/main/deploy/pathplanner/OnePiece.path b/src/main/deploy/pathplanner/OnePiece.path
@@ -3,43 +3,18 @@
     {
       "anchorPoint": {
         "x": 2.0,
-        "y": 3.0
+        "y": 2.7
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.0,
-        "y": 3.0
+        "x": 2.999999999999999,
+        "y": 2.7
       },
       "holonomicAngle": 0,
       "isReversal": false,
-      "velOverride": 2.0,
-      "isLocked": false,
-      "isStopPoint": false,
-      "stopEvent": {
-        "names": [],
-        "executionBehavior": "parallel",
-        "waitBehavior": "none",
-        "waitTime": 0
-      }
-    },
-    {
-      "anchorPoint": {
-        "x": 8.0,
-        "y": 3.0
-      },
-      "prevControl": {
-        "x": 6.999999999999998,
-        "y": 3.0
-      },
-      "nextControl": {
-        "x": 6.999999999999998,
-        "y": 3.0
-      },
-      "holonomicAngle": 0,
-      "isReversal": true,
       "velOverride": null,
       "isLocked": false,
-      "isStopPoint": true,
+      "isStopPoint": false,
       "stopEvent": {
         "names": [],
         "executionBehavior": "parallel",
@@ -49,17 +24,17 @@
     },
     {
       "anchorPoint": {
-        "x": 4.85,
-        "y": 3.0
+        "x": 4.5,
+        "y": 2.7
       },
       "prevControl": {
-        "x": 5.8500000000000005,
-        "y": 3.0
+        "x": 3.5,
+        "y": 2.7
       },
       "nextControl": null,
       "holonomicAngle": 0,
       "isReversal": false,
-      "velOverride": 2.0,
+      "velOverride": null,
       "isLocked": false,
       "isStopPoint": false,
       "stopEvent": {

diff --git a/src/main/deploy/pathplanner/OnePieceMobility.path b/src/main/deploy/pathplanner/OnePieceMobility.path
@@ -0,0 +1,77 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 2.0,
+        "y": 3.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 3.0
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": 2.0,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 8.0,
+        "y": 3.0
+      },
+      "prevControl": {
+        "x": 6.999999999999998,
+        "y": 3.0
+      },
+      "nextControl": {
+        "x": 6.999999999999998,
+        "y": 3.0
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": true,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 4.85,
+        "y": 3.0
+      },
+      "prevControl": {
+        "x": 5.8500000000000005,
+        "y": 3.0
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": 2.0,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "maxVelocity": null,
+  "maxAcceleration": null,
+  "isReversed": true,
+  "markers": []
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -45,9 +45,9 @@ public final class Constants {
   /* Balancing Constants */
   public static final double kLevel = 0;
   public static final double kBalanceToleranceDeg = -1.5;
-  public static final double kBalanceP = .1;
-  public static final double kBalanceI = 0.1;
-  public static final double kBalanceD = 0;
+  public static final double kBalanceP = 0.008;
+  public static final double kBalanceI = 0.08;
+  public static final double kBalanceD = 0.025;
 
   public static final double kAngleCorrectionP = .01;
   public static final double kTurnToleranceDeg = 10;
@@ -73,7 +73,7 @@ public static final class ElevatorConstants {
     public static final int RMotorID = 13;
     public static final int BottomNodeDistance = 1;
     public static final int MidNodeDistance = 12;
-    public static final double HPDistance = 55;
+    public static final double HPDistance = 52;
     public static final int TopNodeDistance = 73;
     public static final int MaxHeight = 72;
     public static final double Kp = 0.0222244;
@@ -226,8 +226,8 @@ public static final class Mod3 {
 
   public static final class AutoConstants {
 
-    public static final double kMaxSpeedMetersPerSecond = 2.5;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 2.5;
+    public static final double kMaxSpeedMetersPerSecond = 3;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared =
       Math.PI;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -128,6 +128,26 @@ public class RobotContainer {
   )
     .isMidNode(true);
 
+  private final OnePieceMobility onePieceMobilityTop = new OnePieceMobility(
+    s_Swerve,
+    pivot,
+    claw,
+    elevator,
+    arm,
+    true
+  )
+    .isMidNode(false);
+
+  private final OnePieceMobility onePieceMobilityMid = new OnePieceMobility(
+    s_Swerve,
+    pivot,
+    claw,
+    elevator,
+    arm,
+    true
+  )
+    .isMidNode(true);
+
   private final TwoPiece twoPieceLeftTop = new TwoPiece(
     s_Swerve,
     pivot,
@@ -161,6 +181,15 @@ public class RobotContainer {
     .isMidNode(false)
     .isLeft(false);
 
+  private final StandStill StandStill = new StandStill(
+    s_Swerve,
+    pivot,
+    claw,
+    elevator,
+    arm,
+    true
+  );
+
   private final TwoPiece twoPieceRightMid = new TwoPiece(
     s_Swerve,
     pivot,
@@ -189,6 +218,14 @@ public RobotContainer() {
 
     m_chooser.setDefaultOption("OnePiece (TOP)", onePieceTop.followPath());
     m_chooser.setDefaultOption("OnePiece (MID)", onePieceMid.followPath());
+    m_chooser.setDefaultOption(
+      "OnePieceMobile (TOP)",
+      onePieceMobilityTop.followPath()
+    );
+    m_chooser.setDefaultOption(
+      "OnePieceMobile (MID)",
+      onePieceMobilityMid.followPath()
+    );
     m_chooser.addOption(
       "TwoPiece (RIGHT) (TOP)",
       twoPieceRightTop.followPath()
@@ -199,6 +236,7 @@ public RobotContainer() {
     );
     m_chooser.addOption("TwoPiece (LEFT) (TOP)", twoPieceLeftTop.followPath());
     m_chooser.addOption("TwoPiece (LEFT) (MID)", twoPieceLeftMid.followPath());
+    m_chooser.addOption("StandStill (TOP)", StandStill.followPath());
     CameraServer.startAutomaticCapture();
 
     SmartDashboard.putData(m_chooser);
@@ -217,7 +255,7 @@ private void configureButtonBindings() {
 
     /* Driver - limelight Buttons */
     alignTarget.whileTrue(new AlignGyro(180.0, s_Swerve));
-    alignCube.whileTrue(new AlignLimelight(Target.Cube, limelight, s_Swerve));
+    alignCube.whileTrue(new AutoBalance(s_Swerve));
     alignCone.whileTrue(new AlignLimelight(Target.Cone, limelight, s_Swerve));
 
     /* Operator Buttons */

diff --git a/src/main/java/frc/robot/autos/OnePiece.java b/src/main/java/frc/robot/autos/OnePiece.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
+import frc.robot.commands.drive.AutoBalance;
 import frc.robot.commands.nodescoring.*;
 import frc.robot.commands.nodescoring.armscoring.*;
 import frc.robot.subsystems.*;
@@ -73,7 +74,7 @@ public Command followPath() {
         new InstantCommand(claw::closeClaw),
         new InstantCommand(pivot::down)
       ),
-      new ParallelDeadlineGroup(new WaitCommand(2), elevatorNode),
+      new ParallelDeadlineGroup(new WaitCommand(1), elevatorNode),
       new ParallelDeadlineGroup(new WaitCommand(2.5), armNode),
       new ParallelDeadlineGroup(
         new WaitCommand(0.5),
@@ -119,10 +120,7 @@ public Command followPath() {
           swerve
         )
       ),
-      new ParallelDeadlineGroup(
-        new WaitCommand(0.3),
-        new InstantCommand(swerve::XFormation)
-      )
+      new AutoBalance(swerve)
     );
   }