mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpimath] Deprecate RamseteController (#6494)
LTVUnicycleController is a drop-in replacement with better tuning knobs. The RamseteCommand examples were removed instead of retrofitted with LTVUnicycleController because we're planning on removing the command controller classes anyway, so it would be wasted effort. The SimpleDifferentialDriveSimulation example shows direct LTVUnicycleController usage.
This commit is contained in:
Tyler Veness
@@ -1,10 +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.
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
const frc::DifferentialDriveKinematics DriveConstants::kDriveKinematics(
|
||||
kTrackwidth);
|
||||
@@ -1,72 +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.
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
void Robot::RobotInit() {}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
*/
|
||||
void Robot::RobotPeriodic() {
|
||||
frc2::CommandScheduler::GetInstance().Run();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode. You
|
||||
* can use it to reset any subsystem information you want to clear when the
|
||||
* robot is disabled.
|
||||
*/
|
||||
void Robot::DisabledInit() {}
|
||||
|
||||
void Robot::DisabledPeriodic() {}
|
||||
|
||||
/**
|
||||
* This autonomous runs the autonomous command selected by your {@link
|
||||
* RobotContainer} class.
|
||||
*/
|
||||
void Robot::AutonomousInit() {
|
||||
m_autonomousCommand = m_container.GetAutonomousCommand();
|
||||
|
||||
if (m_autonomousCommand) {
|
||||
m_autonomousCommand->Schedule();
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::AutonomousPeriodic() {}
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand) {
|
||||
m_autonomousCommand->Cancel();
|
||||
m_autonomousCommand.reset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,94 +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.
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/RamseteController.h>
|
||||
#include <frc/shuffleboard/Shuffleboard.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/RamseteCommand.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
RobotContainer::RobotContainer() {
|
||||
// Initialize all of your commands and subsystems here
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::cmd::Run(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
|
||||
-m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton{&m_driverController, 6}
|
||||
.OnTrue(&m_driveHalfSpeed)
|
||||
.OnFalse(&m_driveFullSpeed);
|
||||
}
|
||||
|
||||
frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
|
||||
// Create a voltage constraint to ensure we don't accelerate too fast
|
||||
frc::DifferentialDriveVoltageConstraint autoVoltageConstraint{
|
||||
frc::SimpleMotorFeedforward<units::meters>{
|
||||
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka},
|
||||
DriveConstants::kDriveKinematics, 10_V};
|
||||
|
||||
// Set up config for trajectory
|
||||
frc::TrajectoryConfig config{AutoConstants::kMaxSpeed,
|
||||
AutoConstants::kMaxAcceleration};
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
config.SetKinematics(DriveConstants::kDriveKinematics);
|
||||
// Apply the voltage constraint
|
||||
config.AddConstraint(autoVoltageConstraint);
|
||||
|
||||
// An example trajectory to follow. All units in meters.
|
||||
auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
// Start at the origin facing the +X direction
|
||||
frc::Pose2d{0_m, 0_m, 0_deg},
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
{frc::Translation2d{1_m, 1_m}, frc::Translation2d{2_m, -1_m}},
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
frc::Pose2d{3_m, 0_m, 0_deg},
|
||||
// Pass the config
|
||||
config);
|
||||
|
||||
frc2::CommandPtr ramseteCommand{frc2::RamseteCommand(
|
||||
exampleTrajectory, [this] { return m_drive.GetPose(); },
|
||||
frc::RamseteController{AutoConstants::kRamseteB,
|
||||
AutoConstants::kRamseteZeta},
|
||||
frc::SimpleMotorFeedforward<units::meters>{
|
||||
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka},
|
||||
DriveConstants::kDriveKinematics,
|
||||
[this] { return m_drive.GetWheelSpeeds(); },
|
||||
frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
|
||||
frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
|
||||
[this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
|
||||
{&m_drive})};
|
||||
|
||||
// Reset odometry to the initial pose of the trajectory, run path following
|
||||
// command, then stop at the end.
|
||||
return frc2::cmd::RunOnce(
|
||||
[this, initialPose = exampleTrajectory.InitialPose()] {
|
||||
m_drive.ResetOdometry(initialPose);
|
||||
},
|
||||
{})
|
||||
.AndThen(std::move(ramseteCommand))
|
||||
.AndThen(
|
||||
frc2::cmd::RunOnce([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
|
||||
}
|
||||
@@ -1,97 +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.
|
||||
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem()
|
||||
: m_left1{kLeftMotor1Port},
|
||||
m_left2{kLeftMotor2Port},
|
||||
m_right1{kRightMotor1Port},
|
||||
m_right2{kRightMotor2Port},
|
||||
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
|
||||
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
|
||||
m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
|
||||
|
||||
m_left1.AddFollower(m_left2);
|
||||
m_right1.AddFollower(m_right2);
|
||||
|
||||
// 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.
|
||||
m_right1.SetInverted(true);
|
||||
|
||||
// Set the distance per pulse for the encoders
|
||||
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
|
||||
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
|
||||
|
||||
ResetEncoders();
|
||||
}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
m_odometry.Update(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
units::meter_t{m_rightEncoder.GetDistance()});
|
||||
}
|
||||
|
||||
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
|
||||
m_drive.ArcadeDrive(fwd, rot);
|
||||
}
|
||||
|
||||
void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
|
||||
m_left1.SetVoltage(left);
|
||||
m_right1.SetVoltage(right);
|
||||
m_drive.Feed();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetEncoders() {
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetAverageEncoderDistance() {
|
||||
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetLeftEncoder() {
|
||||
return m_leftEncoder;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetRightEncoder() {
|
||||
return m_rightEncoder;
|
||||
}
|
||||
|
||||
void DriveSubsystem::SetMaxOutput(double maxOutput) {
|
||||
m_drive.SetMaxOutput(maxOutput);
|
||||
}
|
||||
|
||||
units::degree_t DriveSubsystem::GetHeading() const {
|
||||
return m_gyro.GetRotation2d().Degrees();
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetTurnRate() {
|
||||
return -m_gyro.GetRate();
|
||||
}
|
||||
|
||||
frc::Pose2d DriveSubsystem::GetPose() {
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
|
||||
frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
|
||||
return {units::meters_per_second_t{m_leftEncoder.GetRate()},
|
||||
units::meters_per_second_t{m_rightEncoder.GetRate()}};
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
|
||||
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
units::meter_t{m_rightEncoder.GetDistance()}, pose);
|
||||
}
|
||||
@@ -1,72 +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.
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/kinematics/DifferentialDriveKinematics.h>
|
||||
#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/length.h>
|
||||
#include <units/time.h>
|
||||
#include <units/velocity.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* The Constants header provides a convenient place for teams to hold robot-wide
|
||||
* numerical or bool constants. This should not be used for any other purpose.
|
||||
*
|
||||
* It is generally a good idea to place constants into subsystem- or
|
||||
* command-specific namespaces within this header, which can then be used where
|
||||
* they are needed.
|
||||
*/
|
||||
|
||||
namespace DriveConstants {
|
||||
inline constexpr int kLeftMotor1Port = 0;
|
||||
inline constexpr int kLeftMotor2Port = 1;
|
||||
inline constexpr int kRightMotor1Port = 2;
|
||||
inline constexpr int kRightMotor2Port = 3;
|
||||
|
||||
inline constexpr int kLeftEncoderPorts[]{0, 1};
|
||||
inline constexpr int kRightEncoderPorts[]{2, 3};
|
||||
inline constexpr bool kLeftEncoderReversed = false;
|
||||
inline constexpr bool kRightEncoderReversed = true;
|
||||
|
||||
inline constexpr auto kTrackwidth = 0.69_m;
|
||||
extern const frc::DifferentialDriveKinematics kDriveKinematics;
|
||||
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr units::meter_t kWheelDiameter = 6_in;
|
||||
inline constexpr auto kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter * std::numbers::pi) / static_cast<double>(kEncoderCPR);
|
||||
|
||||
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
// These characterization values MUST be determined either experimentally or
|
||||
// theoretically for *your* robot's drive. The Robot Characterization
|
||||
// Toolsuite provides a convenient tool for obtaining these values for your
|
||||
// robot.
|
||||
inline constexpr auto ks = 0.22_V;
|
||||
inline constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
|
||||
inline constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
|
||||
|
||||
// Example value only - as above, this must be tuned for your drive!
|
||||
inline constexpr double kPDriveVel = 8.5;
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr auto kMaxSpeed = 3_mps;
|
||||
inline constexpr auto kMaxAcceleration = 1_mps_sq;
|
||||
|
||||
// Reasonable baseline values for a RAMSETE follower in units of meters and
|
||||
// seconds
|
||||
inline constexpr auto kRamseteB = 2.0 * 1_rad * 1_rad / (1_m * 1_m);
|
||||
inline constexpr auto kRamseteZeta = 0.7 / 1_rad;
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
inline constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,31 +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 <optional>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
void RobotInit() override;
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it empty by default
|
||||
std::optional<frc2::CommandPtr> m_autonomousCommand;
|
||||
|
||||
RobotContainer m_container;
|
||||
};
|
||||
@@ -1,45 +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 <frc/XboxController.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
*/
|
||||
class RobotContainer {
|
||||
public:
|
||||
RobotContainer();
|
||||
|
||||
frc2::CommandPtr GetAutonomousCommand();
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
|
||||
// The robot's subsystems
|
||||
DriveSubsystem m_drive;
|
||||
|
||||
// RobotContainer-owned commands
|
||||
frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
|
||||
{}};
|
||||
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
|
||||
{}};
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,139 +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 <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/kinematics/DifferentialDriveOdometry.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class DriveSubsystem : public frc2::SubsystemBase {
|
||||
public:
|
||||
DriveSubsystem();
|
||||
|
||||
/**
|
||||
* Will be called periodically whenever the CommandScheduler runs.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
// Subsystem methods go here.
|
||||
|
||||
/**
|
||||
* Drives the robot using arcade controls.
|
||||
*
|
||||
* @param fwd the commanded forward movement
|
||||
* @param rot the commanded rotation
|
||||
*/
|
||||
void ArcadeDrive(double fwd, double rot);
|
||||
|
||||
/**
|
||||
* Controls each side of the drive directly with a voltage.
|
||||
*
|
||||
* @param left the commanded left output
|
||||
* @param right the commanded right output
|
||||
*/
|
||||
void TankDriveVolts(units::volt_t left, units::volt_t right);
|
||||
|
||||
/**
|
||||
* Resets the drive encoders to currently read a position of 0.
|
||||
*/
|
||||
void ResetEncoders();
|
||||
|
||||
/**
|
||||
* Gets the average distance of the TWO encoders.
|
||||
*
|
||||
* @return the average of the TWO encoder readings
|
||||
*/
|
||||
double GetAverageEncoderDistance();
|
||||
|
||||
/**
|
||||
* Gets the left drive encoder.
|
||||
*
|
||||
* @return the left drive encoder
|
||||
*/
|
||||
frc::Encoder& GetLeftEncoder();
|
||||
|
||||
/**
|
||||
* Gets the right drive encoder.
|
||||
*
|
||||
* @return the right drive encoder
|
||||
*/
|
||||
frc::Encoder& GetRightEncoder();
|
||||
|
||||
/**
|
||||
* Sets the max output of the drive. Useful for scaling the drive to drive
|
||||
* more slowly.
|
||||
*
|
||||
* @param maxOutput the maximum output to which the drive will be constrained
|
||||
*/
|
||||
void SetMaxOutput(double maxOutput);
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
units::degree_t GetHeading() const;
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
double GetTurnRate();
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
*
|
||||
* @return The pose.
|
||||
*/
|
||||
frc::Pose2d GetPose();
|
||||
|
||||
/**
|
||||
* Returns the current wheel speeds of the robot.
|
||||
*
|
||||
* @return The current wheel speeds.
|
||||
*/
|
||||
frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
void ResetOdometry(frc::Pose2d pose);
|
||||
|
||||
private:
|
||||
// Components (e.g. motor controllers and sensors) should generally be
|
||||
// declared private and exposed only through public methods.
|
||||
|
||||
// The motor controllers
|
||||
frc::PWMSparkMax m_left1;
|
||||
frc::PWMSparkMax m_left2;
|
||||
frc::PWMSparkMax m_right1;
|
||||
frc::PWMSparkMax m_right2;
|
||||
|
||||
// The robot's drive
|
||||
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
|
||||
[&](double output) { m_right1.Set(output); }};
|
||||
|
||||
// The left-side drive encoder
|
||||
frc::Encoder m_leftEncoder;
|
||||
|
||||
// The right-side drive encoder
|
||||
frc::Encoder m_rightEncoder;
|
||||
|
||||
// The gyro sensor
|
||||
frc::ADXRS450_Gyro m_gyro;
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
frc::DifferentialDriveOdometry m_odometry;
|
||||
};
|
||||
@@ -1,38 +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.
|
||||
|
||||
#include "Drivetrain.h"
|
||||
|
||||
void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
|
||||
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
|
||||
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
|
||||
const double leftOutput = m_leftPIDController.Calculate(
|
||||
m_leftEncoder.GetRate(), speeds.left.value());
|
||||
const double rightOutput = m_rightPIDController.Calculate(
|
||||
m_rightEncoder.GetRate(), speeds.right.value());
|
||||
|
||||
m_leftLeader.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
|
||||
m_rightLeader.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
|
||||
}
|
||||
|
||||
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
|
||||
units::radians_per_second_t rot) {
|
||||
SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
|
||||
}
|
||||
|
||||
void Drivetrain::UpdateOdometry() {
|
||||
m_odometry.Update(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
units::meter_t{m_rightEncoder.GetDistance()});
|
||||
}
|
||||
|
||||
void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
|
||||
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
units::meter_t{m_rightEncoder.GetDistance()}, pose);
|
||||
}
|
||||
|
||||
frc::Pose2d Drivetrain::GetPose() const {
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
@@ -1,100 +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.
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/controller/RamseteController.h>
|
||||
#include <frc/filter/SlewRateLimiter.h>
|
||||
#include <frc/smartdashboard/Field2d.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
|
||||
#include "Drivetrain.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
void AutonomousInit() override {
|
||||
// Start the timer.
|
||||
m_timer.Start();
|
||||
|
||||
// Send Field2d to SmartDashboard.
|
||||
frc::SmartDashboard::PutData(&m_field);
|
||||
|
||||
// Reset the drivetrain's odometry to the starting pose of the trajectory.
|
||||
m_drive.ResetOdometry(m_trajectory.InitialPose());
|
||||
|
||||
// Send our generated trajectory to Field2d.
|
||||
m_field.GetObject("traj")->SetTrajectory(m_trajectory);
|
||||
}
|
||||
|
||||
void AutonomousPeriodic() override {
|
||||
// Update odometry.
|
||||
m_drive.UpdateOdometry();
|
||||
|
||||
// Update robot position on Field2d.
|
||||
m_field.SetRobotPose(m_drive.GetPose());
|
||||
|
||||
if (m_timer.Get() < m_trajectory.TotalTime()) {
|
||||
// Get the desired pose from the trajectory.
|
||||
auto desiredPose = m_trajectory.Sample(m_timer.Get());
|
||||
|
||||
// Get the reference chassis speeds from the Ramsete Controller.
|
||||
auto refChassisSpeeds =
|
||||
m_ramseteController.Calculate(m_drive.GetPose(), desiredPose);
|
||||
|
||||
// Set the linear and angular speeds.
|
||||
m_drive.Drive(refChassisSpeeds.vx, refChassisSpeeds.omega);
|
||||
} else {
|
||||
m_drive.Drive(0_mps, 0_rad_per_s);
|
||||
}
|
||||
}
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
|
||||
Drivetrain::kMaxAngularSpeed;
|
||||
|
||||
m_drive.Drive(xSpeed, rot);
|
||||
}
|
||||
|
||||
private:
|
||||
frc::XboxController m_controller{0};
|
||||
|
||||
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
|
||||
// to 1.
|
||||
frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
|
||||
frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
|
||||
|
||||
Drivetrain m_drive;
|
||||
|
||||
// An example trajectory to follow.
|
||||
frc::Trajectory m_trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
frc::Pose2d{0_m, 0_m, 0_rad},
|
||||
{frc::Translation2d{1_m, 1_m}, frc::Translation2d{2_m, -1_m}},
|
||||
frc::Pose2d{3_m, 0_m, 0_rad}, frc::TrajectoryConfig(3_fps, 3_fps_sq));
|
||||
|
||||
// The Ramsete Controller to follow the trajectory.
|
||||
frc::RamseteController m_ramseteController;
|
||||
|
||||
// The timer to use during the autonomous period.
|
||||
frc::Timer m_timer;
|
||||
|
||||
// Create Field2d for robot and trajectory visualizations.
|
||||
frc::Field2d m_field;
|
||||
};
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,85 +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 <numbers>
|
||||
|
||||
#include <frc/AnalogGyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/kinematics/DifferentialDriveKinematics.h>
|
||||
#include <frc/kinematics/DifferentialDriveOdometry.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/length.h>
|
||||
|
||||
/**
|
||||
* Represents a differential drive style drivetrain.
|
||||
*/
|
||||
class Drivetrain {
|
||||
public:
|
||||
Drivetrain() {
|
||||
m_gyro.Reset();
|
||||
|
||||
m_leftLeader.AddFollower(m_leftFollower);
|
||||
m_rightLeader.AddFollower(m_rightFollower);
|
||||
|
||||
// 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.
|
||||
m_rightLeader.SetInverted(true);
|
||||
|
||||
// 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 * std::numbers::pi * kWheelRadius /
|
||||
kEncoderResolution);
|
||||
m_rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
|
||||
kEncoderResolution);
|
||||
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
}
|
||||
|
||||
static constexpr units::meters_per_second_t kMaxSpeed =
|
||||
3.0_mps; // 3 meters per second
|
||||
static constexpr units::radians_per_second_t kMaxAngularSpeed{
|
||||
std::numbers::pi}; // 1/2 rotation per second
|
||||
|
||||
void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
|
||||
void Drive(units::meters_per_second_t xSpeed,
|
||||
units::radians_per_second_t rot);
|
||||
void UpdateOdometry();
|
||||
void ResetOdometry(const frc::Pose2d& pose);
|
||||
frc::Pose2d GetPose() const;
|
||||
|
||||
private:
|
||||
static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
|
||||
static constexpr double kWheelRadius = 0.0508; // meters
|
||||
static constexpr int kEncoderResolution = 4096;
|
||||
|
||||
frc::PWMSparkMax m_leftLeader{1};
|
||||
frc::PWMSparkMax m_leftFollower{2};
|
||||
frc::PWMSparkMax m_rightLeader{3};
|
||||
frc::PWMSparkMax m_rightFollower{4};
|
||||
|
||||
frc::Encoder m_leftEncoder{0, 1};
|
||||
frc::Encoder m_rightEncoder{2, 3};
|
||||
|
||||
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::DifferentialDriveKinematics m_kinematics{kTrackWidth};
|
||||
frc::DifferentialDriveOdometry m_odometry{
|
||||
m_gyro.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
|
||||
// robot!
|
||||
frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
|
||||
};
|
||||
@@ -5,7 +5,7 @@
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/controller/RamseteController.h>
|
||||
#include <frc/controller/LTVUnicycleController.h>
|
||||
#include <frc/filter/SlewRateLimiter.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
|
||||
@@ -29,7 +29,7 @@ class Robot : public frc::TimedRobot {
|
||||
void AutonomousPeriodic() override {
|
||||
auto elapsed = m_timer.Get();
|
||||
auto reference = m_trajectory.Sample(elapsed);
|
||||
auto speeds = m_ramsete.Calculate(m_drive.GetPose(), reference);
|
||||
auto speeds = m_feedback.Calculate(m_drive.GetPose(), reference);
|
||||
m_drive.Drive(speeds.vx, speeds.omega);
|
||||
}
|
||||
|
||||
@@ -61,7 +61,7 @@ class Robot : public frc::TimedRobot {
|
||||
|
||||
Drivetrain m_drive;
|
||||
frc::Trajectory m_trajectory;
|
||||
frc::RamseteController m_ramsete;
|
||||
frc::LTVUnicycleController m_feedback{20_ms};
|
||||
frc::Timer m_timer;
|
||||
};
|
||||
|
||||
|
||||
@@ -1,13 +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.
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
const frc::DifferentialDriveKinematics DriveConstants::kDriveKinematics(
|
||||
kTrackwidth);
|
||||
|
||||
const frc::LinearSystem<2, 2, 2> DriveConstants::kDrivetrainPlant =
|
||||
frc::LinearSystemId::IdentifyDrivetrainSystem(kv, ka, kvAngular, kaAngular);
|
||||
@@ -1,87 +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.
|
||||
|
||||
#include "Robot.h"
|
||||
|
||||
#include <frc/simulation/BatterySim.h>
|
||||
#include <frc/simulation/RoboRioSim.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <frc2/command/CommandScheduler.h>
|
||||
|
||||
void Robot::RobotInit() {}
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
*/
|
||||
void Robot::RobotPeriodic() {
|
||||
frc2::CommandScheduler::GetInstance().Run();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode. You
|
||||
* can use it to reset any subsystem information you want to clear when the
|
||||
* robot is disabled.
|
||||
*/
|
||||
void Robot::DisabledInit() {
|
||||
frc2::CommandScheduler::GetInstance().CancelAll();
|
||||
m_container.ZeroAllOutputs();
|
||||
}
|
||||
|
||||
void Robot::DisabledPeriodic() {}
|
||||
|
||||
/**
|
||||
* This autonomous runs the autonomous command selected by your {@link
|
||||
* RobotContainer} class.
|
||||
*/
|
||||
void Robot::AutonomousInit() {
|
||||
m_autonomousCommand = m_container.GetAutonomousCommand();
|
||||
|
||||
if (m_autonomousCommand != nullptr) {
|
||||
m_autonomousCommand->Schedule();
|
||||
}
|
||||
}
|
||||
|
||||
void Robot::AutonomousPeriodic() {}
|
||||
|
||||
void Robot::TeleopInit() {
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand != nullptr) {
|
||||
m_autonomousCommand->Cancel();
|
||||
m_autonomousCommand = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
|
||||
void Robot::SimulationPeriodic() {
|
||||
// Here we calculate the battery voltage based on drawn current.
|
||||
// As our robot draws more power from the battery its voltage drops.
|
||||
// The estimated voltage is highly dependent on the battery's internal
|
||||
// resistance.
|
||||
auto current = m_container.GetRobotDrive().GetCurrentDraw();
|
||||
auto loadedVoltage = frc::sim::BatterySim::Calculate({current});
|
||||
frc::sim::RoboRioSim::SetVInVoltage(loadedVoltage);
|
||||
}
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
int main() {
|
||||
return frc::StartRobot<Robot>();
|
||||
}
|
||||
#endif
|
||||
@@ -1,106 +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.
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/RamseteController.h>
|
||||
#include <frc/shuffleboard/Shuffleboard.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/RamseteCommand.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
RobotContainer::RobotContainer() {
|
||||
// Initialize all of your commands and subsystems here
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
|
||||
// Set up default drive command
|
||||
// A split-stick arcade command, with forward/backward controlled by the left
|
||||
// hand, and turning controlled by the right.
|
||||
// If you are using the keyboard as a joystick, it is recommended that you go
|
||||
// to the following link to read about editing the keyboard settings.
|
||||
// https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/simulation-gui.html#using-the-keyboard-as-a-joystick
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
|
||||
-m_driverController.GetLeftX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
void RobotContainer::ZeroAllOutputs() {
|
||||
m_drive.TankDriveVolts(0_V, 0_V);
|
||||
}
|
||||
|
||||
const DriveSubsystem& RobotContainer::GetRobotDrive() const {
|
||||
return m_drive;
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController,
|
||||
frc::XboxController::Button::kRightBumper)
|
||||
.OnTrue(&m_driveHalfSpeed)
|
||||
.OnFalse(&m_driveFullSpeed);
|
||||
}
|
||||
|
||||
frc2::Command* RobotContainer::GetAutonomousCommand() {
|
||||
// Create a voltage constraint to ensure we don't accelerate too fast
|
||||
frc::DifferentialDriveVoltageConstraint autoVoltageConstraint(
|
||||
frc::SimpleMotorFeedforward<units::meters>(
|
||||
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
|
||||
DriveConstants::kDriveKinematics, 10_V);
|
||||
|
||||
// Set up config for trajectory
|
||||
frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
|
||||
AutoConstants::kMaxAcceleration);
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
config.SetKinematics(DriveConstants::kDriveKinematics);
|
||||
// Apply the voltage constraint
|
||||
config.AddConstraint(autoVoltageConstraint);
|
||||
|
||||
// An example trajectory to follow. All units in meters.
|
||||
auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
// Start at (1, 2) facing the +X direction
|
||||
frc::Pose2d{1_m, 2_m, 0_deg},
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
{frc::Translation2d{2_m, 3_m}, frc::Translation2d{3_m, 1_m}},
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
frc::Pose2d{4_m, 2_m, 0_deg},
|
||||
// Pass the config
|
||||
config);
|
||||
|
||||
frc2::RamseteCommand ramseteCommand(
|
||||
exampleTrajectory, [this] { return m_drive.GetPose(); },
|
||||
frc::RamseteController{AutoConstants::kRamseteB,
|
||||
AutoConstants::kRamseteZeta},
|
||||
frc::SimpleMotorFeedforward<units::meters>(
|
||||
DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
|
||||
DriveConstants::kDriveKinematics,
|
||||
[this] { return m_drive.GetWheelSpeeds(); },
|
||||
frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
|
||||
frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
|
||||
[this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
|
||||
{&m_drive});
|
||||
|
||||
// Reset odometry to starting pose of trajectory.
|
||||
m_drive.ResetOdometry(exampleTrajectory.InitialPose());
|
||||
|
||||
// no auto
|
||||
return new frc2::SequentialCommandGroup(
|
||||
std::move(ramseteCommand),
|
||||
frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
|
||||
}
|
||||
@@ -1,119 +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.
|
||||
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/SPI.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
|
||||
#include <frc/simulation/SimDeviceSim.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem() {
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
|
||||
|
||||
m_left1.AddFollower(m_left2);
|
||||
m_right1.AddFollower(m_right2);
|
||||
|
||||
// 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.
|
||||
m_right1.SetInverted(true);
|
||||
|
||||
// Set the distance per pulse for the encoders
|
||||
m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
|
||||
ResetEncoders();
|
||||
frc::SmartDashboard::PutData("Field", &m_fieldSim);
|
||||
}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
m_odometry.Update(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
units::meter_t{m_rightEncoder.GetDistance()});
|
||||
m_fieldSim.SetRobotPose(m_odometry.GetPose());
|
||||
}
|
||||
|
||||
void DriveSubsystem::SimulationPeriodic() {
|
||||
// To update our simulation, we set motor voltage inputs, update the
|
||||
// simulation, and write the simulated positions and velocities to our
|
||||
// simulated encoder and gyro. We negate the right side so that positive
|
||||
// voltages make the right side move forward.
|
||||
m_drivetrainSimulator.SetInputs(
|
||||
units::volt_t{m_left1.Get()} * frc::RobotController::GetInputVoltage(),
|
||||
units::volt_t{m_right1.Get()} * frc::RobotController::GetInputVoltage());
|
||||
m_drivetrainSimulator.Update(20_ms);
|
||||
|
||||
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
|
||||
m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
|
||||
m_rightEncoderSim.SetDistance(
|
||||
m_drivetrainSimulator.GetRightPosition().value());
|
||||
m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
|
||||
m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees());
|
||||
}
|
||||
|
||||
units::ampere_t DriveSubsystem::GetCurrentDraw() const {
|
||||
return m_drivetrainSimulator.GetCurrentDraw();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
|
||||
m_drive.ArcadeDrive(fwd, rot);
|
||||
}
|
||||
|
||||
void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
|
||||
m_left1.SetVoltage(left);
|
||||
m_right1.SetVoltage(right);
|
||||
m_drive.Feed();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetEncoders() {
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetAverageEncoderDistance() {
|
||||
return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetLeftEncoder() {
|
||||
return m_leftEncoder;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetRightEncoder() {
|
||||
return m_rightEncoder;
|
||||
}
|
||||
|
||||
void DriveSubsystem::SetMaxOutput(double maxOutput) {
|
||||
m_drive.SetMaxOutput(maxOutput);
|
||||
}
|
||||
|
||||
units::degree_t DriveSubsystem::GetHeading() const {
|
||||
return m_gyro.GetRotation2d().Degrees();
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetTurnRate() {
|
||||
return -m_gyro.GetRate();
|
||||
}
|
||||
|
||||
frc::Pose2d DriveSubsystem::GetPose() {
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
|
||||
frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
|
||||
return {units::meters_per_second_t{m_leftEncoder.GetRate()},
|
||||
units::meters_per_second_t{m_rightEncoder.GetRate()}};
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
|
||||
m_drivetrainSimulator.SetPose(pose);
|
||||
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
units::meter_t{m_rightEncoder.GetDistance()}, pose);
|
||||
}
|
||||
@@ -1,84 +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.
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/kinematics/DifferentialDriveKinematics.h>
|
||||
#include <frc/system/plant/DCMotor.h>
|
||||
#include <frc/system/plant/LinearSystemId.h>
|
||||
#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/length.h>
|
||||
#include <units/time.h>
|
||||
#include <units/velocity.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* The Constants header provides a convenient place for teams to hold robot-wide
|
||||
* numerical or bool constants. This should not be used for any other purpose.
|
||||
*
|
||||
* It is generally a good idea to place constants into subsystem- or
|
||||
* command-specific namespaces within this header, which can then be used where
|
||||
* they are needed.
|
||||
*/
|
||||
|
||||
namespace DriveConstants {
|
||||
inline constexpr int kLeftMotor1Port = 0;
|
||||
inline constexpr int kLeftMotor2Port = 1;
|
||||
inline constexpr int kRightMotor1Port = 2;
|
||||
inline constexpr int kRightMotor2Port = 3;
|
||||
|
||||
inline constexpr int kLeftEncoderPorts[]{0, 1};
|
||||
inline constexpr int kRightEncoderPorts[]{2, 3};
|
||||
inline constexpr bool kLeftEncoderReversed = false;
|
||||
inline constexpr bool kRightEncoderReversed = true;
|
||||
|
||||
inline constexpr auto kTrackwidth = 0.69_m;
|
||||
extern const frc::DifferentialDriveKinematics kDriveKinematics;
|
||||
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr auto kWheelDiameter = 6_in;
|
||||
inline constexpr double kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter.value() * std::numbers::pi) /
|
||||
static_cast<double>(kEncoderCPR);
|
||||
|
||||
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
// These characterization values MUST be determined either experimentally or
|
||||
// theoretically for *your* robot's drive. The Robot Characterization
|
||||
// Toolsuite provides a convenient tool for obtaining these values for your
|
||||
// robot.
|
||||
inline constexpr auto ks = 0.22_V;
|
||||
inline constexpr auto kv = 1.98 * 1_V / 1_mps;
|
||||
inline constexpr auto ka = 0.2 * 1_V / 1_mps_sq;
|
||||
|
||||
inline constexpr auto kvAngular = 1.5 * 1_V / 1_mps;
|
||||
inline constexpr auto kaAngular = 0.3 * 1_V / 1_mps_sq;
|
||||
|
||||
extern const frc::LinearSystem<2, 2, 2> kDrivetrainPlant;
|
||||
|
||||
// Example values only -- use what's on your physical robot!
|
||||
inline constexpr auto kDrivetrainGearbox = frc::DCMotor::CIM(2);
|
||||
inline constexpr auto kDrivetrainGearing = 8.0;
|
||||
|
||||
// Example value only - as above, this must be tuned for your drive!
|
||||
inline constexpr double kPDriveVel = 8.5;
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr auto kMaxSpeed = 3_mps;
|
||||
inline constexpr auto kMaxAcceleration = 3_mps_sq;
|
||||
|
||||
// Reasonable baseline values for a RAMSETE follower in units of meters and
|
||||
// seconds
|
||||
inline constexpr auto kRamseteB = 2 * 1_rad * 1_rad / (1_m * 1_m);
|
||||
inline constexpr auto kRamseteZeta = 0.7 / 1_rad;
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
inline constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,31 +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 <frc/TimedRobot.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
void RobotInit() override;
|
||||
void RobotPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void AutonomousInit() override;
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void SimulationPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
// doesn't have undefined behavior and potentially crash.
|
||||
frc2::Command* m_autonomousCommand = nullptr;
|
||||
|
||||
RobotContainer m_container;
|
||||
};
|
||||
@@ -1,52 +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 <frc/XboxController.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/PIDCommand.h>
|
||||
#include <frc2/command/ParallelRaceGroup.h>
|
||||
#include <frc2/command/RunCommand.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
*/
|
||||
class RobotContainer {
|
||||
public:
|
||||
RobotContainer();
|
||||
|
||||
void ZeroAllOutputs();
|
||||
frc2::Command* GetAutonomousCommand();
|
||||
const DriveSubsystem& GetRobotDrive() const;
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
|
||||
// The robot's subsystems
|
||||
DriveSubsystem m_drive;
|
||||
|
||||
frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
|
||||
{}};
|
||||
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
|
||||
{}};
|
||||
|
||||
// The chooser for the autonomous routines
|
||||
frc::SendableChooser<frc2::Command*> m_chooser;
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,172 +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 <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/kinematics/DifferentialDriveOdometry.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc/simulation/ADXRS450_GyroSim.h>
|
||||
#include <frc/simulation/DifferentialDrivetrainSim.h>
|
||||
#include <frc/simulation/EncoderSim.h>
|
||||
#include <frc/smartdashboard/Field2d.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class DriveSubsystem : public frc2::SubsystemBase {
|
||||
public:
|
||||
DriveSubsystem();
|
||||
|
||||
/**
|
||||
* Will be called periodically whenever the CommandScheduler runs.
|
||||
*/
|
||||
void Periodic() override;
|
||||
|
||||
/**
|
||||
* Will be called periodically during simulation.
|
||||
*/
|
||||
void SimulationPeriodic() override;
|
||||
|
||||
// Subsystem methods go here.
|
||||
|
||||
units::ampere_t GetCurrentDraw() const;
|
||||
|
||||
/**
|
||||
* Drives the robot using arcade controls.
|
||||
*
|
||||
* @param fwd the commanded forward movement
|
||||
* @param rot the commanded rotation
|
||||
*/
|
||||
void ArcadeDrive(double fwd, double rot);
|
||||
|
||||
/**
|
||||
* Controls each side of the drive directly with a voltage.
|
||||
*
|
||||
* @param left the commanded left output
|
||||
* @param right the commanded right output
|
||||
*/
|
||||
void TankDriveVolts(units::volt_t left, units::volt_t right);
|
||||
|
||||
/**
|
||||
* Resets the drive encoders to currently read a position of 0.
|
||||
*/
|
||||
void ResetEncoders();
|
||||
|
||||
/**
|
||||
* Gets the average distance of the TWO encoders.
|
||||
*
|
||||
* @return the average of the TWO encoder readings
|
||||
*/
|
||||
double GetAverageEncoderDistance();
|
||||
|
||||
/**
|
||||
* Gets the left drive encoder.
|
||||
*
|
||||
* @return the left drive encoder
|
||||
*/
|
||||
frc::Encoder& GetLeftEncoder();
|
||||
|
||||
/**
|
||||
* Gets the right drive encoder.
|
||||
*
|
||||
* @return the right drive encoder
|
||||
*/
|
||||
frc::Encoder& GetRightEncoder();
|
||||
|
||||
/**
|
||||
* Sets the max output of the drive. Useful for scaling the drive to drive
|
||||
* more slowly.
|
||||
*
|
||||
* @param maxOutput the maximum output to which the drive will be constrained
|
||||
*/
|
||||
void SetMaxOutput(double maxOutput);
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
units::degree_t GetHeading() const;
|
||||
|
||||
/**
|
||||
* Returns the turn rate of the robot.
|
||||
*
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
double GetTurnRate();
|
||||
|
||||
/**
|
||||
* Returns the currently-estimated pose of the robot.
|
||||
*
|
||||
* @return The pose.
|
||||
*/
|
||||
frc::Pose2d GetPose();
|
||||
|
||||
/**
|
||||
* Returns the current wheel speeds of the robot.
|
||||
*
|
||||
* @return The current wheel speeds.
|
||||
*/
|
||||
frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
void ResetOdometry(frc::Pose2d pose);
|
||||
|
||||
private:
|
||||
// Components (e.g. motor controllers and sensors) should generally be
|
||||
// declared private and exposed only through public methods.
|
||||
|
||||
// The motor controllers
|
||||
frc::PWMSparkMax m_left1{DriveConstants::kLeftMotor1Port};
|
||||
frc::PWMSparkMax m_left2{DriveConstants::kLeftMotor2Port};
|
||||
frc::PWMSparkMax m_right1{DriveConstants::kRightMotor1Port};
|
||||
frc::PWMSparkMax m_right2{DriveConstants::kRightMotor2Port};
|
||||
|
||||
// The robot's drive
|
||||
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
|
||||
[&](double output) { m_right1.Set(output); }};
|
||||
|
||||
// The left-side drive encoder
|
||||
frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
|
||||
DriveConstants::kLeftEncoderPorts[1],
|
||||
DriveConstants::kLeftEncoderReversed};
|
||||
|
||||
// The right-side drive encoder
|
||||
frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
|
||||
DriveConstants::kRightEncoderPorts[1],
|
||||
DriveConstants::kRightEncoderReversed};
|
||||
|
||||
// The gyro sensor
|
||||
frc::ADXRS450_Gyro m_gyro;
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
frc::DifferentialDriveOdometry m_odometry{
|
||||
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
|
||||
units::meter_t{m_rightEncoder.GetDistance()}};
|
||||
|
||||
// These classes help simulate our drivetrain.
|
||||
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
|
||||
DriveConstants::kDrivetrainPlant,
|
||||
DriveConstants::kTrackwidth,
|
||||
DriveConstants::kDrivetrainGearbox,
|
||||
DriveConstants::kDrivetrainGearing,
|
||||
DriveConstants::kWheelDiameter / 2,
|
||||
{0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
|
||||
|
||||
frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
|
||||
frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
|
||||
frc::sim::ADXRS450_GyroSim m_gyroSim{m_gyro};
|
||||
|
||||
// The Field2d class shows the field in the sim GUI.
|
||||
frc::Field2d m_fieldSim;
|
||||
};
|
||||
@@ -502,24 +502,6 @@
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "RamseteCommand",
|
||||
"description": "Follow a pre-generated trajectory with a differential drive using RamseteCommand.",
|
||||
"tags": [
|
||||
"Differential Drive",
|
||||
"Command-based",
|
||||
"Ramsete",
|
||||
"Trajectory",
|
||||
"Path Following",
|
||||
"Odometry",
|
||||
"Encoder",
|
||||
"Gyro",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "RamseteCommand",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "Arcade Drive Xbox Controller",
|
||||
"description": "Control a differential drive with split-stick arcade drive in teleop.",
|
||||
@@ -671,23 +653,6 @@
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "RamseteController",
|
||||
"description": "Follow a pre-generated trajectory with a differential drive using RamseteController.",
|
||||
"tags": [
|
||||
"Basic Robot",
|
||||
"Differential Drive",
|
||||
"Ramsete",
|
||||
"PID",
|
||||
"Odometry",
|
||||
"Path Following",
|
||||
"Trajectory",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "RamseteController",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "RomiReference",
|
||||
"description": "An example command-based robot program that can be used with the Romi reference robot design.",
|
||||
@@ -887,20 +852,6 @@
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "StateSpaceDriveSimulation",
|
||||
"description": "Simulate a differential drivetrain and follow trajectories with RamseteCommand (command-based).",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Differential Drive",
|
||||
"State-Space",
|
||||
"XboxController",
|
||||
"Simulation"
|
||||
],
|
||||
"foldername": "StateSpaceDifferentialDriveSimulation",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "SwerveDrivePoseEstimator",
|
||||
"description": "Combine swerve-drive odometry with vision data using SwerveDrivePoseEstimator.",
|
||||
|
||||
Reference in New Issue
Block a user