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