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 */
019public class ChassisSpeeds {
020  /** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
021  public double vxMetersPerSecond;
022
023  /** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */
024  public double vyMetersPerSecond;
025
026  /** Represents the angular velocity of the robot frame. (CCW is +) */
027  public double omegaRadiansPerSecond;
028
029  /** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */
030  public ChassisSpeeds() {}
031
032  /**
033   * Constructs a ChassisSpeeds object.
034   *
035   * @param vxMetersPerSecond Forward velocity.
036   * @param vyMetersPerSecond Sideways velocity.
037   * @param omegaRadiansPerSecond Angular velocity.
038   */
039  public ChassisSpeeds(
040      double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) {
041    this.vxMetersPerSecond = vxMetersPerSecond;
042    this.vyMetersPerSecond = vyMetersPerSecond;
043    this.omegaRadiansPerSecond = omegaRadiansPerSecond;
044  }
045
046  /**
047   * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
048   * object.
049   *
050   * @param vxMetersPerSecond The component of speed in the x direction relative to the field.
051   *     Positive x is away from your alliance wall.
052   * @param vyMetersPerSecond The component of speed in the y direction relative to the field.
053   *     Positive y is to your left when standing behind the alliance wall.
054   * @param omegaRadiansPerSecond The angular rate of the robot.
055   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
056   *     considered to be zero when it is facing directly away from your alliance station wall.
057   *     Remember that this should be CCW positive.
058   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
059   */
060  public static ChassisSpeeds fromFieldRelativeSpeeds(
061      double vxMetersPerSecond,
062      double vyMetersPerSecond,
063      double omegaRadiansPerSecond,
064      Rotation2d robotAngle) {
065    return new ChassisSpeeds(
066        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
067        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
068        omegaRadiansPerSecond);
069  }
070
071  /**
072   * Converts a user provided field-relative ChassisSpeeds object into a robot-relative
073   * ChassisSpeeds object.
074   *
075   * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds in the field frame
076   *     of reference. Positive x is away from your alliance wall. Positive y is to your left when
077   *     standing behind the alliance wall.
078   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
079   *     considered to be zero when it is facing directly away from your alliance station wall.
080   *     Remember that this should be CCW positive.
081   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
082   */
083  public static ChassisSpeeds fromFieldRelativeSpeeds(
084      ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
085    return fromFieldRelativeSpeeds(
086        fieldRelativeSpeeds.vxMetersPerSecond,
087        fieldRelativeSpeeds.vyMetersPerSecond,
088        fieldRelativeSpeeds.omegaRadiansPerSecond,
089        robotAngle);
090  }
091
092  @Override
093  public String toString() {
094    return String.format(
095        "ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
096        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
097  }
098}