diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp index 87aac29c09..dd418d2a75 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp @@ -64,16 +64,20 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { // Pass the config config); + frc::ProfiledPIDController thetaController{ + AutoConstants::kPThetaController, 0, 0, + AutoConstants::kThetaControllerConstraints}; + + thetaController.EnableContinuousInput(units::radian_t(-wpi::math::pi), + units::radian_t(wpi::math::pi)); + frc2::SwerveControllerCommand<4> swerveControllerCommand( exampleTrajectory, [this]() { return m_drive.GetPose(); }, m_drive.kDriveKinematics, frc2::PIDController(AutoConstants::kPXController, 0, 0), - frc2::PIDController(AutoConstants::kPYController, 0, 0), - frc::ProfiledPIDController( - AutoConstants::kPThetaController, 0, 0, - AutoConstants::kThetaControllerConstraints), + frc2::PIDController(AutoConstants::kPYController, 0, 0), thetaController, [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); }, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java index 503cede410..26e6f22043 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -98,6 +98,10 @@ public class RobotContainer { config ); + var thetaController = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, + AutoConstants.kThetaControllerConstraints); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( exampleTrajectory, m_robotDrive::getPose, //Functional interface to feed supplier @@ -105,9 +109,7 @@ public class RobotContainer { //Position controllers new PIDController(AutoConstants.kPXController, 0, 0), - new PIDController(AutoConstants.kPYController, 0, 0), - new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, - AutoConstants.kThetaControllerConstraints), + new PIDController(AutoConstants.kPYController, 0, 0), thetaController, m_robotDrive::setModuleStates,