mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[commands, wpimath] Remove Mecanum/SwerveControllerCommand and HolonomicDriveController (#8119)
This commit is contained in:
Gold856
@@ -1,22 +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"
|
||||
|
||||
namespace DriveConstants {
|
||||
|
||||
const frc::MecanumDriveKinematics kDriveKinematics{
|
||||
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
|
||||
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
|
||||
const frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
|
||||
|
||||
} // namespace AutoConstants
|
||||
@@ -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>
|
||||
|
||||
Robot::Robot() {}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
frc2::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
|
||||
}
|
||||
}
|
||||
|
||||
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,121 +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/geometry/Translation2d.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/MecanumControllerCommand.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
RobotContainer::RobotContainer() {
|
||||
// Initialize all of your commands and subsystems here
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.Drive(-m_driverController.GetLeftY(),
|
||||
-m_driverController.GetRightX(),
|
||||
-m_driverController.GetLeftX(), false);
|
||||
},
|
||||
{&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::CommandPtr RobotContainer::GetAutonomousCommand() {
|
||||
// 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);
|
||||
|
||||
// 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 mecanumControllerCommand =
|
||||
frc2::MecanumControllerCommand(
|
||||
exampleTrajectory, [this]() { return m_drive.GetPose(); },
|
||||
|
||||
frc::SimpleMotorFeedforward<units::meters>(ks, kv, ka),
|
||||
DriveConstants::kDriveKinematics,
|
||||
|
||||
frc::PIDController{AutoConstants::kPXController, 0, 0},
|
||||
frc::PIDController{AutoConstants::kPYController, 0, 0},
|
||||
frc::ProfiledPIDController<units::radians>(
|
||||
AutoConstants::kPThetaController, 0, 0,
|
||||
AutoConstants::kThetaControllerConstraints),
|
||||
|
||||
AutoConstants::kMaxSpeed,
|
||||
|
||||
[this]() {
|
||||
return frc::MecanumDriveWheelSpeeds{
|
||||
units::meters_per_second_t{
|
||||
m_drive.GetFrontLeftEncoder().GetRate()},
|
||||
units::meters_per_second_t{
|
||||
m_drive.GetFrontRightEncoder().GetRate()},
|
||||
units::meters_per_second_t{
|
||||
m_drive.GetRearLeftEncoder().GetRate()},
|
||||
units::meters_per_second_t{
|
||||
m_drive.GetRearRightEncoder().GetRate()}};
|
||||
},
|
||||
|
||||
frc::PIDController{DriveConstants::kPFrontLeftVel, 0, 0},
|
||||
frc::PIDController{DriveConstants::kPRearLeftVel, 0, 0},
|
||||
frc::PIDController{DriveConstants::kPFrontRightVel, 0, 0},
|
||||
frc::PIDController{DriveConstants::kPRearRightVel, 0, 0},
|
||||
|
||||
[this](units::volt_t frontLeft, units::volt_t rearLeft,
|
||||
units::volt_t frontRight, units::volt_t rearRight) {
|
||||
m_drive.SetMotorControllersVolts(frontLeft, rearLeft, frontRight,
|
||||
rearRight);
|
||||
},
|
||||
|
||||
{&m_drive})
|
||||
.ToPtr();
|
||||
|
||||
// Reset odometry to the initial pose of the trajectory, run path following
|
||||
// command, then stop at the end.
|
||||
return frc2::cmd::Sequence(
|
||||
frc2::InstantCommand(
|
||||
[this, initialPose = exampleTrajectory.InitialPose()]() {
|
||||
m_drive.ResetOdometry(initialPose);
|
||||
},
|
||||
{})
|
||||
.ToPtr(),
|
||||
std::move(mecanumControllerCommand),
|
||||
frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {})
|
||||
.ToPtr());
|
||||
}
|
||||
@@ -1,136 +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 <units/angle.h>
|
||||
#include <units/velocity.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem()
|
||||
: m_frontLeft{kFrontLeftMotorPort},
|
||||
m_rearLeft{kRearLeftMotorPort},
|
||||
m_frontRight{kFrontRightMotorPort},
|
||||
m_rearRight{kRearRightMotorPort},
|
||||
|
||||
m_frontLeftEncoder{kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1],
|
||||
kFrontLeftEncoderReversed},
|
||||
m_rearLeftEncoder{kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1],
|
||||
kRearLeftEncoderReversed},
|
||||
m_frontRightEncoder{kFrontRightEncoderPorts[0],
|
||||
kFrontRightEncoderPorts[1],
|
||||
kFrontRightEncoderReversed},
|
||||
m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
|
||||
kRearRightEncoderReversed},
|
||||
|
||||
m_odometry{kDriveKinematics, m_gyro.GetRotation2d(),
|
||||
getCurrentWheelDistances(), frc::Pose2d{}} {
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_frontLeft);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_rearLeft);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_frontRight);
|
||||
wpi::SendableRegistry::AddChild(&m_drive, &m_rearRight);
|
||||
|
||||
// Set the distance per pulse for the encoders
|
||||
m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_frontRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
m_rearRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
// 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_frontRight.SetInverted(true);
|
||||
m_rearRight.SetInverted(true);
|
||||
}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
m_odometry.Update(m_gyro.GetRotation2d(), getCurrentWheelDistances());
|
||||
}
|
||||
|
||||
void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot,
|
||||
bool fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
m_drive.DriveCartesian(xSpeed, ySpeed, rot, m_gyro.GetRotation2d());
|
||||
} else {
|
||||
m_drive.DriveCartesian(xSpeed, ySpeed, rot);
|
||||
}
|
||||
}
|
||||
|
||||
void DriveSubsystem::SetMotorControllersVolts(units::volt_t frontLeftPower,
|
||||
units::volt_t rearLeftPower,
|
||||
units::volt_t frontRightPower,
|
||||
units::volt_t rearRightPower) {
|
||||
m_frontLeft.SetVoltage(frontLeftPower);
|
||||
m_rearLeft.SetVoltage(rearLeftPower);
|
||||
m_frontRight.SetVoltage(frontRightPower);
|
||||
m_rearRight.SetVoltage(rearRightPower);
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetEncoders() {
|
||||
m_frontLeftEncoder.Reset();
|
||||
m_rearLeftEncoder.Reset();
|
||||
m_frontRightEncoder.Reset();
|
||||
m_rearRightEncoder.Reset();
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetFrontLeftEncoder() {
|
||||
return m_frontLeftEncoder;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetRearLeftEncoder() {
|
||||
return m_rearLeftEncoder;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetFrontRightEncoder() {
|
||||
return m_frontRightEncoder;
|
||||
}
|
||||
|
||||
frc::Encoder& DriveSubsystem::GetRearRightEncoder() {
|
||||
return m_rearRightEncoder;
|
||||
}
|
||||
|
||||
frc::MecanumDriveWheelSpeeds DriveSubsystem::getCurrentWheelSpeeds() {
|
||||
return (frc::MecanumDriveWheelSpeeds{
|
||||
units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
|
||||
units::meters_per_second_t{m_rearLeftEncoder.GetRate()},
|
||||
units::meters_per_second_t{m_frontRightEncoder.GetRate()},
|
||||
units::meters_per_second_t{m_rearRightEncoder.GetRate()}});
|
||||
}
|
||||
|
||||
frc::MecanumDriveWheelPositions DriveSubsystem::getCurrentWheelDistances() {
|
||||
return (frc::MecanumDriveWheelPositions{
|
||||
units::meter_t{m_frontLeftEncoder.GetDistance()},
|
||||
units::meter_t{m_rearLeftEncoder.GetDistance()},
|
||||
units::meter_t{m_frontRightEncoder.GetDistance()},
|
||||
units::meter_t{m_rearRightEncoder.GetDistance()}});
|
||||
}
|
||||
|
||||
void DriveSubsystem::SetMaxOutput(double maxOutput) {
|
||||
m_drive.SetMaxOutput(maxOutput);
|
||||
}
|
||||
|
||||
units::degree_t DriveSubsystem::GetHeading() const {
|
||||
return m_gyro.GetRotation2d().Degrees();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ZeroHeading() {
|
||||
m_gyro.Reset();
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetTurnRate() {
|
||||
return -m_gyro.GetRate();
|
||||
}
|
||||
|
||||
frc::Pose2d DriveSubsystem::GetPose() {
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
|
||||
m_odometry.ResetPosition(m_gyro.GetRotation2d(), getCurrentWheelDistances(),
|
||||
pose);
|
||||
}
|
||||
@@ -1,91 +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/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/MecanumDriveKinematics.h>
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_acceleration.h>
|
||||
#include <units/angular_velocity.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 kFrontLeftMotorPort = 0;
|
||||
inline constexpr int kRearLeftMotorPort = 1;
|
||||
inline constexpr int kFrontRightMotorPort = 2;
|
||||
inline constexpr int kRearRightMotorPort = 3;
|
||||
|
||||
inline constexpr int kFrontLeftEncoderPorts[]{0, 1};
|
||||
inline constexpr int kRearLeftEncoderPorts[]{2, 3};
|
||||
inline constexpr int kFrontRightEncoderPorts[]{4, 5};
|
||||
inline constexpr int kRearRightEncoderPorts[]{6, 7};
|
||||
|
||||
inline constexpr bool kFrontLeftEncoderReversed = false;
|
||||
inline constexpr bool kRearLeftEncoderReversed = true;
|
||||
inline constexpr bool kFrontRightEncoderReversed = false;
|
||||
inline constexpr bool kRearRightEncoderReversed = true;
|
||||
|
||||
inline constexpr auto kTrackwidth =
|
||||
0.5_m; // Distance between centers of right and left wheels on robot
|
||||
inline constexpr auto kWheelBase =
|
||||
0.7_m; // Distance between centers of front and back wheels on robot
|
||||
extern const frc::MecanumDriveKinematics kDriveKinematics;
|
||||
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr auto kWheelDiameter = 0.15_m;
|
||||
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 SysId tool provides a convenient
|
||||
// method for obtaining these values for your robot.
|
||||
inline constexpr auto ks = 1_V;
|
||||
inline constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
|
||||
inline constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
|
||||
|
||||
// Example value only - as above, this must be tuned for your drive!
|
||||
inline constexpr double kPFrontLeftVel = 0.5;
|
||||
inline constexpr double kPRearLeftVel = 0.5;
|
||||
inline constexpr double kPFrontRightVel = 0.5;
|
||||
inline constexpr double kPRearRightVel = 0.5;
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr auto kMaxSpeed = 3_mps;
|
||||
inline constexpr auto kMaxAcceleration = 3_mps_sq;
|
||||
inline constexpr auto kMaxAngularSpeed = 3_rad_per_s;
|
||||
inline constexpr auto kMaxAngularAcceleration = 3_rad_per_s_sq;
|
||||
|
||||
inline constexpr double kPXController = 0.5;
|
||||
inline constexpr double kPYController = 0.5;
|
||||
inline constexpr double kPThetaController = 0.5;
|
||||
|
||||
extern const frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints;
|
||||
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
inline constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,32 +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/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
Robot();
|
||||
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 null by default so that if testing teleop it
|
||||
// doesn't have undefined behavior and potentially crash.
|
||||
std::optional<frc2::CommandPtr> m_autonomousCommand;
|
||||
|
||||
RobotContainer m_container;
|
||||
};
|
||||
@@ -1,48 +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/controller/ProfiledPIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
#include <frc2/command/InstantCommand.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();
|
||||
|
||||
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;
|
||||
|
||||
frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
|
||||
{}};
|
||||
frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
|
||||
{}};
|
||||
|
||||
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/AnalogGyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/MecanumDrive.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/kinematics/MecanumDriveOdometry.h>
|
||||
#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.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 at given x, y and theta speeds. Speeds range from [-1, 1]
|
||||
* and the linear speeds have no effect on the angular speed.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction
|
||||
* (forward/backwards).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to
|
||||
* the field.
|
||||
*/
|
||||
void Drive(double xSpeed, double ySpeed, double rot, bool fieldRelative);
|
||||
|
||||
/**
|
||||
* Resets the drive encoders to currently read a position of 0.
|
||||
*/
|
||||
void ResetEncoders();
|
||||
|
||||
/**
|
||||
* Gets the front left drive encoder.
|
||||
*
|
||||
* @return the front left drive encoder
|
||||
*/
|
||||
frc::Encoder& GetFrontLeftEncoder();
|
||||
|
||||
/**
|
||||
* Gets the rear left drive encoder.
|
||||
*
|
||||
* @return the rear left drive encoder
|
||||
*/
|
||||
frc::Encoder& GetRearLeftEncoder();
|
||||
|
||||
/**
|
||||
* Gets the front right drive encoder.
|
||||
*
|
||||
* @return the front right drive encoder
|
||||
*/
|
||||
frc::Encoder& GetFrontRightEncoder();
|
||||
|
||||
/**
|
||||
* Gets the rear right drive encoder.
|
||||
*
|
||||
* @return the rear right drive encoder
|
||||
*/
|
||||
frc::Encoder& GetRearRightEncoder();
|
||||
|
||||
/**
|
||||
* Gets the wheel speeds.
|
||||
*
|
||||
* @return the current wheel speeds.
|
||||
*/
|
||||
frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds();
|
||||
|
||||
/**
|
||||
* Gets the distances travelled by each wheel.
|
||||
*
|
||||
* @return the distances travelled by each wheel.
|
||||
*/
|
||||
frc::MecanumDriveWheelPositions getCurrentWheelDistances();
|
||||
|
||||
/**
|
||||
* Sets the drive MotorControllers to a desired voltage.
|
||||
*/
|
||||
void SetMotorControllersVolts(units::volt_t frontLeftPower,
|
||||
units::volt_t rearLeftPower,
|
||||
units::volt_t frontRightPower,
|
||||
units::volt_t rearRightPower);
|
||||
|
||||
/**
|
||||
* 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;
|
||||
|
||||
/**
|
||||
* Zeroes the heading of the robot.
|
||||
*/
|
||||
void ZeroHeading();
|
||||
|
||||
/**
|
||||
* 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();
|
||||
|
||||
/**
|
||||
* 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_frontLeft;
|
||||
frc::PWMSparkMax m_rearLeft;
|
||||
frc::PWMSparkMax m_frontRight;
|
||||
frc::PWMSparkMax m_rearRight;
|
||||
|
||||
// The robot's drive
|
||||
frc::MecanumDrive m_drive{[&](double output) { m_frontLeft.Set(output); },
|
||||
[&](double output) { m_rearLeft.Set(output); },
|
||||
[&](double output) { m_frontRight.Set(output); },
|
||||
[&](double output) { m_rearRight.Set(output); }};
|
||||
|
||||
// The front-left-side drive encoder
|
||||
frc::Encoder m_frontLeftEncoder;
|
||||
|
||||
// The rear-left-side drive encoder
|
||||
frc::Encoder m_rearLeftEncoder;
|
||||
|
||||
// The front-right--side drive encoder
|
||||
frc::Encoder m_frontRightEncoder;
|
||||
|
||||
// The rear-right-side drive encoder
|
||||
frc::Encoder m_rearRightEncoder;
|
||||
|
||||
// The gyro sensor
|
||||
frc::AnalogGyro m_gyro{0};
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
frc::MecanumDriveOdometry m_odometry;
|
||||
};
|
||||
@@ -1,12 +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"
|
||||
|
||||
namespace AutoConstants {
|
||||
|
||||
const frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
|
||||
|
||||
} // namespace AutoConstants
|
||||
@@ -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>
|
||||
|
||||
Robot::Robot() {}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
frc2::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
|
||||
}
|
||||
}
|
||||
|
||||
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,104 +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/geometry/Translation2d.h>
|
||||
#include <frc/trajectory/Trajectory.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <frc2/command/Commands.h>
|
||||
#include <frc2/command/InstantCommand.h>
|
||||
#include <frc2/command/SequentialCommandGroup.h>
|
||||
#include <frc2/command/SwerveControllerCommand.h>
|
||||
#include <frc2/command/button/JoystickButton.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/velocity.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "subsystems/DriveSubsystem.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
RobotContainer::RobotContainer() {
|
||||
// Initialize all of your commands and subsystems here
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
|
||||
// Set up default drive command
|
||||
// The left stick controls translation of the robot.
|
||||
// Turning is controlled by the X axis of the right stick.
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.Drive(
|
||||
// Multiply by max speed to map the joystick unitless inputs to
|
||||
// actual units. This will map the [-1, 1] to [max speed backwards,
|
||||
// max speed forwards], converting them to actual units.
|
||||
m_driverController.GetLeftY() * AutoConstants::kMaxSpeed,
|
||||
m_driverController.GetLeftX() * AutoConstants::kMaxSpeed,
|
||||
m_driverController.GetRightX() * AutoConstants::kMaxAngularSpeed,
|
||||
false);
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {}
|
||||
|
||||
frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
|
||||
// Set up config for trajectory
|
||||
frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
|
||||
AutoConstants::kMaxAcceleration);
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
config.SetKinematics(m_drive.kDriveKinematics);
|
||||
|
||||
// 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);
|
||||
|
||||
frc::ProfiledPIDController<units::radians> thetaController{
|
||||
AutoConstants::kPThetaController, 0, 0,
|
||||
AutoConstants::kThetaControllerConstraints};
|
||||
|
||||
thetaController.EnableContinuousInput(units::radian_t{-std::numbers::pi},
|
||||
units::radian_t{std::numbers::pi});
|
||||
|
||||
frc2::CommandPtr swerveControllerCommand =
|
||||
frc2::SwerveControllerCommand<4>(
|
||||
exampleTrajectory, [this]() { return m_drive.GetPose(); },
|
||||
|
||||
m_drive.kDriveKinematics,
|
||||
|
||||
frc::PIDController{AutoConstants::kPXController, 0, 0},
|
||||
frc::PIDController{AutoConstants::kPYController, 0, 0},
|
||||
thetaController,
|
||||
|
||||
[this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
|
||||
|
||||
{&m_drive})
|
||||
.ToPtr();
|
||||
|
||||
// Reset odometry to the initial pose of the trajectory, run path following
|
||||
// command, then stop at the end.
|
||||
return frc2::cmd::Sequence(
|
||||
frc2::InstantCommand(
|
||||
[this, initialPose = exampleTrajectory.InitialPose()]() {
|
||||
m_drive.ResetOdometry(initialPose);
|
||||
},
|
||||
{})
|
||||
.ToPtr(),
|
||||
std::move(swerveControllerCommand),
|
||||
frc2::InstantCommand(
|
||||
[this] { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {})
|
||||
.ToPtr());
|
||||
}
|
||||
@@ -1,111 +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 <units/angle.h>
|
||||
#include <units/angular_velocity.h>
|
||||
#include <units/velocity.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
using namespace DriveConstants;
|
||||
|
||||
DriveSubsystem::DriveSubsystem()
|
||||
: m_frontLeft{kFrontLeftDriveMotorPort,
|
||||
kFrontLeftTurningMotorPort,
|
||||
kFrontLeftDriveEncoderPorts,
|
||||
kFrontLeftTurningEncoderPorts,
|
||||
kFrontLeftDriveEncoderReversed,
|
||||
kFrontLeftTurningEncoderReversed},
|
||||
|
||||
m_rearLeft{
|
||||
kRearLeftDriveMotorPort, kRearLeftTurningMotorPort,
|
||||
kRearLeftDriveEncoderPorts, kRearLeftTurningEncoderPorts,
|
||||
kRearLeftDriveEncoderReversed, kRearLeftTurningEncoderReversed},
|
||||
|
||||
m_frontRight{
|
||||
kFrontRightDriveMotorPort, kFrontRightTurningMotorPort,
|
||||
kFrontRightDriveEncoderPorts, kFrontRightTurningEncoderPorts,
|
||||
kFrontRightDriveEncoderReversed, kFrontRightTurningEncoderReversed},
|
||||
|
||||
m_rearRight{
|
||||
kRearRightDriveMotorPort, kRearRightTurningMotorPort,
|
||||
kRearRightDriveEncoderPorts, kRearRightTurningEncoderPorts,
|
||||
kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed},
|
||||
|
||||
m_odometry{kDriveKinematics,
|
||||
m_gyro.GetRotation2d(),
|
||||
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
|
||||
m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
|
||||
frc::Pose2d{}} {}
|
||||
|
||||
void DriveSubsystem::Periodic() {
|
||||
// Implementation of subsystem periodic method goes here.
|
||||
m_odometry.Update(m_gyro.GetRotation2d(),
|
||||
{m_frontLeft.GetPosition(), m_rearLeft.GetPosition(),
|
||||
m_frontRight.GetPosition(), m_rearRight.GetPosition()});
|
||||
}
|
||||
|
||||
void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
|
||||
units::meters_per_second_t ySpeed,
|
||||
units::radians_per_second_t rot, bool fieldRelative,
|
||||
units::second_t period) {
|
||||
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
|
||||
if (fieldRelative) {
|
||||
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d());
|
||||
}
|
||||
chassisSpeeds = chassisSpeeds.Discretize(period);
|
||||
|
||||
auto states = kDriveKinematics.ToSwerveModuleStates(chassisSpeeds);
|
||||
kDriveKinematics.DesaturateWheelSpeeds(&states, AutoConstants::kMaxSpeed);
|
||||
|
||||
auto [fl, fr, bl, br] = states;
|
||||
m_frontLeft.SetDesiredState(fl);
|
||||
m_frontRight.SetDesiredState(fr);
|
||||
m_rearLeft.SetDesiredState(bl);
|
||||
m_rearRight.SetDesiredState(br);
|
||||
}
|
||||
|
||||
void DriveSubsystem::SetModuleStates(
|
||||
wpi::array<frc::SwerveModuleState, 4> desiredStates) {
|
||||
kDriveKinematics.DesaturateWheelSpeeds(&desiredStates,
|
||||
AutoConstants::kMaxSpeed);
|
||||
m_frontLeft.SetDesiredState(desiredStates[0]);
|
||||
m_frontRight.SetDesiredState(desiredStates[1]);
|
||||
m_rearLeft.SetDesiredState(desiredStates[2]);
|
||||
m_rearRight.SetDesiredState(desiredStates[3]);
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetEncoders() {
|
||||
m_frontLeft.ResetEncoders();
|
||||
m_rearLeft.ResetEncoders();
|
||||
m_frontRight.ResetEncoders();
|
||||
m_rearRight.ResetEncoders();
|
||||
}
|
||||
|
||||
units::degree_t DriveSubsystem::GetHeading() const {
|
||||
return m_gyro.GetRotation2d().Degrees();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ZeroHeading() {
|
||||
m_gyro.Reset();
|
||||
}
|
||||
|
||||
double DriveSubsystem::GetTurnRate() {
|
||||
return -m_gyro.GetRate();
|
||||
}
|
||||
|
||||
frc::Pose2d DriveSubsystem::GetPose() {
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
|
||||
m_odometry.ResetPosition(
|
||||
GetHeading(),
|
||||
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
|
||||
m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
|
||||
pose);
|
||||
}
|
||||
@@ -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.
|
||||
|
||||
#include "subsystems/SwerveModule.h"
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
SwerveModule::SwerveModule(int driveMotorChannel, int turningMotorChannel,
|
||||
const int driveEncoderPorts[2],
|
||||
const int turningEncoderPorts[2],
|
||||
bool driveEncoderReversed,
|
||||
bool turningEncoderReversed)
|
||||
: m_driveMotor(driveMotorChannel),
|
||||
m_turningMotor(turningMotorChannel),
|
||||
m_driveEncoder(driveEncoderPorts[0], driveEncoderPorts[1]),
|
||||
m_turningEncoder(turningEncoderPorts[0], turningEncoderPorts[1]) {
|
||||
// Set the distance per pulse for the drive encoder. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_driveEncoder.SetDistancePerPulse(
|
||||
ModuleConstants::kDriveEncoderDistancePerPulse);
|
||||
|
||||
// Set whether drive encoder should be reversed or not
|
||||
m_driveEncoder.SetReverseDirection(driveEncoderReversed);
|
||||
|
||||
// Set the distance (in this case, angle) per pulse for the turning encoder.
|
||||
// This is the the angle through an entire rotation (2 * std::numbers::pi)
|
||||
// divided by the encoder resolution.
|
||||
m_turningEncoder.SetDistancePerPulse(
|
||||
ModuleConstants::kTurningEncoderDistancePerPulse);
|
||||
|
||||
// Set whether turning encoder should be reversed or not
|
||||
m_turningEncoder.SetReverseDirection(turningEncoderReversed);
|
||||
|
||||
// Limit the PID Controller's input range between -pi and pi and set the input
|
||||
// to be continuous.
|
||||
m_turningPIDController.EnableContinuousInput(
|
||||
units::radian_t{-std::numbers::pi}, units::radian_t{std::numbers::pi});
|
||||
}
|
||||
|
||||
frc::SwerveModuleState SwerveModule::GetState() {
|
||||
return {units::meters_per_second_t{m_driveEncoder.GetRate()},
|
||||
units::radian_t{m_turningEncoder.GetDistance()}};
|
||||
}
|
||||
|
||||
frc::SwerveModulePosition SwerveModule::GetPosition() {
|
||||
return {units::meter_t{m_driveEncoder.GetDistance()},
|
||||
units::radian_t{m_turningEncoder.GetDistance()}};
|
||||
}
|
||||
|
||||
void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
|
||||
frc::Rotation2d encoderRotation{
|
||||
units::radian_t{m_turningEncoder.GetDistance()}};
|
||||
|
||||
// Optimize the reference state to avoid spinning further than 90 degrees
|
||||
referenceState.Optimize(encoderRotation);
|
||||
|
||||
// Scale speed by cosine of angle error. This scales down movement
|
||||
// perpendicular to the desired direction of travel that can occur when
|
||||
// modules change directions. This results in smoother driving.
|
||||
referenceState.CosineScale(encoderRotation);
|
||||
|
||||
// Calculate the drive output from the drive PID controller.
|
||||
const auto driveOutput = m_drivePIDController.Calculate(
|
||||
m_driveEncoder.GetRate(), referenceState.speed.value());
|
||||
|
||||
// Calculate the turning motor output from the turning PID controller.
|
||||
auto turnOutput = m_turningPIDController.Calculate(
|
||||
units::radian_t{m_turningEncoder.GetDistance()},
|
||||
referenceState.angle.Radians());
|
||||
|
||||
// Set the motor outputs.
|
||||
m_driveMotor.Set(driveOutput);
|
||||
m_turningMotor.Set(turnOutput);
|
||||
}
|
||||
|
||||
void SwerveModule::ResetEncoders() {
|
||||
m_driveEncoder.Reset();
|
||||
m_turningEncoder.Reset();
|
||||
}
|
||||
@@ -1,116 +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/TimedRobot.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/angular_acceleration.h>
|
||||
#include <units/angular_velocity.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 kFrontLeftDriveMotorPort = 0;
|
||||
inline constexpr int kRearLeftDriveMotorPort = 2;
|
||||
inline constexpr int kFrontRightDriveMotorPort = 4;
|
||||
inline constexpr int kRearRightDriveMotorPort = 6;
|
||||
|
||||
inline constexpr int kFrontLeftTurningMotorPort = 1;
|
||||
inline constexpr int kRearLeftTurningMotorPort = 3;
|
||||
inline constexpr int kFrontRightTurningMotorPort = 5;
|
||||
inline constexpr int kRearRightTurningMotorPort = 7;
|
||||
|
||||
inline constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1};
|
||||
inline constexpr int kRearLeftTurningEncoderPorts[2]{2, 3};
|
||||
inline constexpr int kFrontRightTurningEncoderPorts[2]{4, 5};
|
||||
inline constexpr int kRearRightTurningEncoderPorts[2]{6, 7};
|
||||
|
||||
inline constexpr bool kFrontLeftTurningEncoderReversed = false;
|
||||
inline constexpr bool kRearLeftTurningEncoderReversed = true;
|
||||
inline constexpr bool kFrontRightTurningEncoderReversed = false;
|
||||
inline constexpr bool kRearRightTurningEncoderReversed = true;
|
||||
|
||||
inline constexpr int kFrontLeftDriveEncoderPorts[2]{8, 9};
|
||||
inline constexpr int kRearLeftDriveEncoderPorts[2]{10, 11};
|
||||
inline constexpr int kFrontRightDriveEncoderPorts[2]{12, 13};
|
||||
inline constexpr int kRearRightDriveEncoderPorts[2]{14, 15};
|
||||
|
||||
inline constexpr bool kFrontLeftDriveEncoderReversed = false;
|
||||
inline constexpr bool kRearLeftDriveEncoderReversed = true;
|
||||
inline constexpr bool kFrontRightDriveEncoderReversed = false;
|
||||
inline constexpr bool kRearRightDriveEncoderReversed = true;
|
||||
|
||||
// If you call DriveSubsystem::Drive with a different period make sure to update
|
||||
// this.
|
||||
inline constexpr units::second_t kDrivePeriod = frc::TimedRobot::kDefaultPeriod;
|
||||
|
||||
// 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 SysId tool provides a convenient
|
||||
// method for obtaining these values for your robot.
|
||||
inline constexpr auto ks = 1_V;
|
||||
inline constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
|
||||
inline constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
|
||||
|
||||
// Example value only - as above, this must be tuned for your drive!
|
||||
inline constexpr double kPFrontLeftVel = 0.5;
|
||||
inline constexpr double kPRearLeftVel = 0.5;
|
||||
inline constexpr double kPFrontRightVel = 0.5;
|
||||
inline constexpr double kPRearRightVel = 0.5;
|
||||
} // namespace DriveConstants
|
||||
|
||||
namespace ModuleConstants {
|
||||
inline constexpr int kEncoderCPR = 1024;
|
||||
inline constexpr auto kWheelDiameter = 0.15_m;
|
||||
inline constexpr double kDriveEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter.value() * std::numbers::pi) /
|
||||
static_cast<double>(kEncoderCPR);
|
||||
|
||||
inline constexpr double kTurningEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(std::numbers::pi * 2) / static_cast<double>(kEncoderCPR);
|
||||
|
||||
inline constexpr double kPModuleTurningController = 1;
|
||||
inline constexpr double kPModuleDriveController = 1;
|
||||
} // namespace ModuleConstants
|
||||
|
||||
namespace AutoConstants {
|
||||
inline constexpr auto kMaxSpeed = 3_mps;
|
||||
inline constexpr auto kMaxAcceleration = 3_mps_sq;
|
||||
inline constexpr auto kMaxAngularSpeed = 3.142_rad_per_s;
|
||||
inline constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq;
|
||||
|
||||
inline constexpr double kPXController = 0.5;
|
||||
inline constexpr double kPYController = 0.5;
|
||||
inline constexpr double kPThetaController = 0.5;
|
||||
|
||||
//
|
||||
|
||||
extern const frc::TrapezoidProfile<units::radians>::Constraints
|
||||
kThetaControllerConstraints;
|
||||
|
||||
} // namespace AutoConstants
|
||||
|
||||
namespace OIConstants {
|
||||
inline constexpr int kDriverControllerPort = 0;
|
||||
} // namespace OIConstants
|
||||
@@ -1,32 +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/Command.h>
|
||||
|
||||
#include "RobotContainer.h"
|
||||
|
||||
class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
Robot();
|
||||
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 null by default so that if testing teleop it
|
||||
// doesn't have undefined behavior and potentially crash.
|
||||
std::optional<frc2::CommandPtr> m_autonomousCommand;
|
||||
|
||||
RobotContainer m_container;
|
||||
};
|
||||
@@ -1,43 +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/controller/ProfiledPIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/CommandPtr.h>
|
||||
#include <frc2/command/InstantCommand.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();
|
||||
|
||||
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;
|
||||
|
||||
void ConfigureButtonBindings();
|
||||
};
|
||||
@@ -1,117 +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/AnalogGyro.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/drive/MecanumDrive.h>
|
||||
#include <frc/geometry/Pose2d.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/kinematics/ChassisSpeeds.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/kinematics/SwerveDriveOdometry.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc2/command/SubsystemBase.h>
|
||||
|
||||
#include "Constants.h"
|
||||
#include "SwerveModule.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 at given x, y and theta speeds. Speeds range from [-1, 1]
|
||||
* and the linear speeds have no effect on the angular speed.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction
|
||||
* (forward/backwards).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to
|
||||
* the field.
|
||||
*/
|
||||
void Drive(units::meters_per_second_t xSpeed,
|
||||
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
|
||||
bool fieldRelative,
|
||||
units::second_t period = DriveConstants::kDrivePeriod);
|
||||
|
||||
/**
|
||||
* Resets the drive encoders to currently read a position of 0.
|
||||
*/
|
||||
void ResetEncoders();
|
||||
|
||||
/**
|
||||
* Sets the drive MotorControllers to a power from -1 to 1.
|
||||
*/
|
||||
void SetModuleStates(wpi::array<frc::SwerveModuleState, 4> desiredStates);
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot.
|
||||
*
|
||||
* @return the robot's heading in degrees, from 180 to 180
|
||||
*/
|
||||
units::degree_t GetHeading() const;
|
||||
|
||||
/**
|
||||
* Zeroes the heading of the robot.
|
||||
*/
|
||||
void ZeroHeading();
|
||||
|
||||
/**
|
||||
* 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();
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
void ResetOdometry(frc::Pose2d pose);
|
||||
|
||||
units::meter_t kTrackwidth =
|
||||
0.5_m; // Distance between centers of right and left wheels on robot
|
||||
units::meter_t kWheelBase =
|
||||
0.7_m; // Distance between centers of front and back wheels on robot
|
||||
|
||||
frc::SwerveDriveKinematics<4> kDriveKinematics{
|
||||
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
|
||||
|
||||
private:
|
||||
// Components (e.g. motor controllers and sensors) should generally be
|
||||
// declared private and exposed only through public methods.
|
||||
|
||||
SwerveModule m_frontLeft;
|
||||
SwerveModule m_rearLeft;
|
||||
SwerveModule m_frontRight;
|
||||
SwerveModule m_rearRight;
|
||||
|
||||
// The gyro sensor
|
||||
frc::AnalogGyro m_gyro{0};
|
||||
|
||||
// Odometry class for tracking robot pose
|
||||
// 4 defines the number of modules
|
||||
frc::SwerveDriveOdometry<4> m_odometry;
|
||||
};
|
||||
@@ -1,57 +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/Encoder.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/kinematics/SwerveModulePosition.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/motorcontrol/Spark.h>
|
||||
#include <frc/trajectory/TrapezoidProfile.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class SwerveModule {
|
||||
public:
|
||||
SwerveModule(int driveMotorChannel, int turningMotorChannel,
|
||||
const int driveEncoderPorts[2], const int turningEncoderPorts[2],
|
||||
bool driveEncoderReversed, bool turningEncoderReversed);
|
||||
|
||||
frc::SwerveModuleState GetState();
|
||||
|
||||
frc::SwerveModulePosition GetPosition();
|
||||
|
||||
void SetDesiredState(frc::SwerveModuleState& state);
|
||||
|
||||
void ResetEncoders();
|
||||
|
||||
private:
|
||||
// We have to use meters here instead of radians due to the fact that
|
||||
// ProfiledPIDController's constraints only take in meters per second and
|
||||
// meters per second squared.
|
||||
|
||||
static constexpr auto kModuleMaxAngularVelocity =
|
||||
units::radians_per_second_t{std::numbers::pi};
|
||||
static constexpr auto kModuleMaxAngularAcceleration =
|
||||
units::radians_per_second_squared_t{std::numbers::pi * 2.0};
|
||||
|
||||
frc::Spark m_driveMotor;
|
||||
frc::Spark m_turningMotor;
|
||||
|
||||
frc::Encoder m_driveEncoder;
|
||||
frc::Encoder m_turningEncoder;
|
||||
|
||||
frc::PIDController m_drivePIDController{
|
||||
ModuleConstants::kPModuleDriveController, 0, 0};
|
||||
frc::ProfiledPIDController<units::radians> m_turningPIDController{
|
||||
ModuleConstants::kPModuleTurningController,
|
||||
0.0,
|
||||
0.0,
|
||||
{kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
|
||||
};
|
||||
@@ -462,42 +462,6 @@
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "MecanumControllerCommand",
|
||||
"description": "Follow a pre-generated trajectory with a mecanum drive using MecanumControllerCommand.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Mecanum Drive",
|
||||
"Gyro",
|
||||
"Encoder",
|
||||
"Odometry",
|
||||
"Trajectory",
|
||||
"Path Following",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "MecanumControllerCommand",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2,
|
||||
"hasunittests": true
|
||||
},
|
||||
{
|
||||
"name": "SwerveControllerCommand",
|
||||
"description": "Follow a pre-generated trajectory with a swerve drive using SwerveControllerCommand.",
|
||||
"tags": [
|
||||
"Command-based",
|
||||
"Swerve Drive",
|
||||
"Gyro",
|
||||
"Encoder",
|
||||
"Odometry",
|
||||
"Trajectory",
|
||||
"Path Following",
|
||||
"XboxController"
|
||||
],
|
||||
"foldername": "SwerveControllerCommand",
|
||||
"gradlebase": "cpp",
|
||||
"commandversion": 2,
|
||||
"hasunittests": true
|
||||
},
|
||||
{
|
||||
"name": "DriveDistanceOffboard",
|
||||
"description": "Drive a differential drivetrain a set distance using TrapezoidProfile and smart motor controller PID.",
|
||||
|
||||
Reference in New Issue
Block a user