mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
841ef91c0f
The odometry classes previously took in the robot angle as an argument, meaning that users had to take care of offsetting the gyro themselves to accurately report the robot angle. This change will make it so that users will not have to worry about resetting gyros and adding offsets themselves, as this will be handled by the odometry classes.
79 lines
2.8 KiB
C++
79 lines
2.8 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#pragma once
|
|
|
|
#include <frc/AnalogGyro.h>
|
|
#include <frc/Encoder.h>
|
|
#include <frc/Spark.h>
|
|
#include <frc/SpeedControllerGroup.h>
|
|
#include <frc/controller/PIDController.h>
|
|
#include <frc/kinematics/DifferentialDriveKinematics.h>
|
|
#include <frc/kinematics/DifferentialDriveOdometry.h>
|
|
#include <units/units.h>
|
|
#include <wpi/math>
|
|
|
|
/**
|
|
* Represents a differential drive style drivetrain.
|
|
*/
|
|
class Drivetrain {
|
|
public:
|
|
Drivetrain() {
|
|
m_gyro.Reset();
|
|
// 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.
|
|
m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
|
|
kEncoderResolution);
|
|
m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
|
|
kEncoderResolution);
|
|
}
|
|
|
|
/**
|
|
* Get the robot angle as a Rotation2d.
|
|
*/
|
|
frc::Rotation2d GetAngle() const {
|
|
// Negating the angle because WPILib Gyros are CW positive.
|
|
return frc::Rotation2d(units::degree_t(-m_gyro.GetAngle()));
|
|
}
|
|
|
|
static constexpr units::meters_per_second_t kMaxSpeed =
|
|
3.0_mps; // 3 meters per second
|
|
static constexpr units::radians_per_second_t kMaxAngularSpeed{
|
|
wpi::math::pi}; // 1/2 rotation per second
|
|
|
|
frc::DifferentialDriveWheelSpeeds GetSpeeds() const;
|
|
void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
|
|
void Drive(units::meters_per_second_t xSpeed,
|
|
units::radians_per_second_t rot);
|
|
void UpdateOdometry();
|
|
|
|
private:
|
|
static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
|
|
static constexpr double kWheelRadius = 0.0508; // meters
|
|
static constexpr int kEncoderResolution = 4096;
|
|
|
|
frc::Spark m_leftMaster{1};
|
|
frc::Spark m_leftFollower{2};
|
|
frc::Spark m_rightMaster{3};
|
|
frc::Spark m_rightFollower{4};
|
|
|
|
frc::SpeedControllerGroup m_leftGroup{m_leftMaster, m_leftFollower};
|
|
frc::SpeedControllerGroup m_rightGroup{m_rightMaster, m_rightFollower};
|
|
|
|
frc::Encoder m_leftEncoder{0, 1};
|
|
frc::Encoder m_rightEncoder{2, 3};
|
|
|
|
frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
|
|
frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
|
|
|
|
frc::AnalogGyro m_gyro{0};
|
|
|
|
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
|
|
frc::DifferentialDriveOdometry m_odometry{m_kinematics, GetAngle()};
|
|
};
|