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.Rotation2d;
008
009/**
010 * Represents the speed of a robot chassis. Although this struct contains similar members compared
011 * to a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
012 * w.r.t to the robot frame of reference, this ChassisSpeeds struct represents a velocity w.r.t to
013 * the robot frame of reference.
014 *
015 * <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
016 * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
017 * will often have all three components.
018 */
019@SuppressWarnings("MemberName")
020public class ChassisSpeeds {
021  /** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
022  public double vxMetersPerSecond;
023
024  /** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */
025  public double vyMetersPerSecond;
026
027  /** Represents the angular velocity of the robot frame. (CCW is +) */
028  public double omegaRadiansPerSecond;
029
030  /** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */
031  public ChassisSpeeds() {}
032
033  /**
034   * Constructs a ChassisSpeeds object.
035   *
036   * @param vxMetersPerSecond Forward velocity.
037   * @param vyMetersPerSecond Sideways velocity.
038   * @param omegaRadiansPerSecond Angular velocity.
039   */
040  public ChassisSpeeds(
041      double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) {
042    this.vxMetersPerSecond = vxMetersPerSecond;
043    this.vyMetersPerSecond = vyMetersPerSecond;
044    this.omegaRadiansPerSecond = omegaRadiansPerSecond;
045  }
046
047  /**
048   * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
049   * object.
050   *
051   * @param vxMetersPerSecond The component of speed in the x direction relative to the field.
052   *     Positive x is away from your alliance wall.
053   * @param vyMetersPerSecond The component of speed in the y direction relative to the field.
054   *     Positive y is to your left when standing behind the alliance wall.
055   * @param omegaRadiansPerSecond The angular rate of the robot.
056   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
057   *     considered to be zero when it is facing directly away from your alliance station wall.
058   *     Remember that this should be CCW positive.
059   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
060   */
061  public static ChassisSpeeds fromFieldRelativeSpeeds(
062      double vxMetersPerSecond,
063      double vyMetersPerSecond,
064      double omegaRadiansPerSecond,
065      Rotation2d robotAngle) {
066    return new ChassisSpeeds(
067        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
068        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
069        omegaRadiansPerSecond);
070  }
071
072  @Override
073  public String toString() {
074    return String.format(
075        "ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
076        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
077  }
078}