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