Auton significantly overshooting the path in pathplanner by length

[Samay]

We are using differential drive with path planner software. On the path planner software we have max velocity and max acceleration equal to 3. We also have max angular velocity to 540 and angular acceleration to 720. We are using TalonFx motors. We believe this is the code causing issues.

public void drive(ChassisSpeeds chassisSpeeds) {
DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(
ABC.feet_to_meters(kTrackWidthFeet));
double linearSpeed = chassisSpeeds.vxMetersPerSecond;
double angularSpeed = chassisSpeeds.omegaRadiansPerSecond;
ChassisSpeeds currentSpeeds = getCurrentSpeeds();
linearSpeed = currentSpeeds.vxMetersPerSecond;
angularSpeed = currentSpeeds.omegaRadiansPerSecond;

            DifferentialDriveWheelSpeeds wheelSpeeds = kinematics
                            .toWheelSpeeds(new ChassisSpeeds(linearSpeed, 0, angularSpeed));

            double leftVelocity = wheelSpeeds.leftMetersPerSecond;

            double rightVelocity = wheelSpeeds.rightMetersPerSecond;

            setMotorSpeed(leftVelocity, rightVelocity);
    }

    public ChassisSpeeds getCurrentSpeeds() {
            DifferentialDriveKinematics mKinematics = new DifferentialDriveKinematics(
                            ABC.feet_to_meters(kTrackWidthFeet));

            var mWheelSpeeds = new DifferentialDriveWheelSpeeds(0.25, 0.25);

            ChassisSpeeds mFieldRelativeSpeeds = mKinematics.toChassisSpeeds(mWheelSpeeds);

            ChassisSpeeds mRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(mFieldRelativeSpeeds,
                            mGyro.getHeading());
            return mRobotRelativeSpeeds;
    }

Here is the drivetrain code that had to be added for pathplanner and other code that is related to auton:

public void periodic() { // This method will be called once per scheduler run (usually, once every 20
// ms),
runTest(() → {
mField.setRobotPose(getPose());
REVLibCAN.logFaults(Stream.of(mLeftMaster, mLeftFollower, mRightMaster, mRightFollower));

                    var mGyroAngle = mGyro.getHeading();
                    mOdometry.update(mGyroAngle, mLeftEncoder.getVelocity(), mRightEncoder.getVelocity());

                    leftMaster.setDouble(mLeftMaster.get());
                    leftFollower.setDouble(mLeftFollower.get());
                    rightMaster.setDouble(mRightMaster.get());
                    rightFollower.setDouble(mRightFollower.get());
                    gyro.setDouble(mGyro.getHeading().getDegrees());
                    // ... Other periodic tasks
            });
    }
    
    public void drive(ChassisSpeeds chassisSpeeds) {
            DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(                        
            ABC.feet_to_meters(kTrackWidthFeet));
            double linearSpeed = chassisSpeeds.vxMetersPerSecond;
            double angularSpeed = chassisSpeeds.omegaRadiansPerSecond;
            ChassisSpeeds currentSpeeds = getCurrentSpeeds();
            linearSpeed = currentSpeeds.vxMetersPerSecond;
            angularSpeed = currentSpeeds.omegaRadiansPerSecond;

            DifferentialDriveWheelSpeeds wheelSpeeds = kinematics
                            .toWheelSpeeds(new ChassisSpeeds(linearSpeed, 0, angularSpeed));

            double leftVelocity = wheelSpeeds.leftMetersPerSecond;

            double rightVelocity = wheelSpeeds.rightMetersPerSecond;

            setMotorSpeed(leftVelocity, rightVelocity);
    }

    public ChassisSpeeds getCurrentSpeeds() {
            DifferentialDriveKinematics mKinematics = new DifferentialDriveKinematics(
                            ABC.feet_to_meters(kTrackWidthFeet));

            var mWheelSpeeds = new DifferentialDriveWheelSpeeds(0.25, 0.25);

            ChassisSpeeds mFieldRelativeSpeeds = mKinematics.toChassisSpeeds(mWheelSpeeds);

            ChassisSpeeds mRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(mFieldRelativeSpeeds,
                            mGyro.getHeading());
            return mRobotRelativeSpeeds;
    }

    public Pose2d getPose() {
            return mOdometry.getPoseMeters();
    }

    public void resetPose(Pose2d pose) {
            mOdometry.resetPosition(mGyro.getHeading(), mLeftEncoder.getPosition(), mRightEncoder.getPosition(),
                            pose);
    }

    public void setMotorSpeed(final double leftSpeed, final double rightSpeed) {
            mLeftMaster.set(leftSpeed);
            mRightMaster.set(rightSpeed);
    }

    public double getMotorSpeed() {
            // Getting motor speed using the ".get()" method from the CANSparkMax class
            return mLeftMaster.get();
    }

public void reset() {
runTest(() → {
Stream.of(mLeftEncoder, mRightEncoder)
.forEach(encoder → encoder.setPosition(0.0));
Stream.of(mLeftMaster, mRightMaster)
.forEach(motor → motor.set(0.0));
Stream.of(mLeftMaster, mRightMaster)
.forEach(motor → motor.stopMotor());
});
}

[Samay]

Correction we are using CANSparkMax motors.