From b3deda38c9f458168846c866f90b149481861dd8 Mon Sep 17 00:00:00 2001
From: Prateek Machiraju <prateek.machiraju@gmail.com>
Date: Fri, 4 Dec 2020 20:34:16 -0500
Subject: [PATCH] [examples] Zero motors on disabledInit() in sim physics
 examples (#2906)

This ensures that mechanisms will stop moving if the robot is disabled while motors are in motion
---
 .../StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp    | 1 +
 .../cpp/RobotContainer.cpp                                 | 2 ++
 .../include/RobotContainer.h                               | 1 +
 .../statespacedifferentialdrivesimulation/Robot.java       | 1 +
 .../RobotContainer.java                                    | 7 +++++++
 5 files changed, 12 insertions(+)

diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp
index 26d7499969..3d5602ee49 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp
@@ -31,6 +31,7 @@ void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
  */
 void Robot::DisabledInit() {
   frc2::CommandScheduler::GetInstance().CancelAll();
+  m_container.ZeroAllOutputs();
 }
 
 void Robot::DisabledPeriodic() {}
diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
index 760812278e..d74395e7a2 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
@@ -36,6 +36,8 @@ RobotContainer::RobotContainer() {
       {&m_drive}));
 }
 
+void RobotContainer::ZeroAllOutputs() { m_drive.TankDriveVolts(0_V, 0_V); }
+
 const DriveSubsystem& RobotContainer::GetRobotDrive() const { return m_drive; }
 
 void RobotContainer::ConfigureButtonBindings() {
diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h
index 06cbf7cfc2..9f734a055d 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h
@@ -30,6 +30,7 @@ class RobotContainer {
  public:
   RobotContainer();
 
+  void ZeroAllOutputs();
   frc2::Command* GetAutonomousCommand();
   const DriveSubsystem& GetRobotDrive() const;
 
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
index 485dfc3be6..64ab06f629 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
@@ -55,5 +55,6 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledInit() {
     CommandScheduler.getInstance().cancelAll();
+    m_robotContainer.zeroAllOutputs();
   }
 }
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
index 0790a40d14..901e7ab448 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
@@ -77,6 +77,13 @@ public class RobotContainer {
     return m_robotDrive;
   }
 
+  /**
+   * Zeros the outputs of all subsystems.
+   */
+  public void zeroAllOutputs() {
+    m_robotDrive.tankDriveVolts(0, 0);
+  }
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *