WPILibC++  2020.3.2-60-g3011ebe
ChassisSpeeds.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <units/units.h>
11 
12 #include "frc/geometry/Rotation2d.h"
13 
14 namespace frc {
26 struct ChassisSpeeds {
30  units::meters_per_second_t vx = 0_mps;
31 
35  units::meters_per_second_t vy = 0_mps;
36 
40  units::radians_per_second_t omega = 0_rad_per_s;
41 
59  units::meters_per_second_t vx, units::meters_per_second_t vy,
60  units::radians_per_second_t omega, const Rotation2d& robotAngle) {
61  return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
62  -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
63  }
64 };
65 } // namespace frc
frc::Rotation2d::Cos
double Cos() const
Returns the cosine of the rotation.
Definition: Rotation2d.h:160
frc::Rotation2d
A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:22
frc::ChassisSpeeds::FromFieldRelativeSpeeds
static ChassisSpeeds FromFieldRelativeSpeeds(units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, const Rotation2d &robotAngle)
Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds object.
Definition: ChassisSpeeds.h:58
frc::ChassisSpeeds
Represents the speed of a robot chassis.
Definition: ChassisSpeeds.h:26
frc::Rotation2d::Sin
double Sin() const
Returns the sine of the rotation.
Definition: Rotation2d.h:167
frc::ChassisSpeeds::omega
units::radians_per_second_t omega
Represents the angular velocity of the robot frame.
Definition: ChassisSpeeds.h:40
frc::ChassisSpeeds::vy
units::meters_per_second_t vy
Represents strafe velocity w.r.t the robot frame of reference.
Definition: ChassisSpeeds.h:35
frc
A class that enforces constraints on the differential drive kinematics.
Definition: PDPSim.h:16
frc::ChassisSpeeds::vx
units::meters_per_second_t vx
Represents forward velocity w.r.t the robot frame of reference.
Definition: ChassisSpeeds.h:30