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

20
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.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. */
@@ -7,7 +7,6 @@
#include "subsystems/DriveSubsystem.h"
#include <frc/geometry/Rotation2d.h>
#include <units/units.h>
#include "Constants.h"
@@ -30,9 +29,7 @@ DriveSubsystem::DriveSubsystem()
m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
kRearRightEncoderReversed},
m_odometry{kDriveKinematics,
frc::Rotation2d(units::degree_t(GetHeading())),
frc::Pose2d()} {
m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d()} {
// Set the distance per pulse for the encoders
m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -43,7 +40,7 @@ DriveSubsystem::DriveSubsystem()
void DriveSubsystem::Periodic() {
// Implementation of subsystem periodic method goes here.
m_odometry.Update(
frc::Rotation2d(units::degree_t(GetHeading())),
m_gyro.GetRotation2d(),
frc::MecanumDriveWheelSpeeds{
units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
units::meters_per_second_t(m_rearLeftEncoder.GetRate()),
@@ -103,19 +100,16 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
double DriveSubsystem::GetHeading() {
return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
units::degree_t DriveSubsystem::GetHeading() const {
return m_gyro.GetRotation2d().Degrees();
}
void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
double DriveSubsystem::GetTurnRate() {
return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
}
double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
m_odometry.ResetPosition(pose,
frc::Rotation2d(units::degree_t(GetHeading())));
m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
}

4
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.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. */
@@ -50,8 +50,6 @@ constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
constexpr bool kGyroReversed = false;
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These characterization values MUST be determined either experimentally or
// theoretically for *your* robot's drive. The RobotPy Characterization

6
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.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. */
@@ -103,9 +103,9 @@ class DriveSubsystem : public frc2::SubsystemBase {
/**
* Returns the heading of the robot.
*
* @return the robot's heading in degrees, from 180 to 180
* @return the robot's heading in degrees, from -180 to 180
*/
double GetHeading();
units::degree_t GetHeading() const;
/**
* Zeroes the heading of the robot.