Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib, examples] Remove AnalogGyro (#8205)

Browse Source
This commit is contained in:
Ryan Blue
2025-10-10 15:44:39 -04:00
committed by GitHub
parent 35e4a18e86
commit 33f91589b4
30 changed files with 109 additions and 563 deletions
Download Patch File Download Diff File Expand all files Collapse all files

216
wpilibc/src/main/native/include/frc/AnalogGyro.h
View File

@@ -1,216 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <memory>
#include <hal/Types.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/geometry/Rotation2d.h"
namespace frc {
class AnalogInput;
/**
* Use a rate gyro to return the robots heading relative to a starting position.
* The Gyro class tracks the robots heading based on the starting position. As
* the robot rotates the new heading is computed by integrating the rate of
* rotation returned by the sensor. When the class is instantiated, it does a
* short calibration routine where it samples the gyro while at rest to
* determine the default offset. This is subtracted from each sample to
* determine the heading. This gyro class must be used with a channel that is
* assigned one of the Analog accumulators from the FPGA. See AnalogInput for
* the current accumulator assignments.
*
* This class is for gyro sensors that connect to an analog input.
*/
class AnalogGyro : public wpi::Sendable,
public wpi::SendableHelper<AnalogGyro> {
public:
/**
* %Gyro constructor using the Analog Input channel number.
*
* @param channel The analog channel the gyro is connected to. Gyros can only
* be used on on-board Analog Inputs 0-1.
*/
explicit AnalogGyro(int channel) {}
/**
* Gyro constructor with a precreated AnalogInput object.
*
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor.
*
* Gyros can only be used on on-board channels 0-1.
*
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
*/
explicit AnalogGyro(AnalogInput* channel) {}
/**
* %Gyro constructor with a precreated AnalogInput object.
*
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor.
*
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
*/
explicit AnalogGyro(std::shared_ptr<AnalogInput> channel) {}
/**
* %Gyro constructor using the Analog Input channel number with parameters for
* presetting the center and offset values. Bypasses calibration.
*
* @param channel The analog channel the gyro is connected to. Gyros can only
* be used on on-board Analog Inputs 0-1.
* @param center Preset uncalibrated value to use as the accumulator center
* value.
* @param offset Preset uncalibrated value to use as the gyro offset.
*/
AnalogGyro(int channel, int center, double offset) {}
/**
* %Gyro constructor with a precreated AnalogInput object and calibrated
* parameters.
*
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor.
*
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
* @param center Preset uncalibrated value to use as the accumulator center
* value.
* @param offset Preset uncalibrated value to use as the gyro offset.
*/
AnalogGyro(std::shared_ptr<AnalogInput> channel, int center, double offset) {}
AnalogGyro(AnalogGyro&& rhs) = default;
AnalogGyro& operator=(AnalogGyro&& rhs) = default;
~AnalogGyro() override = default;
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by the
* oversampling rate, the gyro type and the A/D calibration values. The angle
* is continuous, that is it will continue from 360->361 degrees. This allows
* algorithms that wouldn't want to see a discontinuity in the gyro output as
* it sweeps from 360 to 0 on the second time around.
*
* @return The current heading of the robot in degrees. This heading is based
* on integration of the returned rate from the gyro.
*/
double GetAngle() const { return 0; }
/**
* Return the rate of rotation of the gyro
*
* The rate is based on the most recent reading of the gyro analog value
*
* @return the current rate in degrees per second
*/
double GetRate() const { return 0; }
/**
* Return the gyro center value. If run after calibration,
* the center value can be used as a preset later.
*
* @return the current center value
*/
virtual int GetCenter() const { return 0; }
/**
* Return the gyro offset value. If run after calibration,
* the offset value can be used as a preset later.
*
* @return the current offset value
*/
virtual double GetOffset() const { return 0; }
/**
* Set the gyro sensitivity.
*
* This takes the number of volts/degree/second sensitivity of the gyro and
* uses it in subsequent calculations to allow the code to work with multiple
* gyros. This value is typically found in the gyro datasheet.
*
* @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second
*/
void SetSensitivity(double voltsPerDegreePerSecond) {}
/**
* Set the size of the neutral zone.
*
* Any voltage from the gyro less than this amount from the center is
* considered stationary. Setting a deadband will decrease the amount of
* drift when the gyro isn't rotating, but will make it less accurate.
*
* @param volts The size of the deadband in volts
*/
void SetDeadband(double volts) {}
/**
* Reset the gyro.
*
* Resets the gyro to a heading of zero. This can be used if there is
* significant drift in the gyro and it needs to be recalibrated after it has
* been running.
*/
void Reset() {}
/**
* Initialize the gyro.
*
* Calibration is handled by Calibrate().
*/
void InitGyro() {}
/**
* Calibrate the gyro by running for a number of samples and computing the
* center value. Then use the center value as the Accumulator center value for
* subsequent measurements.
*
* It's important to make sure that the robot is not moving while the
* centering calculations are in progress, this is typically done when the
* robot is first turned on while it's sitting at rest before the competition
* starts.
*/
void Calibrate() {}
/**
* Return the heading of the robot as a Rotation2d.
*
* The angle is continuous, that is it will continue from 360 to 361 degrees.
* This allows algorithms that wouldn't want to see a discontinuity in the
* gyro output as it sweeps past from 360 to 0 on the second time around.
*
* The angle is expected to increase as the gyro turns counterclockwise when
* looked at from the top. It needs to follow the NWU axis convention.
*
* @return the current heading of the robot as a Rotation2d. This heading is
* based on integration of the returned rate from the gyro.
*/
Rotation2d GetRotation2d() const { return {}; }
/**
* Gets the analog input for the gyro.
*
* @return AnalogInput
*/
std::shared_ptr<AnalogInput> GetAnalogInput() const { return nullptr; }
void InitSendable(wpi::SendableBuilder& builder) override {}
};
} // namespace frc

2
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
View File

@@ -22,7 +22,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_gyro.GetRotation2d(),
m_odometry.Update(m_imu.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()});
}

8
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
View File

@@ -6,8 +6,8 @@
#include <numbers>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/OnboardIMU.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
@@ -32,7 +32,7 @@ class Drivetrain {
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.SetInverted(true);
m_gyro.Reset();
m_imu.ResetYaw();
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
// resolution.
@@ -71,11 +71,11 @@ class Drivetrain {
frc::PIDController m_leftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_rightPIDController{1.0, 0.0, 0.0};
frc::AnalogGyro m_gyro{0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
frc::DifferentialDriveKinematics m_kinematics{kTrackwidth};
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
m_imu.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own

5
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -15,7 +15,7 @@ Drivetrain::Drivetrain() {
// gearbox is constructed, you might have to invert the left side instead.
m_rightLeader.SetInverted(true);
m_gyro.Reset();
m_imu.ResetYaw();
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
@@ -84,7 +84,7 @@ frc::Pose3d Drivetrain::ObjectToRobotPose(
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_gyro.GetRotation2d(),
m_poseEstimator.Update(m_imu.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()});
@@ -125,6 +125,7 @@ void Drivetrain::SimulationPeriodic() {
m_drivetrainSimulator.GetRightPosition().value());
m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
// m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
// // TODO(Ryan): fixup when sim implemented
}
void Drivetrain::Periodic() {

6
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
View File

@@ -6,9 +6,9 @@
#include <numbers>
#include <frc/AnalogGyro.h>
#include <frc/ComputerVisionUtil.h>
#include <frc/Encoder.h>
#include <frc/OnboardIMU.h>
#include <frc/RobotController.h>
#include <frc/Timer.h>
#include <frc/apriltag/AprilTagFieldLayout.h>
@@ -144,7 +144,7 @@ class Drivetrain {
frc::PIDController m_leftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_rightPIDController{1.0, 0.0, 0.0};
frc::AnalogGyro m_gyro{0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
frc::DifferentialDriveKinematics m_kinematics{kTrackwidth};
@@ -152,7 +152,7 @@ class Drivetrain {
// robot!
frc::DifferentialDrivePoseEstimator m_poseEstimator{
m_kinematics,
m_gyro.GetRotation2d(),
m_imu.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()},
frc::Pose2d{},

15
wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
View File

@@ -4,8 +4,8 @@
#include <cmath>
#include <frc/AnalogGyro.h>
#include <frc/Joystick.h>
#include <frc/OnboardIMU.h>
#include <frc/TimedRobot.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMSparkMax.h>
@@ -18,7 +18,6 @@
class Robot : public frc::TimedRobot {
public:
Robot() {
m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
@@ -34,7 +33,8 @@ class Robot : public frc::TimedRobot {
* angle.
*/
void TeleopPeriodic() override {
double turningValue = (kAngleSetpoint - m_gyro.GetAngle()) * kP;
double turningValue =
(kAngleSetpoint - m_imu.GetRotation2d().Degrees().value()) * kP;
m_drive.ArcadeDrive(-m_joystick.GetY(), -turningValue);
}
@@ -42,13 +42,10 @@ class Robot : public frc::TimedRobot {
static constexpr double kAngleSetpoint = 0.0;
static constexpr double kP = 0.005; // Proportional turning constant
// Gyro calibration constant, may need to be adjusted. Gyro value of 360 is
// set to correspond to one full revolution.
static constexpr double kVoltsPerDegreePerSecond = 0.0128;
static constexpr int kLeftMotorPort = 0;
static constexpr int kRightMotorPort = 1;
static constexpr int kGyroPort = 0;
static constexpr frc::OnboardIMU::MountOrientation kIMUMountOrientation =
frc::OnboardIMU::kFlat;
static constexpr int kJoystickPort = 0;
frc::PWMSparkMax m_left{kLeftMotorPort};
@@ -56,7 +53,7 @@ class Robot : public frc::TimedRobot {
frc::DifferentialDrive m_drive{[&](double output) { m_left.Set(output); },
[&](double output) { m_right.Set(output); }};
frc::AnalogGyro m_gyro{kGyroPort};
frc::OnboardIMU m_imu{kIMUMountOrientation};
frc::Joystick m_joystick{kJoystickPort};
};

15
wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
View File

@@ -2,8 +2,8 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <frc/AnalogGyro.h>
#include <frc/Joystick.h>
#include <frc/OnboardIMU.h>
#include <frc/TimedRobot.h>
#include <frc/drive/MecanumDrive.h>
#include <frc/motorcontrol/PWMSparkMax.h>
@@ -25,8 +25,6 @@ class Robot : public frc::TimedRobot {
// match your robot.
m_frontRight.SetInverted(true);
m_rearRight.SetInverted(true);
m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
}
/**
@@ -34,19 +32,16 @@ class Robot : public frc::TimedRobot {
*/
void TeleopPeriodic() override {
m_robotDrive.DriveCartesian(-m_joystick.GetY(), -m_joystick.GetX(),
-m_joystick.GetZ(), m_gyro.GetRotation2d());
-m_joystick.GetZ(), m_imu.GetRotation2d());
}
private:
// Gyro calibration constant, may need to be adjusted. Gyro value of 360 is
// set to correspond to one full revolution.
static constexpr double kVoltsPerDegreePerSecond = 0.0128;
static constexpr int kFrontLeftMotorPort = 0;
static constexpr int kRearLeftMotorPort = 1;
static constexpr int kFrontRightMotorPort = 2;
static constexpr int kRearRightMotorPort = 3;
static constexpr int kGyroPort = 0;
static constexpr frc::OnboardIMU::MountOrientation kIMUMountOrientation =
frc::OnboardIMU::kFlat;
static constexpr int kJoystickPort = 0;
frc::PWMSparkMax m_frontLeft{kFrontLeftMotorPort};
@@ -59,7 +54,7 @@ class Robot : public frc::TimedRobot {
[&](double output) { m_frontRight.Set(output); },
[&](double output) { m_rearRight.Set(output); }};
frc::AnalogGyro m_gyro{kGyroPort};
frc::OnboardIMU m_imu{kIMUMountOrientation};
frc::Joystick m_joystick{kJoystickPort};
};

4
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -55,12 +55,12 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::second_t period) {
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d());
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_imu.GetRotation2d());
}
SetSpeeds(m_kinematics.ToWheelSpeeds(chassisSpeeds.Discretize(period))
.Desaturate(kMaxSpeed));
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_gyro.GetRotation2d(), GetCurrentWheelDistances());
m_odometry.Update(m_imu.GetRotation2d(), GetCurrentWheelDistances());
}

8
wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
View File

@@ -6,8 +6,8 @@
#include <numbers>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/OnboardIMU.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/geometry/Translation2d.h>
@@ -22,7 +22,7 @@
class Drivetrain {
public:
Drivetrain() {
m_gyro.Reset();
m_imu.ResetYaw();
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
@@ -64,13 +64,13 @@ class Drivetrain {
frc::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
frc::AnalogGyro m_gyro{0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
frc::MecanumDriveKinematics m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
frc::MecanumDriveOdometry m_odometry{m_kinematics, m_gyro.GetRotation2d(),
frc::MecanumDriveOdometry m_odometry{m_kinematics, m_imu.GetRotation2d(),
GetCurrentWheelDistances()};
// Gains are for example purposes only - must be determined for your own

4
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -58,7 +58,9 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentDistances());
m_poseEstimator.Update(
m_imu.GetRotation2d(),
GetCurrentDistances()); // TODO(Ryan): fixup when sim implemented
// Also apply vision measurements. We use 0.3 seconds in the past as an
// example -- on a real robot, this must be calculated based either on latency

10
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
View File

@@ -6,8 +6,8 @@
#include <numbers>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/OnboardIMU.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/estimator/MecanumDrivePoseEstimator.h>
@@ -23,7 +23,7 @@
class Drivetrain {
public:
Drivetrain() {
m_gyro.Reset();
m_imu.ResetYaw();
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
@@ -64,7 +64,7 @@ class Drivetrain {
frc::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
frc::AnalogGyro m_gyro{0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
frc::MecanumDriveKinematics m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
@@ -77,6 +77,6 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::MecanumDrivePoseEstimator m_poseEstimator{
m_kinematics, m_gyro.GetRotation2d(), GetCurrentDistances(),
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.1, 0.1, 0.1}};
m_kinematics, m_imu.GetRotation2d(), GetCurrentDistances(),
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.1, 0.1, 0.1}};
};

4
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp
View File

@@ -62,10 +62,10 @@ frc2::CommandPtr Drive::DriveDistanceCommand(units::meter_t distance,
frc2::CommandPtr Drive::TurnToAngleCommand(units::degree_t angle) {
return StartRun(
[this] { m_controller.Reset(m_gyro.GetRotation2d().Degrees()); },
[this] { m_controller.Reset(m_imu.GetRotation2d().Degrees()); },
[this, angle] {
m_drive.ArcadeDrive(
0, m_controller.Calculate(m_gyro.GetRotation2d().Degrees(),
0, m_controller.Calculate(m_imu.GetRotation2d().Degrees(),
angle) +
// Divide feedforward voltage by battery voltage to
// normalize it to [-1, 1]

4
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h
View File

@@ -6,8 +6,8 @@
#include <functional>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/OnboardIMU.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/drive/DifferentialDrive.h>
@@ -65,7 +65,7 @@ class Drive : public frc2::SubsystemBase {
DriveConstants::kRightEncoderPorts[1],
DriveConstants::kRightEncoderReversed};
frc::AnalogGyro m_gyro{0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
frc::ProfiledPIDController<units::radians> m_controller{
DriveConstants::kTurnP,

4
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
View File

@@ -24,14 +24,14 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_gyro.GetRotation2d(),
m_odometry.Update(m_imu.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()});
}
void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
m_drivetrainSimulator.SetPose(pose);
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
m_odometry.ResetPosition(m_imu.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}, pose);
}

8
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
View File

@@ -6,8 +6,8 @@
#include <numbers>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/OnboardIMU.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
@@ -29,7 +29,7 @@
class Drivetrain {
public:
Drivetrain() {
m_gyro.Reset();
m_imu.ResetYaw();
m_leftLeader.AddFollower(m_leftFollower);
m_rightLeader.AddFollower(m_rightFollower);
@@ -87,11 +87,11 @@ class Drivetrain {
frc::PIDController m_leftPIDController{8.5, 0.0, 0.0};
frc::PIDController m_rightPIDController{8.5, 0.0, 0.0};
frc::AnalogGyro m_gyro{0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
frc::DifferentialDriveKinematics m_kinematics{kTrackwidth};
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
m_imu.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own

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

@@ -10,7 +10,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::second_t period) {
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d());
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_imu.GetRotation2d());
}
chassisSpeeds = chassisSpeeds.Discretize(period);
@@ -25,7 +25,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_gyro.GetRotation2d(),
m_odometry.Update(m_imu.GetRotation2d(),
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()});
}

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

@@ -6,7 +6,7 @@
#include <numbers>
#include <frc/AnalogGyro.h>
#include <frc/OnboardIMU.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/kinematics/SwerveDriveOdometry.h>
@@ -18,7 +18,7 @@
*/
class Drivetrain {
public:
Drivetrain() { m_gyro.Reset(); }
Drivetrain() { m_imu.ResetYaw(); }
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
@@ -41,7 +41,7 @@ class Drivetrain {
SwerveModule m_backLeft{5, 6, 8, 9, 10, 11};
SwerveModule m_backRight{7, 8, 12, 13, 14, 15};
frc::AnalogGyro m_gyro{0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
frc::SwerveDriveKinematics<4> m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
@@ -49,7 +49,7 @@ class Drivetrain {
frc::SwerveDriveOdometry<4> m_odometry{
m_kinematics,
m_gyro.GetRotation2d(),
m_imu.GetRotation2d(),
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()}};
};

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -30,7 +30,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_gyro.GetRotation2d(),
m_poseEstimator.Update(m_imu.GetRotation2d(),
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()});

6
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
View File

@@ -6,7 +6,7 @@
#include <numbers>
#include <frc/AnalogGyro.h>
#include <frc/OnboardIMU.h>
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
@@ -19,7 +19,7 @@
*/
class Drivetrain {
public:
Drivetrain() { m_gyro.Reset(); }
Drivetrain() { m_imu.ResetYaw(); }
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
@@ -41,7 +41,7 @@ class Drivetrain {
SwerveModule m_backLeft{5, 6, 8, 9, 10, 11};
SwerveModule m_backRight{7, 8, 12, 13, 14, 15};
frc::AnalogGyro m_gyro{0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
frc::SwerveDriveKinematics<4> m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,

225
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
View File

@@ -1,225 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
/**
* Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
* tracks the robots heading based on the starting position. As the robot rotates the new heading is
* computed by integrating the rate of rotation returned by the sensor. When the class is
* instantiated, it does a short calibration routine where it samples the gyro while at rest to
* determine the default offset. This is subtracted from each sample to determine the heading.
*
* <p>This class is for gyro sensors that connect to an analog input.
*/
public class AnalogGyro implements Sendable, AutoCloseable {
private AnalogInput m_analog;
private boolean m_channelAllocated;
/** Initialize the gyro. Calibration is handled by calibrate(). */
private void initGyro() {
HAL.reportUsage("AnalogGyro", m_analog.getChannel(), "");
SendableRegistry.add(this, "AnalogGyro", m_analog.getChannel());
}
/**
* Calibrate the gyro by running for a number of samples and computing the center value. Then use
* the center value as the Accumulator center value for subsequent measurements.
*
* <p>It's important to make sure that the robot is not moving while the centering calculations
* are in progress, this is typically done when the robot is first turned on while it's sitting at
* rest before the competition starts.
*/
public void calibrate() {}
/**
* Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
*
* <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
* algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
* 360 to 0 on the second time around.
*
* <p>The angle is expected to increase as the gyro turns counterclockwise when looked at from the
* top. It needs to follow the NWU axis convention.
*
* <p>This heading is based on integration of the returned rate from the gyro.
*
* @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
*/
public Rotation2d getRotation2d() {
return Rotation2d.fromDegrees(-getAngle());
}
/**
* Gyro constructor using the channel number.
*
* @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
* channels 0-1.
*/
@SuppressWarnings("this-escape")
public AnalogGyro(int channel) {
this(new AnalogInput(channel));
m_channelAllocated = true;
SendableRegistry.addChild(this, m_analog);
}
/**
* Gyro constructor with a precreated analog channel object. Use this constructor when the analog
* channel needs to be shared.
*
* @param channel The AnalogInput object that the gyro is connected to. Gyros can only be used on
* on-board channels 0-1.
*/
@SuppressWarnings("this-escape")
public AnalogGyro(AnalogInput channel) {
requireNonNullParam(channel, "channel", "AnalogGyro");
m_analog = channel;
initGyro();
calibrate();
}
/**
* Gyro constructor using the channel number along with parameters for presetting the center and
* offset values. Bypasses calibration.
*
* @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
* channels 0-1.
* @param center Preset uncalibrated value to use as the accumulator center value.
* @param offset Preset uncalibrated value to use as the gyro offset.
*/
@SuppressWarnings("this-escape")
public AnalogGyro(int channel, int center, double offset) {
this(new AnalogInput(channel), center, offset);
m_channelAllocated = true;
SendableRegistry.addChild(this, m_analog);
}
/**
* Gyro constructor with a precreated analog channel object along with parameters for presetting
* the center and offset values. Bypasses calibration.
*
* @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
* channels 0-1.
* @param center Preset uncalibrated value to use as the accumulator center value.
* @param offset Preset uncalibrated value to use as the gyro offset.
*/
@SuppressWarnings({"this-escape", "PMD.UnusedFormalParameter"})
public AnalogGyro(AnalogInput channel, int center, double offset) {
requireNonNullParam(channel, "channel", "AnalogGyro");
m_analog = channel;
initGyro();
reset();
}
/**
* Reset the gyro.
*
* <p>Resets the gyro to a heading of zero. This can be used if there is significant drift in the
* gyro, and it needs to be recalibrated after it has been running.
*/
public void reset() {}
/** Delete (free) the accumulator and the analog components used for the gyro. */
@Override
public void close() {
SendableRegistry.remove(this);
if (m_analog != null && m_channelAllocated) {
m_analog.close();
}
m_analog = null;
}
/**
* Return the heading of the robot in degrees.
*
* <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
* algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
* 360 to 0 on the second time around.
*
* <p>The angle is expected to increase as the gyro turns clockwise when looked at from the top.
* It needs to follow the NED axis convention.
*
* <p>This heading is based on integration of the returned rate from the gyro.
*
* @return the current heading of the robot in degrees.
*/
public double getAngle() {
return 0.0;
}
/**
* Return the rate of rotation of the gyro.
*
* <p>The rate is based on the most recent reading of the gyro analog value
*
* <p>The rate is expected to be positive as the gyro turns clockwise when looked at from the top.
* It needs to follow the NED axis convention.
*
* @return the current rate in degrees per second
*/
public double getRate() {
return 0.0;
}
/**
* Return the gyro offset value set during calibration to use as a future preset.
*
* @return the current offset value
*/
public double getOffset() {
return 0.0;
}
/**
* Return the gyro center value set during calibration to use as a future preset.
*
* @return the current center value
*/
public int getCenter() {
return 0;
}
/**
* Set the gyro sensitivity. This takes the number of volts/degree/second sensitivity of the gyro
* and uses it in subsequent calculations to allow the code to work with multiple gyros. This
* value is typically found in the gyro datasheet.
*
* @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second.
*/
public void setSensitivity(double voltsPerDegreePerSecond) {}
/**
* Set the size of the neutral zone. Any voltage from the gyro less than this amount from the
* center is considered stationary. Setting a deadband will decrease the amount of drift when the
* gyro isn't rotating, but will make it less accurate.
*
* @param volts The size of the deadband in volts
*/
public void setDeadband(double volts) {}
/**
* Gets the analog input for the gyro.
*
* @return AnalogInput
*/
public AnalogInput getAnalogInput() {
return m_analog;
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
View File

@@ -10,8 +10,8 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/** Represents a differential drive style drivetrain. */
@@ -31,7 +31,7 @@ public class Drivetrain {
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
@@ -49,7 +49,7 @@ public class Drivetrain {
* gyro.
*/
public Drivetrain() {
m_gyro.reset();
m_imu.resetYaw();
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
@@ -70,7 +70,7 @@ public class Drivetrain {
m_odometry =
new DifferentialDriveOdometry(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/**
@@ -103,6 +103,6 @@ public class Drivetrain {
/** Updates the field-relative position. */
public void updateOdometry() {
m_odometry.update(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
}

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
View File

@@ -27,8 +27,8 @@ import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.DoubleArrayEntry;
import edu.wpi.first.networktables.DoubleArrayTopic;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
@@ -54,7 +54,7 @@ public class Drivetrain {
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
@@ -79,7 +79,7 @@ public class Drivetrain {
private final DifferentialDrivePoseEstimator m_poseEstimator =
new DifferentialDrivePoseEstimator(
m_kinematics,
m_gyro.getRotation2d(),
m_imu.getRotation2d(),
m_leftEncoder.getDistance(),
m_rightEncoder.getDistance(),
Pose2d.kZero,
@@ -103,7 +103,7 @@ public class Drivetrain {
* gyro.
*/
public Drivetrain(DoubleArrayTopic cameraToObjectTopic) {
m_gyro.reset();
m_imu.resetYaw();
m_leftLeader.addFollower(m_leftFollower);
m_rightLeader.addFollower(m_rightFollower);
@@ -220,7 +220,7 @@ public class Drivetrain {
/** Updates the field-relative position. */
public void updateOdometry() {
m_poseEstimator.update(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
// Publish cameraToObject transformation to networktables --this would normally be handled by
// the
@@ -254,7 +254,8 @@ public class Drivetrain {
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocity());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPosition());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocity());
// m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
// m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); // TODO(Ryan): fixup
// when sim implemented
}
/** This function is called periodically, no matter the mode. */

14
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
View File

@@ -5,8 +5,8 @@
package edu.wpi.first.wpilibj.examples.gyro;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
@@ -20,20 +20,17 @@ public class Robot extends TimedRobot {
private static final double kAngleSetpoint = 0.0;
private static final double kP = 0.005; // proportional turning constant
// gyro calibration constant, may need to be adjusted;
// gyro value of 360 is set to correspond to one full revolution
private static final double kVoltsPerDegreePerSecond = 0.0128;
private static final int kLeftMotorPort = 0;
private static final int kRightMotorPort = 1;
private static final int kGyroPort = 0;
private static final OnboardIMU.MountOrientation kIMUMountOrientation =
OnboardIMU.MountOrientation.kFlat;
private static final int kJoystickPort = 0;
private final PWMSparkMax m_leftDrive = new PWMSparkMax(kLeftMotorPort);
private final PWMSparkMax m_rightDrive = new PWMSparkMax(kRightMotorPort);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
private final AnalogGyro m_gyro = new AnalogGyro(kGyroPort);
private final OnboardIMU m_imu = new OnboardIMU(kIMUMountOrientation);
private final Joystick m_joystick = new Joystick(kJoystickPort);
/** Called once at the beginning of the robot program. */
@@ -41,7 +38,6 @@ public class Robot extends TimedRobot {
SendableRegistry.addChild(m_robotDrive, m_leftDrive);
SendableRegistry.addChild(m_robotDrive, m_rightDrive);
m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
@@ -54,7 +50,7 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
double turningValue = (kAngleSetpoint - m_gyro.getAngle()) * kP;
double turningValue = (kAngleSetpoint - m_imu.getRotation2d().getDegrees()) * kP;
m_robotDrive.arcadeDrive(-m_joystick.getY(), -turningValue);
}
}

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
View File

@@ -5,8 +5,8 @@
package edu.wpi.first.wpilibj.examples.gyromecanum;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
@@ -16,19 +16,16 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
* in relation to the starting orientation of the robot (field-oriented controls).
*/
public class Robot extends TimedRobot {
// gyro calibration constant, may need to be adjusted;
// gyro value of 360 is set to correspond to one full revolution
private static final double kVoltsPerDegreePerSecond = 0.0128;
private static final int kFrontLeftChannel = 0;
private static final int kRearLeftChannel = 1;
private static final int kFrontRightChannel = 2;
private static final int kRearRightChannel = 3;
private static final int kGyroPort = 0;
private static final OnboardIMU.MountOrientation kIMUMountOrientation =
OnboardIMU.MountOrientation.kFlat;
private static final int kJoystickPort = 0;
private final MecanumDrive m_robotDrive;
private final AnalogGyro m_gyro = new AnalogGyro(kGyroPort);
private final OnboardIMU m_imu = new OnboardIMU(kIMUMountOrientation);
private final Joystick m_joystick = new Joystick(kJoystickPort);
/** Called once at the beginning of the robot program. */
@@ -45,8 +42,6 @@ public class Robot extends TimedRobot {
m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);
m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
SendableRegistry.addChild(m_robotDrive, frontLeft);
SendableRegistry.addChild(m_robotDrive, rearLeft);
SendableRegistry.addChild(m_robotDrive, frontRight);
@@ -57,6 +52,6 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
m_robotDrive.driveCartesian(
-m_joystick.getY(), -m_joystick.getX(), -m_joystick.getZ(), m_gyro.getRotation2d());
-m_joystick.getY(), -m_joystick.getX(), -m_joystick.getZ(), m_imu.getRotation2d());
}
}

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -12,8 +12,8 @@ import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/** Represents a mecanum drive style drivetrain. */
@@ -41,21 +41,21 @@ public class Drivetrain {
private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
private final MecanumDriveKinematics m_kinematics =
new MecanumDriveKinematics(
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(m_kinematics, m_gyro.getRotation2d(), getCurrentDistances());
new MecanumDriveOdometry(m_kinematics, m_imu.getRotation2d(), getCurrentDistances());
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
/** Constructs a MecanumDrive and resets the gyro. */
public Drivetrain() {
m_gyro.reset();
m_imu.resetYaw();
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
@@ -127,13 +127,13 @@ public class Drivetrain {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
chassisSpeeds = chassisSpeeds.toRobotRelative(m_imu.getRotation2d());
}
setSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(kMaxSpeed));
}
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_odometry.update(m_gyro.getRotation2d(), getCurrentDistances());
m_odometry.update(m_imu.getRotation2d(), getCurrentDistances());
}
}

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -15,8 +15,8 @@ import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
@@ -45,7 +45,7 @@ public class Drivetrain {
private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
private final MecanumDriveKinematics m_kinematics =
new MecanumDriveKinematics(
@@ -56,7 +56,7 @@ public class Drivetrain {
private final MecanumDrivePoseEstimator m_poseEstimator =
new MecanumDrivePoseEstimator(
m_kinematics,
m_gyro.getRotation2d(),
m_imu.getRotation2d(),
getCurrentDistances(),
Pose2d.kZero,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
@@ -67,7 +67,7 @@ public class Drivetrain {
/** Constructs a MecanumDrive and resets the gyro. */
public Drivetrain() {
m_gyro.reset();
m_imu.resetYaw();
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
@@ -147,7 +147,7 @@ public class Drivetrain {
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentDistances());
m_poseEstimator.update(m_imu.getRotation2d(), getCurrentDistances());
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
View File

@@ -10,8 +10,8 @@ import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
@@ -49,7 +49,7 @@ public class Drive extends SubsystemBase {
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
private final ProfiledPIDController m_controller =
new ProfiledPIDController(
DriveConstants.kTurnP,
@@ -129,11 +129,11 @@ public class Drive extends SubsystemBase {
*/
public Command turnToAngleCommand(double angleDeg) {
return startRun(
() -> m_controller.reset(m_gyro.getRotation2d().getDegrees()),
() -> m_controller.reset(m_imu.getRotation2d().getDegrees()),
() ->
m_drive.arcadeDrive(
0,
m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg)
m_controller.calculate(m_imu.getRotation2d().getDegrees(), angleDeg)
// Divide feedforward voltage by battery voltage to normalize it to [-1, 1]
+ m_feedforward.calculate(m_controller.getSetpoint().velocity)
/ RobotController.getBatteryVoltage()))

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
View File

@@ -15,8 +15,8 @@ import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
@@ -45,13 +45,13 @@ public class Drivetrain {
private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0);
private final PIDController m_rightPIDController = new PIDController(8.5, 0, 0);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
private final DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(kTrackwidth);
private final DifferentialDriveOdometry m_odometry =
new DifferentialDriveOdometry(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
// Gains are for example purposes only - must be determined for your own
// robot!
@@ -114,14 +114,14 @@ public class Drivetrain {
/** Update robot odometry. */
public void updateOdometry() {
m_odometry.update(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/** Resets robot odometry. */
public void resetOdometry(Pose2d pose) {
m_drivetrainSimulator.setPose(pose);
m_odometry.resetPosition(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
m_imu.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
}
/** Check the current robot pose. */

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View File

@@ -9,7 +9,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.OnboardIMU;
/** Represents a swerve drive style drivetrain. */
public class Drivetrain {
@@ -26,7 +26,7 @@ public class Drivetrain {
private final SwerveModule m_backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
private final SwerveModule m_backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(
@@ -35,7 +35,7 @@ public class Drivetrain {
private final SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(
m_kinematics,
m_gyro.getRotation2d(),
m_imu.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
@@ -44,7 +44,7 @@ public class Drivetrain {
});
public Drivetrain() {
m_gyro.reset();
m_imu.resetYaw();
}
/**
@@ -59,7 +59,7 @@ public class Drivetrain {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
chassisSpeeds = chassisSpeeds.toRobotRelative(m_imu.getRotation2d());
}
chassisSpeeds = chassisSpeeds.discretize(period);
@@ -75,7 +75,7 @@ public class Drivetrain {
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_odometry.update(
m_gyro.getRotation2d(),
m_imu.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -12,7 +12,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.OnboardIMU;
import edu.wpi.first.wpilibj.Timer;
/** Represents a swerve drive style drivetrain. */
@@ -30,7 +30,7 @@ public class Drivetrain {
private final SwerveModule m_backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
private final SwerveModule m_backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final OnboardIMU m_imu = new OnboardIMU(OnboardIMU.MountOrientation.kFlat);
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(
@@ -41,7 +41,7 @@ public class Drivetrain {
private final SwerveDrivePoseEstimator m_poseEstimator =
new SwerveDrivePoseEstimator(
m_kinematics,
m_gyro.getRotation2d(),
m_imu.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
@@ -53,7 +53,7 @@ public class Drivetrain {
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
public Drivetrain() {
m_gyro.reset();
m_imu.resetYaw();
}
/**
@@ -86,7 +86,7 @@ public class Drivetrain {
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_poseEstimator.update(
m_gyro.getRotation2d(),
m_imu.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),