Path following

[devsamuelv]

Changes Introduced

• Setup the AutoBuilder last in the SwerveSubsystem constructor and created a new constant called kPathFollowerConfig. This centralizes the follower config in one place and defines the PID controllers for x & y axes, not to mention the drivetrain constraints.

public static HolonomicPathFollowerConfig kPathFollowerConfig = new HolonomicPathFollowerConfig(new PIDConstants(0),
    new PIDConstants(0),
    DriveConstants.kPhysicalMaxSpeedMetersPerSecond, DriveConstants.kBaseRadius,
    new ReplanningConfig());

• Added FollowPathCommand to getAutonomousCommand to execute the path from pathplanner in RobotContainer.

new FollowPathCommand(path, swerveSubsystem::getCurrentPose, swerveSubsystem::getSpeeds,
        swerveSubsystem::setStates,
        new PPHolonomicDriveController(new PIDConstants(.3), new PIDConstants(.3),
            Constants.DriveConstants.kPhysicalMaxSpeedMetersPerSecond, Constants.DriveConstants.kBaseRadius),
        new ReplanningConfig(), swerveSubsystem);

[devsamuelv]

Dev Note
Updated the drive gear ratio to L4 and updated the max speed to 2.9 m/s since that's the real speed of L4 with neo's. This however, changes the max slope on the skew limiters responsible for joystick control. These values need updating and verification.

[devsamuelv]

Dev Note
As of commit e2b6ea0. The path following branch is now on WPI version 2k24-beta3. This was a necessary migration in-order to use the new tools included in pathplanner & advantage-kit.

[devsamuelv]

Dev Note
COMMIT:16916b5

Added some counter skew into setStates on SwerveSubsystem to reduce the accruing error while driving. I'm not sure if the amount of drivetrain error has decreased. I need to actually test it, i just ran-out of time today. However, the handling of the drivetrain has changed and I think for the better, the translational movements are smoother. it still needs accel tuning though. Not to mention I updated the backRight swerve pod offset from -1 rad to -0.79 rad. This fixed most of the angular drift.

So, this takes the current position of the robot & calculates the desired position from demanded speeds and takes the Pose2d created from that data & uses the Pose2d.log method to return a Twist2d which is basically the change in distance across the two pose2d's. Then that's converted into the necessary speeds. Also the looper variable was intended to be how many control looks we look ahead, keep in mind that the typical control loop takes 30ms. My thanks to Jared from 254 for this solution.

double looper = .25;
Pose2d currentPose = getCurrentPose();
Pose2d desired = new Pose2d(currentPose.getX() + (speeds.vxMetersPerSecond *
    looper),
    currentPose.getY() + (speeds.vyMetersPerSecond * looper),
    currentPose.getRotation().plus(Rotation2d.fromRadians(speeds.omegaRadiansPerSecond
        * angle_looper)));

Twist2d twist_vel = currentPose.log(desired);
ChassisSpeeds updated_speeds = new ChassisSpeeds(twist_vel.dx / looper,
    twist_vel.dy / looper,
    twist_vel.dtheta / looper);

[devsamuelv]

Added gyro update rate into angular calculations to hopefully solve the issue. However, I might need to look at pid error first otherwise the issue will need to be dropped. chiefdelphi post on field relative drift.

[devsamuelv]

Dev Note

So as of commit 3ec6647 the position drift has been mostly corrected the next step is to tune the SwervePoseEstimator to increase the reliability of the pose estimates. I found during my testing that the error between the perceived position & real position where very constant during speed testing. So I moved the robot to different positions on the field marked by distance using meters and recorded both distances. And luckily the perceived distance was linear.

I ended-up using linear regression to calculate the slope for robot distance and added it to the end of getDrivePosition method which returns the distance in meters the swerve pod has traveled using the motor encoders. The factor is 1.305 and I just multiply distance by a ratio to avoid dividing by zero.

@Override
public double getDrivePosition() {
  /*
   * I'm going to remove factor 0f 42 to see if the converion is done by revlib
   * already
   */
  return (((driveMotor.getEncoder().getPosition()) * Constants.ModuleConstants.kDriveMotorGearRatio)
      * (Constants.ModuleConstants.kWheelDiameterMeters * Math.PI)) * (1 / 1.305);
}

This means we are missing something when we calculate the robot position. However, currently it makes more sense to run with this solution instead of hunting down this issue. Otherwise, the next step is to calculate the standard deviation of the drive encoders to tune our SwerveDriveEstimator since at this point when I run auto paths it either under or over predicts pose estimates.