Implement Swerve

[ErikCald]

The recommended tasks involved with programming it yourself are:

• [x] Clone FRC3512's repo onto your machine
• [x] Solve any errors in this repo due to the repo being in 2022 and our robot being a 2023 robot
• [x] Make modifications to Constants.java to match our settings (things like can id's)
• [x] Swap the order of burnFlash() and resetEncodersToAbsolute() in SwerveModule.java (call burnFlash() last)
• [x] Test and get this repo to work on the robot, tune the steer offsets.
• [x] Learn how this repo works then translate it into code on 2023-2706-Fall-Poseidon
• [x] Make modifications to the code to follow merge naming standards and merge's general format for code.

[ErikCald]

Check out the issue #6 where there's a checklist of things to log

[ErikCald]

List of most important tasks:

• [x] Add JavaDocs to all methods in SwerveSubsystem.java and SwerveModule.java
• [x] Publish odometry pose as a DoubleArrayTopic
• [x] Switch drive method to use a ChassisSpeed object as a parameter
• [x] Change SwerveSubsystem into a singleton class
• [x] Remove zeroGyro from SwerveSubsystem
• [x] Create SetHeadingCommand and SetOdometryCommand
• [x] Reduce speed of the wheel based on angle error (talk to Erik and see #24)
• [x] Add the thread sleep before burn flashing (talk to Erik and see #24)
• [x] Wrap sparkmax setting methods in configureSpark method. Import this method statically from the CheckError.java file.
• [x] enable voltage compensation on the sparkmax
• [x] Live update SimpleMotorFeedforward with network tables (Use an object called UpdateSimpleFeedforward which is already in the lib2706 folder of the SwerveBeta2024 branch.
• [x] PoseBuffer (talk to Erik)
• [ ] PathPlanner!
• [x] Copy over the ProfiledPidController stuff from last year's code that is currently being used by kiera/samuel (talk to Erik)

Less important goals:

• [x] Switch state optimizing function and use the sparkmax pid wrapping feature
• [x] Live tune swerve module offset with networktables
• [ ] Account for coupling ratio (start with just for odometry)
• [x] Account for skew when driving forward and rotating (see #24)
• [x] If robot hasn't moved for 0.3 seconds
• [ ] Use a pid to hold the heading stable when commanding no angular velocity
• [x] Add a Command + supporting Subsystem code to lock the wheels in an X pattern

[ErikCald]

• [ ] setCANTimeout(150) then after configuring everything set it to 0

[ErikCald]

• [x] Pid wrapping should be 0 to 2pi instead of 0 to 360 because units
• [x] In SwerveModule periodic, only update angleOffset if swerveTuning is true in Config. Also call setDefaultValue(...) on entryAngleOffset.
• [x] Make getYaw() private in SwerveSubsystem. Add a public Rotation2d getHeading() which returns getPose().getRotation()
• [x] Add a getFieldRelativeSpeeds() to SwerveSubsystem which returns ChassisSpeeds.fromRobotRelativeSpeeds(getRobotRelativeSpeeds(), getHeading())
• [x] resetDriveToPose() needs to also account for current velocity (talk to Erik)
• [x] Refactor the code in periodic to check if the robot is moving into a method. Aka move it to a method called isChassisMoving() that returns a boolean. (I think vision could take advantage of this method)
• [x] Make sure to run lastAngle = getAngle() whenever the encoders are synced.
• [ ] Switch to using the other version of SwerveDriveKinematics.desaturateWheelSpeeds
• [x] Lock wheels in X will likely be messed up by anti-jitter code. Add a boolean to setDesiredState for disabling anti jitter which can then be used by the X code. Can then do this to make other code work without change:

  public void public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
  setDesiredState(desiredState, isOpenLoop, false); // false to not disable the anti-jitter code
  }

[ErikCald]

• [x] mod.periodic() is called in SwerveSubsystem, move it from the isChassisMoving() to the top of the SwerveSubsystem's periodic (because it should only be called once per robot cycle so if someone else calls your isChassisMoving that it would be called an extra time)
• [x] The drive function in SwerveSubsystem should use getHeading() instead of getYaw() (all the other places that getYaw is used are good)
• [ ] Make another static command factory method to create a command that drives to a given pose using the driveToPose method (which we can use to test that whole system)
• [x] resetDriveToPose should also reset the pidControlRotation for field relative angular velocity (which we get from getFieldRelativeSpeeds and maybe later we can get it from the gyro instead)

Tasks for later (aka after the initial testing and verification because we have too much untested code right now):

• [ ] Put PathPlanner constants and ProfiledPidController constants into config
• [ ] Make isChassisMoving method take a parameter for the speed tolerance instead of using a fixed value (so that if vision uses this they could pick a different tolerance potentially)
• [x] Add resetToAbsolute() right before burnFlash() in SwerveModule. We do it automatically if the robot is not moving but would be good to do it once at boot up as a backup.