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.geometry.Twist2d;
008
009/**
010 * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot
011 * code should not use this directly- Instead, use the particular type for your drivetrain (e.g.,
012 * {@link DifferentialDriveKinematics}).
013 *
014 * @param <S> The type of the wheel speeds.
015 * @param <P> The type of the wheel positions.
016 */
017public interface Kinematics<S, P> {
018  /**
019   * Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This
020   * method is often used for odometry -- determining the robot's position on the field using data
021   * from the real-world speed of each wheel on the robot.
022   *
023   * @param wheelSpeeds The speeds of the wheels.
024   * @return The chassis speed.
025   */
026  ChassisSpeeds toChassisSpeeds(S wheelSpeeds);
027
028  /**
029   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
030   * method is often used to convert joystick values into wheel speeds.
031   *
032   * @param chassisSpeeds The desired chassis speed.
033   * @return The wheel speeds.
034   */
035  S toWheelSpeeds(ChassisSpeeds chassisSpeeds);
036
037  /**
038   * Performs forward kinematics to return the resulting Twist2d from the given change in wheel
039   * positions. This method is often used for odometry -- determining the robot's position on the
040   * field using changes in the distance driven by each wheel on the robot.
041   *
042   * @param start The starting distances driven by the wheels.
043   * @param end The ending distances driven by the wheels.
044   * @return The resulting Twist2d in the robot's movement.
045   */
046  Twist2d toTwist2d(P start, P end);
047}