diff --git a/wpilibc/src/main/native/include/frc/AnalogGyro.h b/wpilibc/src/main/native/include/frc/AnalogGyro.h deleted file mode 100644 index 6a2d259b77..0000000000 --- a/wpilibc/src/main/native/include/frc/AnalogGyro.h +++ /dev/null @@ -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 - -#include -#include -#include - -#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 { - 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 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 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 GetAnalogInput() const { return nullptr; } - - void InitSendable(wpi::SendableBuilder& builder) override {} -}; - -} // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp index 9470a7ad3a..f23041ba9f 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp @@ -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()}); } diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h index bc57f727d4..8bde23d446 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h @@ -6,8 +6,8 @@ #include -#include #include +#include #include #include #include @@ -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 diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp index 9d7e28102d..e5ba1fe348 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp @@ -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() { diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h index d0976216da..397ef1e9fa 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h @@ -6,9 +6,9 @@ #include -#include #include #include +#include #include #include #include @@ -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{}, diff --git a/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp index 5aee5916f9..6c97d58ec1 100644 --- a/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp @@ -4,8 +4,8 @@ #include -#include #include +#include #include #include #include @@ -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}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp index e9ee85c8ff..960587e9d5 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp @@ -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 #include +#include #include #include #include @@ -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}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp index 456a6cc5b5..a0ea4e0d2c 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp @@ -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()); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h index 971fe4cc26..9db4ddf4b2 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h @@ -6,8 +6,8 @@ #include -#include #include +#include #include #include #include @@ -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 diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp index 8e6369cf3a..964c47382d 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp @@ -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 diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h index 2dcc50c7fb..55c73e4b24 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h @@ -6,8 +6,8 @@ #include -#include #include +#include #include #include #include @@ -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}}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp index 0030d01681..0cf80df0b5 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp @@ -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] diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h index 798442de7d..cc2cc17c29 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h @@ -6,8 +6,8 @@ #include -#include #include +#include #include #include #include @@ -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 m_controller{ DriveConstants::kTurnP, diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp index 38606156ea..bf5d977f40 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp @@ -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); } diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h index 21378f8671..450809a0d6 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h @@ -6,8 +6,8 @@ #include -#include #include +#include #include #include #include @@ -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 diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp index 39d711b622..30ebe1fe6c 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp @@ -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()}); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h index 7f6f9cf4e5..33ef1fbdd9 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h @@ -6,7 +6,7 @@ #include -#include +#include #include #include #include @@ -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()}}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp index be7f3592f5..0b2d5e7d79 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp @@ -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()}); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h index e71dc43bdf..5473d0bcb5 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h @@ -6,7 +6,7 @@ #include -#include +#include #include #include #include @@ -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, diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java deleted file mode 100644 index eb5a1c5679..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java +++ /dev/null @@ -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. - * - *

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. - * - *

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}. - * - *

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. - * - *

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. - * - *

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. - * - *

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 clockwise when looked at from the top. - * It needs to follow the NED axis convention. - * - *

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. - * - *

The rate is based on the most recent reading of the gyro analog value - * - *

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); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java index cce0831733..bf7662feee 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java @@ -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()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index e23bc65e42..9e6d372b04 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -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. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java index bb7b913b65..f68b1abff2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java @@ -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); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java index ff7a255364..120c7f67f0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java @@ -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()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index 9aa5c049a3..811d6bc843 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -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()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index 6ce731d5ca..8f72611864 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -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. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java index 5b2af06dfd..18fa35aa1a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java @@ -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())) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index b5e86b8bcc..1ed91764d5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -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. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index 2cfbd938df..ac460f22b1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -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(), diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java index 0f155e6eed..409351322e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java @@ -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(),