Merge branch 'main' into 128-make-lift-use-pids

LakotaRobotics1038 · Mar 21, 2024 · 12bb5e2 · 12bb5e2
2 parents 3632c90 + 437c5d4
commit 12bb5e2
Show file tree

Hide file tree

Showing 19 changed files with 76 additions and 87 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -1,14 +1,14 @@
 {
-    "robotWidth": 0.8202676,
-    "robotLength": 0.8202676,
-    "holonomicMode": true,
-    "pathFolders": [
-        "New Folder"
-    ],
-    "autoFolders": [],
-    "defaultMaxVel": 3.0,
-    "defaultMaxAccel": 3.0,
-    "defaultMaxAngVel": 540.0,
-    "defaultMaxAngAccel": 720.0,
-    "maxModuleSpeed": 4.5
+  "robotWidth": 0.82,
+  "robotLength": 0.82,
+  "holonomicMode": true,
+  "pathFolders": [
+    "New Folder"
+  ],
+  "autoFolders": [],
+  "defaultMaxVel": 7.5,
+  "defaultMaxAccel": 1.5,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 6.0
 }
diff --git a/src/main/deploy/pathplanner/paths/From amp to midline acquire.path b/src/main/deploy/pathplanner/paths/From amp to midline acquire.path
@@ -54,8 +54,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 7.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
@@ -70,5 +70,5 @@
     "rotation": -90.0,
     "velocity": 0
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
diff --git a/src/main/deploy/pathplanner/paths/From amp to midline.path b/src/main/deploy/pathplanner/paths/From amp to midline.path
@@ -54,8 +54,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 7.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
@@ -70,5 +70,5 @@
     "rotation": -90.0,
     "velocity": 0
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
diff --git a/src/main/deploy/pathplanner/paths/From amp to top note.path b/src/main/deploy/pathplanner/paths/From amp to top note.path
@@ -64,8 +64,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 7.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
@@ -80,5 +80,5 @@
     "rotation": -90.0,
     "velocity": 0
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
diff --git a/src/main/deploy/pathplanner/paths/From note 1 to amp.path b/src/main/deploy/pathplanner/paths/From note 1 to amp.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 7.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
@@ -48,5 +48,5 @@
     "rotation": -45.0,
     "velocity": 0
   },
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
diff --git a/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path b/src/main/deploy/pathplanner/paths/From position 1 to amp exagerated.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 7.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
@@ -45,5 +45,5 @@
   "reversed": false,
   "folder": null,
   "previewStartingState": null,
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
diff --git a/src/main/deploy/pathplanner/paths/From position 1 to amp.path b/src/main/deploy/pathplanner/paths/From position 1 to amp.path
@@ -54,8 +54,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 7.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
@@ -67,5 +67,5 @@
   "reversed": false,
   "folder": null,
   "previewStartingState": null,
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 1.path b/src/main/deploy/pathplanner/paths/Taxi Position 1.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 7.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
@@ -45,5 +45,5 @@
   "reversed": false,
   "folder": null,
   "previewStartingState": null,
-  "useDefaultConstraints": false
+  "useDefaultConstraints": true
 }
diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 2.path b/src/main/deploy/pathplanner/paths/Taxi Position 2.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 7.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },

diff --git a/src/main/deploy/pathplanner/paths/Taxi Position 3.path b/src/main/deploy/pathplanner/paths/Taxi Position 3.path
@@ -9,37 +9,37 @@
       "prevControl": null,
       "nextControl": {
         "x": 1.263928801703757,
-        "y": 1.6839582397113524
+        "y": 1.6839582397113526
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "startTaxi3"
     },
     {
       "anchor": {
-        "x": 2.5763977418691195,
-        "y": 1.6839582397113522
+        "x": 2.32,
+        "y": 1.68
       },
       "prevControl": {
-        "x": 2.6363977418691196,
-        "y": 1.6839582397113522
+        "x": 2.38,
+        "y": 1.68
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "endTaxi3"
     }
   ],
   "rotationTargets": [],
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 7.5,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0,
+    "rotation": 0.0,
     "rotateFast": false
   },
   "reversed": false,

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -10,9 +10,11 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.autons.Auton;
 import frc.robot.autons.AutonSelector;
+import frc.robot.commands.LiftDownCommand;
 import frc.robot.commands.LiftUpCommand;
 import frc.robot.constants.SwerveModuleConstants;
 import frc.robot.subsystems.Dashboard;
@@ -70,7 +72,9 @@ public void disabledExit() {
 
     @Override
     public void autonomousInit() {
-        new LiftUpCommand().schedule();
+        new SequentialCommandGroup(
+        new LiftDownCommand(),
+        new LiftUpCommand()).schedule();
         driveTrain.zeroHeading();
         autonomousCommand = autonSelector.chooseAuton();
         // if (DriverStation.isFMSAttached()) {

diff --git a/src/main/java/frc/robot/autons/Auton.java b/src/main/java/frc/robot/autons/Auton.java
@@ -17,7 +17,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
-import frc.robot.autons.AutonSelector.AutonChoices;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DriveConstants;
 import frc.robot.constants.FieldConstants;
@@ -62,8 +61,12 @@ public Command followPathCommand(PathPlannerPath path) {
                                                      // ChassisSpeeds
                 new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your
                                                  // Constants class
-                        new PIDConstants(AutoConstants.kPXController, 0.0, 0.0), // Translation PID constants
-                        new PIDConstants(AutoConstants.kPThetaController, 0.0, 0.0), // Rotation PID constants
+                        new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, 0.0), // Translation
+                                                                                                         // PID
+                                                                                                         // constants
+                        new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, 0.0), // Rotation
+                                                                                                                 // PID
+                                                                                                                 // constants
                         DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s
                         DriveConstants.kBaseRadius, // Drive base radius in meters. Distance from robot center to
                                                     // furthest module.

diff --git a/src/main/java/frc/robot/autons/Paths.java b/src/main/java/frc/robot/autons/Paths.java
@@ -5,6 +5,7 @@
 public class Paths {
     public static PathPlannerPath pathFromPosition1ToAmp = PathPlannerPath.fromPathFile("From position 1 to amp");
 
+    public static PathPlannerPath pathFromTaxi3ToPos1 = PathPlannerPath.fromPathFile("Taxi Position 3 reversed");
     public static PathPlannerPath pathFromAmpToNote1 = PathPlannerPath.fromPathFile("From amp to top note");
 
     public static PathPlannerPath pathFromNote1ToAmp = PathPlannerPath.fromPathFile("From note 1 to amp");

diff --git a/src/main/java/frc/robot/autons/Trajectories.java b/src/main/java/frc/robot/autons/Trajectories.java
@@ -11,6 +11,11 @@ public static PathPlannerTrajectory getFromAmpToNote1Trajectory() {
                 Rotation2d.fromDegrees(DriveTrain.getInstance().getHeading()));
     }
 
+    public static PathPlannerTrajectory getFromTaxi3ToPos1Trajectory() {
+        return new PathPlannerTrajectory(Paths.pathFromTaxi3ToPos1, DriveTrain.getInstance().getChassisSpeeds(),
+                Rotation2d.fromDegrees(DriveTrain.getInstance().getHeading()));
+    }
+
     public static PathPlannerTrajectory getFromPos1TaxiTrajectory() {
         return new PathPlannerTrajectory(Paths.pathFromNote1ToAmp, DriveTrain.getInstance().getChassisSpeeds(),
                 Rotation2d.fromDegrees(DriveTrain.getInstance().getHeading()));

diff --git a/src/main/java/frc/robot/commands/LiftUpCommand.java b/src/main/java/frc/robot/commands/LiftUpCommand.java
@@ -24,7 +24,7 @@ public void execute() {
 
     @Override
     public boolean isFinished() {
-        return !lift.isLiftUp();
+        return lift.isLiftUp();
     }
 
     @Override

diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java
@@ -3,14 +3,14 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public final class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = DriveConstants.kMaxSpeedMetersPerSecond;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 7;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final double kPXController = 0.3;
-    public static final double kPYController = 2.0;
-    public static final double kPThetaController = 2.5;
+    public static final double kPXController = 1.0;
+    public static final double kIXController = 0.35;
+    public static final double kPThetaController = 1.0;
+    public static final double kIThetaController = 0.05;
 
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(

diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java
@@ -11,12 +11,6 @@ public final class DriveConstants {
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
     public static final double kFineAdjustmentPercent = 0.2;
 
-    // Rotation PID Constants
-    public static final double kRotationLockP = 0.01;
-    public static final double kRotationLockI = 0.000;
-    public static final double kRotationLockD = 0.000;
-    public static final double kRotationLockTolerance = 1.0;
-
     // Chassis configuration
     public static final double kTrackWidth = Units.inchesToMeters(22.5);
     // Distance between centers of right and left wheels on robot

diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java
@@ -10,9 +10,11 @@
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.BuiltInAccelerometer;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.shuffleboard.WidgetType;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -32,6 +34,7 @@ public class Dashboard extends SubsystemBase {
     // Tabs
     private final ShuffleboardTab driversTab = Shuffleboard.getTab("Drivers");
     private final ShuffleboardTab controlsTab = Shuffleboard.getTab("Controls");
+    private final ShuffleboardTab graphTab = Shuffleboard.getTab("Graph");
     private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault();
 
     // Variables
@@ -100,6 +103,11 @@ private Dashboard() {
                 .withPosition(2, 1)
                 .withSize(4, 3)
                 .withWidget(BuiltInWidgets.kField);
+        BuiltInAccelerometer acc = new BuiltInAccelerometer();
+        graphTab.addDouble("acc", acc::getZ)
+                .withPosition(0, 0)
+                .withSize(2, 2)
+                .withWidget(BuiltInWidgets.kGraph);
 
         PathPlannerLogging.setLogTargetPoseCallback((pose) -> {
             field.getObject("target pose").setPose(pose);

diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -77,32 +77,6 @@ public static DriveTrain getInstance() {
     private DriveTrain() {
         super();
         gyro.reset();
-
-        // Auto Setup
-        AutoBuilder.configureHolonomic(
-                this::getPose,
-                this::resetOdometry,
-                this::getChassisSpeeds,
-                this::applyChassisSpeeds,
-                new HolonomicPathFollowerConfig(
-                        new PIDConstants(AutoConstants.kPXController, 0.0, 0.0), // Translation PID constants
-                        new PIDConstants(AutoConstants.kPThetaController, 0.0, 0.0), // Rotation PID constants
-                        DriveConstants.kMaxSpeedMetersPerSecond,
-                        DriveConstants.kBaseRadius,
-                        new ReplanningConfig()),
-                () -> {
-                    // Boolean supplier that controls when the path will be mirrored for the red
-                    // alliance
-                    // This will flip the path being followed to the red side of the field.
-                    // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-
-                    var alliance = DriverStation.getAlliance();
-                    if (alliance.isPresent()) {
-                        return alliance.get() == DriverStation.Alliance.Red;
-                    }
-                    return false;
-                },
-                this);
     }
 
     @Override