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    implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions> {
021  public final double trackWidthMeters;
022
023  /**
024   * Constructs a differential drive kinematics object.
025   *
026   * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is the distance
027   *     between the left wheels and right wheels. However, the empirical value may be larger than
028   *     the physical measured value due to scrubbing effects.
029   */
030  public DifferentialDriveKinematics(double trackWidthMeters) {
031    this.trackWidthMeters = trackWidthMeters;
032    MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1);
033  }
034
035  /**
036   * Returns a chassis speed from left and right component velocities using forward kinematics.
037   *
038   * @param wheelSpeeds The left and right velocities.
039   * @return The chassis speed.
040   */
041  @Override
042  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
043    return new ChassisSpeeds(
044        (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
045        0,
046        (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters);
047  }
048
049  /**
050   * Returns left and right component velocities from a chassis speed using inverse kinematics.
051   *
052   * @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the
053   *     chassis' speed.
054   * @return The left and right velocities.
055   */
056  @Override
057  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
058    return new DifferentialDriveWheelSpeeds(
059        chassisSpeeds.vxMetersPerSecond
060            - trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond,
061        chassisSpeeds.vxMetersPerSecond
062            + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
063  }
064
065  @Override
066  public Twist2d toTwist2d(DifferentialDriveWheelPositions wheelDeltas) {
067    return toTwist2d(wheelDeltas.leftMeters, wheelDeltas.rightMeters);
068  }
069
070  /**
071   * Performs forward kinematics to return the resulting Twist2d from the given left and right side
072   * distance deltas. This method is often used for odometry -- determining the robot's position on
073   * the field using changes in the distance driven by each wheel on the robot.
074   *
075   * @param leftDistanceMeters The distance measured by the left side encoder.
076   * @param rightDistanceMeters The distance measured by the right side encoder.
077   * @return The resulting Twist2d.
078   */
079  public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) {
080    return new Twist2d(
081        (leftDistanceMeters + rightDistanceMeters) / 2,
082        0,
083        (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters);
084  }
085}