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;
009import edu.wpi.first.math.geometry.Twist2d;
010
011/**
012 * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
013 * velocities for a differential drive.
014 *
015 * <p>Inverse kinematics converts a desired chassis speed into left and right velocity components
016 * whereas forward kinematics converts left and right component velocities into a linear and angular
017 * chassis speed.
018 */
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
062  /**
063   * Performs forward kinematics to return the resulting Twist2d from the given left and right side
064   * distance deltas. This method is often used for odometry -- determining the robot's position on
065   * the field using changes in the distance driven by each wheel on the robot.
066   *
067   * @param leftDistanceMeters The distance measured by the left side encoder.
068   * @param rightDistanceMeters The distance measured by the right side encoder.
069   * @return The resulting Twist2d.
070   */
071  public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) {
072    return new Twist2d(
073        (leftDistanceMeters + rightDistanceMeters) / 2,
074        0,
075        (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters);
076  }
077}