diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp index cd19aeb7d8..be32fbbb71 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-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. */ @@ -27,7 +27,7 @@ void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); } * can use it to reset any subsystem information you want to clear when the * robot is disabled. */ -void Robot::DisabledInit() {} +void Robot::DisabledInit() { m_container.DisablePIDSubsystems(); } void Robot::DisabledPeriodic() {} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp index ba7bbf5777..d3a01338e7 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp @@ -32,12 +32,25 @@ void RobotContainer::ConfigureButtonBindings() { // Move the arm to 2 radians above horizontal when the 'A' button is pressed. frc2::JoystickButton(&m_driverController, 1) - .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm}); + .WhenPressed( + [this] { + m_arm.SetGoal(2_rad); + m_arm.Enable(); + }, + {&m_arm}); // Move the arm to neutral position when the 'B' button is pressed. - frc2::JoystickButton(&m_driverController, 1) - .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); }, - {&m_arm}); + frc2::JoystickButton(&m_driverController, 2) + .WhenPressed( + [this] { + m_arm.SetGoal(ArmConstants::kArmOffset); + m_arm.Enable(); + }, + {&m_arm}); + + // Disable the arm controller when Y is pressed. + frc2::JoystickButton(&m_driverController, 4) + .WhenPressed([this] { m_arm.Disable(); }, {&m_arm}); // While holding the shoulder button, drive at half speed frc2::JoystickButton(&m_driverController, 6) @@ -45,6 +58,8 @@ void RobotContainer::ConfigureButtonBindings() { .WhenReleased([this] { m_drive.SetMaxOutput(1); }); } +void RobotContainer::DisablePIDSubsystems() { m_arm.Disable(); } + frc2::Command* RobotContainer::GetAutonomousCommand() { // Runs the chosen command in autonomous return new frc2::InstantCommand([] {}); diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h index fa07359c84..c238cb6fb8 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h @@ -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. */ @@ -35,6 +35,12 @@ class RobotContainer { public: RobotContainer(); + /** + * Disables all ProfiledPIDSubsystem and PIDSubsystem instances. + * This should be called on robot disable to prevent integral windup. + */ + void DisablePIDSubsystems(); + frc2::Command* GetAutonomousCommand(); private: diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java index a171fc6c99..8c9cef6eb9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-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. */ @@ -54,6 +54,7 @@ public class Robot extends TimedRobot { */ @Override public void disabledInit() { + m_robotContainer.disablePIDSubsystems(); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java index 5558f25ec8..afe7317968 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/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. */ @@ -62,11 +62,21 @@ public class RobotContainer { // Move the arm to 2 radians above horizontal when the 'A' button is pressed. new JoystickButton(m_driverController, Button.kA.value) - .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm); + .whenPressed(() -> { + m_robotArm.setGoal(2); + m_robotArm.enable(); + }, m_robotArm); // Move the arm to neutral position when the 'B' button is pressed. new JoystickButton(m_driverController, Button.kB.value) - .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm); + .whenPressed(() -> { + m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads); + m_robotArm.enable(); + }, m_robotArm); + + // Disable the arm controller when Y is pressed. + new JoystickButton(m_driverController, Button.kY.value) + .whenPressed(m_robotArm::disable); // Drive at half speed when the bumper is held new JoystickButton(m_driverController, Button.kBumperRight.value) @@ -74,6 +84,13 @@ public class RobotContainer { .whenReleased(() -> m_robotDrive.setMaxOutput(1)); } + /** + * Disables all ProfiledPIDSubsystem and PIDSubsystem instances. This + * should be called on robot disable to prevent integral windup. + */ + public void disablePIDSubsystems() { + m_robotArm.disable(); + } /** * Use this to pass the autonomous command to the main {@link Robot} class.