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.geometry.Twist2d;
011import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto;
012import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct;
013import edu.wpi.first.units.measure.Distance;
014import edu.wpi.first.util.protobuf.ProtobufSerializable;
015import edu.wpi.first.util.struct.StructSerializable;
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        ProtobufSerializable,
028        StructSerializable {
029  /** Differential drive trackwidth in meters. */
030  public final double trackwidth;
031
032  /** DifferentialDriveKinematics protobuf for serialization. */
033  public static final DifferentialDriveKinematicsProto proto =
034      new DifferentialDriveKinematicsProto();
035
036  /** DifferentialDriveKinematics struct for serialization. */
037  public static final DifferentialDriveKinematicsStruct struct =
038      new DifferentialDriveKinematicsStruct();
039
040  /**
041   * Constructs a differential drive kinematics object.
042   *
043   * @param trackwidth The trackwidth of the drivetrain in meters. Theoretically, this is the
044   *     distance between the left wheels and right wheels. However, the empirical value may be
045   *     larger than the physical measured value due to scrubbing effects.
046   */
047  public DifferentialDriveKinematics(double trackwidth) {
048    this.trackwidth = trackwidth;
049    MathSharedStore.reportUsage("DifferentialDriveKinematics", "");
050  }
051
052  /**
053   * Constructs a differential drive kinematics object.
054   *
055   * @param trackwidth The trackwidth of the drivetrain in meters. Theoretically, this is the
056   *     distance between the left wheels and right wheels. However, the empirical value may be
057   *     larger than the physical measured value due to scrubbing effects.
058   */
059  public DifferentialDriveKinematics(Distance trackwidth) {
060    this(trackwidth.in(Meters));
061  }
062
063  /**
064   * Returns a chassis speed from left and right component velocities using forward kinematics.
065   *
066   * @param wheelSpeeds The left and right velocities.
067   * @return The chassis speed.
068   */
069  @Override
070  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
071    return new ChassisSpeeds(
072        (wheelSpeeds.left + wheelSpeeds.right) / 2,
073        0,
074        (wheelSpeeds.right - wheelSpeeds.left) / trackwidth);
075  }
076
077  /**
078   * Returns left and right component velocities from a chassis speed using inverse kinematics.
079   *
080   * @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the
081   *     chassis' speed.
082   * @return The left and right velocities.
083   */
084  @Override
085  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
086    return new DifferentialDriveWheelSpeeds(
087        chassisSpeeds.vx - trackwidth / 2 * chassisSpeeds.omega,
088        chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega);
089  }
090
091  @Override
092  public Twist2d toTwist2d(
093      DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) {
094    return toTwist2d(end.left - start.left, end.right - start.right);
095  }
096
097  /**
098   * Performs forward kinematics to return the resulting Twist2d from the given left and right side
099   * distance deltas. This method is often used for odometry -- determining the robot's position on
100   * the field using changes in the distance driven by each wheel on the robot.
101   *
102   * @param leftDistance The distance measured by the left side encoder in meters.
103   * @param rightDistance The distance measured by the right side encoder in meters.
104   * @return The resulting Twist2d.
105   */
106  public Twist2d toTwist2d(double leftDistance, double rightDistance) {
107    return new Twist2d(
108        (leftDistance + rightDistance) / 2, 0, (rightDistance - leftDistance) / trackwidth);
109  }
110
111  @Override
112  public DifferentialDriveWheelPositions copy(DifferentialDriveWheelPositions positions) {
113    return new DifferentialDriveWheelPositions(positions.left, positions.right);
114  }
115
116  @Override
117  public void copyInto(
118      DifferentialDriveWheelPositions positions, DifferentialDriveWheelPositions output) {
119    output.left = positions.left;
120    output.right = positions.right;
121  }
122
123  @Override
124  public DifferentialDriveWheelPositions interpolate(
125      DifferentialDriveWheelPositions startValue,
126      DifferentialDriveWheelPositions endValue,
127      double t) {
128    return startValue.interpolate(endValue, t);
129  }
130}