From e1480ec7985ddfce3558f1267addf76014d1788a Mon Sep 17 00:00:00 2001
From: Omar Zrien <OZrien@crosstheroadelectronics.com>
Date: Tue, 16 Dec 2014 21:37:20 -0500
Subject: [PATCH] Noticed that if changeControlMode() is called every teleop
 loop, it causes motor to stutter as it enters and immediately leaves
 kDisabled. A simple improvement was to only perform the
 disable-before-next-set strategy if the caller's request mode is not equal to
 the current mode.

To keep things simple, SetControlMode was renamed to private method ApplyControlMode so we can still invoke it from c'tor.
Then, the new impl'n of SetControlMode() just calls ApplyControlMode() when caller's request mode is different.  That takes care of direct-calls from team source, and indirect calls through enableControl().

Applied to both c++ and java.
Tested in java so far...

Change-Id: I934c06c5339d933918470659acd635e12eb4d113
---
 wpilibc/wpilibC++Devices/include/CANTalon.h   |  7 ++++++
 wpilibc/wpilibC++Devices/src/CANTalon.cpp     | 22 +++++++++++++++----
 .../java/edu/wpi/first/wpilibj/CANTalon.java  | 20 +++++++++++++----
 3 files changed, 41 insertions(+), 8 deletions(-)

diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h
index ab45adb3e9..85552f8e83 100644
--- a/wpilibc/wpilibC++Devices/include/CANTalon.h
+++ b/wpilibc/wpilibC++Devices/include/CANTalon.h
@@ -139,4 +139,11 @@ private:
 
   double m_setPoint;
   static const unsigned int kDelayForSolicitedSignalsUs = 4000;
+  /**
+   * Fixup the sendMode so Set() serializes the correct demand value.
+   * Also fills the modeSelecet in the control frame to disabled.
+   * @param mode Control mode to ultimately enter once user calls Set().
+   * @see Set()
+   */
+  void ApplyControlMode(CANSpeedController::ControlMode mode);
 };
diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp
index d023234d50..907587eba4 100644
--- a/wpilibc/wpilibC++Devices/src/CANTalon.cpp
+++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp
@@ -21,7 +21,7 @@ CANTalon::CANTalon(int deviceNumber)
   , m_controlMode(kPercentVbus)
   , m_setPoint(0)
 {
-  SetControlMode(m_controlMode);
+  ApplyControlMode(m_controlMode);
   m_impl->SetProfileSlotSelect(m_profile);
 }
 /**
@@ -39,7 +39,7 @@ CANTalon::CANTalon(int deviceNumber,int controlPeriodMs)
   , m_controlMode(kPercentVbus)
   , m_setPoint(0)
 {
-  SetControlMode(m_controlMode);
+  ApplyControlMode(m_controlMode);
   m_impl->SetProfileSlotSelect(m_profile);
 }
 CANTalon::~CANTalon() {
@@ -1100,9 +1100,12 @@ void CANTalon::ConfigFaultTime(float faultTime)
 }
 
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * Fixup the sendMode so Set() serializes the correct demand value.
+ * Also fills the modeSelecet in the control frame to disabled.
+ * @param mode Control mode to ultimately enter once user calls Set().
+ * @see Set()
  */
-void CANTalon::SetControlMode(CANSpeedController::ControlMode mode)
+void CANTalon::ApplyControlMode(CANSpeedController::ControlMode mode)
 {
   m_controlMode = mode;
   switch (mode) {
@@ -1131,6 +1134,17 @@ void CANTalon::SetControlMode(CANSpeedController::ControlMode mode)
 		wpi_setErrorWithContext(status, getHALErrorMessage(status));
 	}
 }
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+void CANTalon::SetControlMode(CANSpeedController::ControlMode mode)
+{
+  if(m_controlMode == mode){
+    /* we already are in this mode, don't perform disable workaround */
+  }else{
+    ApplyControlMode(mode);
+  }
+}
 
 /**
  * TODO documentation (see CANJaguar.cpp)
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java
index baf76b4050..5b74a8d85f 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java
@@ -91,7 +91,7 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController {
     m_profile = 0;
     m_setPoint = 0;
     setProfile(m_profile);
-    changeControlMode(ControlMode.PercentVbus);
+    applyControlMode(ControlMode.PercentVbus);
   }
   public CANTalon(int deviceNumber,int controlPeriodMs) {
     m_deviceNumber = deviceNumber;
@@ -101,7 +101,7 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController {
     m_profile = 0;
     m_setPoint = 0;
     setProfile(m_profile);
-    changeControlMode(ControlMode.PercentVbus);
+    applyControlMode(ControlMode.PercentVbus);
   }
   public CANTalon(int deviceNumber,int controlPeriodMs) {
     m_deviceNumber = deviceNumber;
@@ -425,14 +425,26 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController {
   public ControlMode getControlMode() {
     return m_controlMode;
   }
-
-  public void changeControlMode(ControlMode controlMode) {
+  /**
+   * Fixup the m_controlMode so set() serializes the correct demand value.
+   * Also fills the modeSelecet in the control frame to disabled.
+   * @param controlMode Control mode to ultimately enter once user calls set().
+   * @see #set
+   */  
+  public void applyControlMode(ControlMode controlMode) {
     m_controlMode = controlMode;
     if (controlMode == ControlMode.Disabled)
       m_controlEnabled = false;
     // Disable until set() is called.
     m_impl.SetModeSelect(ControlMode.Disabled.value);
   }
+  public void changeControlMode(ControlMode controlMode) {
+    if(m_controlMode == controlMode){
+      /* we already are in this mode, don't perform disable workaround */
+    }else{
+      applyControlMode(controlMode);
+    }
+  }
 
   public void setFeedbackDevice(FeedbackDevice device) {
     m_impl.SetFeedbackDeviceSelect(device.value);