Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add Gyro::GetRotation2d() (#2555)

Browse Source
This commit is contained in:
Tyler Veness
2020-06-29 19:10:07 -07:00
committed by GitHub
parent d9c7bbd046
commit e50dbe0c43
25 changed files with 130 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,7 +12,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot, bool fieldRelative) {
auto states = m_kinematics.ToSwerveModuleStates(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, GetAngle())
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
@@ -26,6 +26,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(GetAngle(), m_frontLeft.GetState(), m_frontRight.GetState(),
m_backLeft.GetState(), m_backRight.GetState());
m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(),
m_frontRight.GetState(), m_backLeft.GetState(),
m_backRight.GetState());
}

12
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -22,14 +22,6 @@ class Drivetrain {
public:
Drivetrain() { m_gyro.Reset(); }
/**
* Get the robot angle as a Rotation2d.
*/
frc::Rotation2d GetAngle() const {
// Negating the angle because WPILib Gyros are CW positive.
return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
}
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative);
@@ -57,5 +49,5 @@ class Drivetrain {
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, GetAngle()};
frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, m_gyro.GetRotation2d()};
};