001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.kinematics;
006
007import edu.wpi.first.math.MathSharedStore;
008import edu.wpi.first.math.MathUsageId;
009
010/**
011 * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
012 * velocities for a differential drive.
013 *
014 * <p>Inverse kinematics converts a desired chassis speed into left and right velocity components
015 * whereas forward kinematics converts left and right component velocities into a linear and angular
016 * chassis speed.
017 */
018@SuppressWarnings("MemberName")
019public class DifferentialDriveKinematics {
020  public final double trackWidthMeters;
021
022  /**
023   * Constructs a differential drive kinematics object.
024   *
025   * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is the distance
026   *     between the left wheels and right wheels. However, the empirical value may be larger than
027   *     the physical measured value due to scrubbing effects.
028   */
029  public DifferentialDriveKinematics(double trackWidthMeters) {
030    this.trackWidthMeters = trackWidthMeters;
031    MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1);
032  }
033
034  /**
035   * Returns a chassis speed from left and right component velocities using forward kinematics.
036   *
037   * @param wheelSpeeds The left and right velocities.
038   * @return The chassis speed.
039   */
040  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
041    return new ChassisSpeeds(
042        (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
043        0,
044        (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters);
045  }
046
047  /**
048   * Returns left and right component velocities from a chassis speed using inverse kinematics.
049   *
050   * @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the
051   *     chassis' speed.
052   * @return The left and right velocities.
053   */
054  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
055    return new DifferentialDriveWheelSpeeds(
056        chassisSpeeds.vxMetersPerSecond
057            - trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond,
058        chassisSpeeds.vxMetersPerSecond
059            + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
060  }
061}