Merge master

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,17 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "Shuffleboard": {
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

src/main/deploy/pathplanner/autos/From Subwoofer Far Edge to Amp.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7046435911053639,
+      "y": 4.3751818412727745
+    },
+    "rotation": 120.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "From Subwoofer Far Edge to Amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/From Subwoofer Front to Amp.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.29,
+      "y": 5.58
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "From Subwoofer Front to Amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/From Subwoofer Far Edge to Amp.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7046435911053639,
+        "y": 4.3751818412727745
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.4368920788482864,
+        "y": 3.1622483921028106
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.82409310273166,
+        "y": 7.565612949407717
+      },
+      "prevControl": {
+        "x": 1.8176568333482528,
+        "y": 6.336387277909188
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 89.21368742867472,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 122.73522627210761,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/From Subwoofer Front to Amp.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2945574370576298,
+        "y": 5.580559282764361
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2786077412442953,
+        "y": 5.580559282764361
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.8232420006879377,
+        "y": 7.533755031731888
+      },
+      "prevControl": {
+        "x": 1.8232420006879377,
+        "y": 6.694612522270728
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/frc/robot/RobotContainer.java

@@ -4,35 +4,40 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+
 import edu.wpi.first.math.geometry.Pose2d;
+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.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.utilities.PoseSendable;
 
-/*
- * The objective for this part of testing is to just make sure swerve is working in a perfect world scenario.
- * This is designed to be as close the REV template as possible, the only difference being the naming of certain 
- * Constants and the positioning of instance variables and there declarations. 
- * 
- * We should try to solve in this branch:
- * - Does Field Oriented Drive work in a perfect world scenario?
- * - Is our weird module spin behavior still present?
- *  - If it is, can we do something about it?
- * 
- * Any changes we make should be able to be carried forward to part 2
- */
 public class RobotContainer {
     private Drivetrain drivetrain;
 
     private CommandXboxController driver;
 
+    private PoseSendable poseSendable;
+
+    private SendableChooser<Command> autoChooser;
+
     public RobotContainer() {
         drivetrain = new Drivetrain(new Pose2d());
 
         driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
 
+        poseSendable = new PoseSendable();
+
+        autoChooser = AutoBuilder.buildAutoChooser();
+
         configureBindings();
+        shuffleboardConfig();
     }
 
     private void configureBindings() {
@@ -46,10 +51,35 @@ public class RobotContainer {
         );
 
         driver.a().onTrue(drivetrain.zeroHeadingCommand());
-        
-  }
 
-  public Command getAutonomousCommand() {
-        return null;
-  }
+        driver.b().onTrue(
+            new InstantCommand(() -> {
+                System.out.println("X: " + poseSendable.getX());
+                System.out.println("Y: " + poseSendable.getY());
+                System.out.println("Rotation: " + poseSendable.getRotation());
+                drivetrain.resetOdometry(poseSendable.getPose());
+            }
+        ));
+    }
+
+    private void shuffleboardConfig() {
+        ShuffleboardTab tab = Shuffleboard.getTab("Testing");
+        tab.add("Drivetrain Pose", poseSendable)
+            .withPosition(0, 0)
+            .withSize(2, 3);
+
+        tab.add("Auto Selector", autoChooser)
+            .withPosition(2, 0)
+            .withSize(2, 1);
+
+        tab.addDouble("Current Pose Rotation", () -> { return drivetrain.getPose().getRotation().getDegrees(); })
+            .withPosition(2, 1)
+            .withSize(2, 1)
+            .withWidget(BuiltInWidgets.kTextView);
+    }
+
+    public Command getAutonomousCommand() {
+        return drivetrain.zeroHeadingCommand().andThen(autoChooser.getSelected());
+        
+    }
 }

src/main/java/frc/robot/constants/AutoConstants.java

@@ -0,0 +1,34 @@
+package frc.robot.constants;
+
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+
+public final class AutoConstants {
+    public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6
+    public static final double kMaxAccelerationMetersPerSecondSquared = 3; // max capable = 7.4
+    public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+    public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
+
+    public static final double kPXController = 1.05;
+    public static final double kPYController = 1.05;
+    public static final double kPThetaController = 0.95; // needs to be separate from heading control
+
+    public static final double kPHeadingController = 0.02; // for heading control NOT PATHING
+    public static final double kDHeadingController = 0.0025;
+
+    public static final HolonomicPathFollowerConfig kPFConfig = new HolonomicPathFollowerConfig(
+        new PIDConstants(kPXController), 
+        new PIDConstants(kPThetaController), 
+        kMaxSpeedMetersPerSecond, 
+        Units.inchesToMeters(Math.sqrt(Math.pow(14-1.75, 2) + Math.pow(14-1.75, 2))), 
+        new ReplanningConfig()
+    );
+
+    // Constraint for the motion profiled robot angle controller
+    public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+}

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -11,11 +11,15 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.WPIUtilJNI;
 
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
 import com.kauailabs.navx.frc.AHRS;
+import com.pathplanner.lib.auto.AutoBuilder;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.SPI;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.utilities.MAXSwerveModule;
 import frc.robot.utilities.SwerveUtils;
@@ -87,10 +91,28 @@ public class Drivetrain extends SubsystemBase {
             },
             initialPose
         );
+
         m_currentRotation = 0.0;
         m_currentTranslationDir = 0.0;
         m_currentTranslationMag = 0.0;
         m_prevTime =  WPIUtilJNI.now() * 1e-6;
+
+        AutoBuilder.configureHolonomic(
+            this::getPose, 
+            this::resetOdometry, 
+            this::getCurrentChassisSpeeds, 
+            this::driveWithChassisSpeeds, 
+            AutoConstants.kPFConfig, 
+            () -> {
+                Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+            }, 
+            this
+        );
+
     }
 
     @Override
@@ -208,7 +230,7 @@ public class Drivetrain extends SubsystemBase {
 
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative
-                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle()))
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, m_poseEstimator.getEstimatedPosition().getRotation())
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
             swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);

src/main/java/frc/robot/utilities/PoseSendable.java

@@ -0,0 +1,62 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+
+public class PoseSendable implements Sendable {
+
+    private double x;
+    private double y;
+    private double rot;
+
+    public PoseSendable() {
+        this(0, 0, 0);
+    }
+
+    public PoseSendable(double x, double y, double rot) {
+        this.x = x;
+        this.y = y;
+        this.rot = rot;
+    }
+
+    public double getX() {
+        return x;
+    }
+
+    public double getY() {
+        return y;
+    }
+
+    public double getRotation() {
+        return rot;
+    }
+
+    public Rotation2d getRotation2d() {
+        return Rotation2d.fromDegrees(rot);
+    }
+
+    public Pose2d getPose() {
+        return new Pose2d(x, y, getRotation2d());
+    }
+
+    public void setX(double x) {
+        this.x = x;
+    }
+
+    public void setY(double y) {
+        this.y = y;
+    }
+
+    public void setRotation(double rot) {
+        this.rot = rot;
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        builder.addDoubleProperty("X", this::getX, this::setX);
+        builder.addDoubleProperty("Y", this::getY, this::setY);
+        builder.addDoubleProperty("Rotation", this::getRotation, this::setRotation);
+    }
+}