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 static edu.wpi.first.units.Units.Meters;
008
009import edu.wpi.first.math.MathSharedStore;
010import edu.wpi.first.math.MathUsageId;
011import edu.wpi.first.math.geometry.Twist2d;
012import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto;
013import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct;
014import edu.wpi.first.units.Distance;
015import edu.wpi.first.units.Measure;
016
017/**
018 * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
019 * velocities for a differential drive.
020 *
021 * <p>Inverse kinematics converts a desired chassis speed into left and right velocity components
022 * whereas forward kinematics converts left and right component velocities into a linear and angular
023 * chassis speed.
024 */
025public class DifferentialDriveKinematics
026    implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions> {
027  public final double trackWidthMeters;
028
029  public static final DifferentialDriveKinematicsProto proto =
030      new DifferentialDriveKinematicsProto();
031  public static final DifferentialDriveKinematicsStruct struct =
032      new DifferentialDriveKinematicsStruct();
033
034  /**
035   * Constructs a differential drive kinematics object.
036   *
037   * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is the distance
038   *     between the left wheels and right wheels. However, the empirical value may be larger than
039   *     the physical measured value due to scrubbing effects.
040   */
041  public DifferentialDriveKinematics(double trackWidthMeters) {
042    this.trackWidthMeters = trackWidthMeters;
043    MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1);
044  }
045
046  /**
047   * Constructs a differential drive kinematics object.
048   *
049   * @param trackWidth The track width of the drivetrain. Theoretically, this is the distance
050   *     between the left wheels and right wheels. However, the empirical value may be larger than
051   *     the physical measured value due to scrubbing effects.
052   */
053  public DifferentialDriveKinematics(Measure<Distance> trackWidth) {
054    this(trackWidth.in(Meters));
055  }
056
057  /**
058   * Returns a chassis speed from left and right component velocities using forward kinematics.
059   *
060   * @param wheelSpeeds The left and right velocities.
061   * @return The chassis speed.
062   */
063  @Override
064  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
065    return new ChassisSpeeds(
066        (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
067        0,
068        (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters);
069  }
070
071  /**
072   * Returns left and right component velocities from a chassis speed using inverse kinematics.
073   *
074   * @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the
075   *     chassis' speed.
076   * @return The left and right velocities.
077   */
078  @Override
079  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
080    return new DifferentialDriveWheelSpeeds(
081        chassisSpeeds.vxMetersPerSecond
082            - trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond,
083        chassisSpeeds.vxMetersPerSecond
084            + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
085  }
086
087  @Override
088  public Twist2d toTwist2d(
089      DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) {
090    return toTwist2d(end.leftMeters - start.leftMeters, end.rightMeters - start.rightMeters);
091  }
092
093  /**
094   * Performs forward kinematics to return the resulting Twist2d from the given left and right side
095   * distance deltas. This method is often used for odometry -- determining the robot's position on
096   * the field using changes in the distance driven by each wheel on the robot.
097   *
098   * @param leftDistanceMeters The distance measured by the left side encoder.
099   * @param rightDistanceMeters The distance measured by the right side encoder.
100   * @return The resulting Twist2d.
101   */
102  public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) {
103    return new Twist2d(
104        (leftDistanceMeters + rightDistanceMeters) / 2,
105        0,
106        (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters);
107  }
108}