From 952ebb11adfe93462211cee6ba6f1743dcaa5c9e Mon Sep 17 00:00:00 2001 From: Fredric Silberberg Date: Wed, 10 Feb 2016 14:00:06 -0500 Subject: [PATCH] This adds StopMotor() to the SpeedController interface for C++ and Java. For Java, this is as simple as just adding it, as all motors already have an implementation from MotorSafety that is correctly resolved. For C++, I had to override StopMotor in the classes that descend from SafePWM and explicitly call the SafePWM version. RobotDrive now calls StopMotor on each of its SpeedControllers, instead of calling Disable or setting the motor to 0.0 as it was doing previously. Additional small formatting corrections to the previous commit starting this were added. Change-Id: Ie94565394927a910ce74bc628670ac3d658d8df9 --- wpilibc/Athena/include/CANSpeedController.h | 1 + wpilibc/Athena/include/Jaguar.h | 1 + wpilibc/Athena/include/SD540.h | 1 + wpilibc/Athena/include/Spark.h | 1 + wpilibc/Athena/include/SpeedController.h | 5 + wpilibc/Athena/include/Talon.h | 1 + wpilibc/Athena/include/TalonSRX.h | 1 + wpilibc/Athena/include/Victor.h | 1 + wpilibc/Athena/include/VictorSP.h | 1 + wpilibc/Athena/src/CANJaguar.cpp | 4 +- wpilibc/Athena/src/CANTalon.cpp | 8 +- wpilibc/Athena/src/Jaguar.cpp | 5 + wpilibc/Athena/src/RobotDrive.cpp | 8 +- wpilibc/Athena/src/SD540.cpp | 5 + wpilibc/Athena/src/Spark.cpp | 5 + wpilibc/Athena/src/Talon.cpp | 5 + wpilibc/Athena/src/TalonSRX.cpp | 5 + wpilibc/Athena/src/Victor.cpp | 5 + wpilibc/Athena/src/VictorSP.cpp | 5 + .../java/edu/wpi/first/wpilibj/CANJaguar.java | 20 ++- .../java/edu/wpi/first/wpilibj/CANTalon.java | 12 +- .../wpi/first/wpilibj/MotorSafetyHelper.java | 2 +- .../edu/wpi/first/wpilibj/RobotDrive.java | 8 +- .../wpi/first/wpilibj/SpeedController.java | 8 +- .../wpi/first/wpilibj/MotorSafetyHelper.java | 132 ++++++++++++++++++ 25 files changed, 215 insertions(+), 35 deletions(-) rename wpilibj/src/{shared => athena}/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java (97%) create mode 100644 wpilibj/src/sim/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java diff --git a/wpilibc/Athena/include/CANSpeedController.h b/wpilibc/Athena/include/CANSpeedController.h index 773fb9984f..5d7602639f 100644 --- a/wpilibc/Athena/include/CANSpeedController.h +++ b/wpilibc/Athena/include/CANSpeedController.h @@ -65,6 +65,7 @@ class CANSpeedController : public SpeedController { virtual float Get() const = 0; virtual void Set(float value, uint8_t syncGroup = 0) = 0; + virtual void StopMotor() = 0; virtual void Disable() = 0; virtual void SetP(double p) = 0; virtual void SetI(double i) = 0; diff --git a/wpilibc/Athena/include/Jaguar.h b/wpilibc/Athena/include/Jaguar.h index 7e25b9b4a4..c129559869 100644 --- a/wpilibc/Athena/include/Jaguar.h +++ b/wpilibc/Athena/include/Jaguar.h @@ -21,6 +21,7 @@ class Jaguar : public SafePWM, public SpeedController { virtual void Set(float value, uint8_t syncGroup = 0) override; virtual float Get() const override; virtual void Disable() override; + virtual void StopMotor() override; virtual void PIDWrite(float output) override; virtual void SetInverted(bool isInverted) override; diff --git a/wpilibc/Athena/include/SD540.h b/wpilibc/Athena/include/SD540.h index e2c98e9e80..9e98678929 100644 --- a/wpilibc/Athena/include/SD540.h +++ b/wpilibc/Athena/include/SD540.h @@ -21,6 +21,7 @@ class SD540 : public SafePWM, public SpeedController { virtual void Set(float value, uint8_t syncGroup = 0) override; virtual float Get() const override; virtual void Disable() override; + virtual void StopMotor() override; virtual void PIDWrite(float output) override; diff --git a/wpilibc/Athena/include/Spark.h b/wpilibc/Athena/include/Spark.h index 65d5c6ab7a..d4859ce216 100644 --- a/wpilibc/Athena/include/Spark.h +++ b/wpilibc/Athena/include/Spark.h @@ -21,6 +21,7 @@ class Spark : public SafePWM, public SpeedController { virtual void Set(float value, uint8_t syncGroup = 0) override; virtual float Get() const override; virtual void Disable() override; + virtual void StopMotor() override; virtual void PIDWrite(float output) override; diff --git a/wpilibc/Athena/include/SpeedController.h b/wpilibc/Athena/include/SpeedController.h index 6adba5d787..47ba84371c 100644 --- a/wpilibc/Athena/include/SpeedController.h +++ b/wpilibc/Athena/include/SpeedController.h @@ -48,4 +48,9 @@ class SpeedController : public PIDOutput { * @return isInverted The state of inversion, true is inverted. */ virtual bool GetInverted() const = 0; + + /** + * Common interface to stop the motor until Set is called again. + */ + virtual void StopMotor() = 0; }; diff --git a/wpilibc/Athena/include/Talon.h b/wpilibc/Athena/include/Talon.h index 1908d19b60..0a8801229b 100644 --- a/wpilibc/Athena/include/Talon.h +++ b/wpilibc/Athena/include/Talon.h @@ -25,6 +25,7 @@ class Talon : public SafePWM, public SpeedController { virtual void PIDWrite(float output) override; virtual void SetInverted(bool isInverted) override; virtual bool GetInverted() const override; + virtual void StopMotor() override; private: bool m_isInverted = false; diff --git a/wpilibc/Athena/include/TalonSRX.h b/wpilibc/Athena/include/TalonSRX.h index b64324ee6b..61b1fd8fbe 100644 --- a/wpilibc/Athena/include/TalonSRX.h +++ b/wpilibc/Athena/include/TalonSRX.h @@ -22,6 +22,7 @@ class TalonSRX : public SafePWM, public SpeedController { virtual void Set(float value, uint8_t syncGroup = 0) override; virtual float Get() const override; virtual void Disable() override; + virtual void StopMotor() override; virtual void PIDWrite(float output) override; virtual void SetInverted(bool isInverted) override; diff --git a/wpilibc/Athena/include/Victor.h b/wpilibc/Athena/include/Victor.h index eddc2e28ba..2aa2ecfd2b 100644 --- a/wpilibc/Athena/include/Victor.h +++ b/wpilibc/Athena/include/Victor.h @@ -24,6 +24,7 @@ class Victor : public SafePWM, public SpeedController { virtual void Set(float value, uint8_t syncGroup = 0) override; virtual float Get() const override; virtual void Disable() override; + virtual void StopMotor() override; virtual void PIDWrite(float output) override; diff --git a/wpilibc/Athena/include/VictorSP.h b/wpilibc/Athena/include/VictorSP.h index e90a293463..a53ccfde77 100644 --- a/wpilibc/Athena/include/VictorSP.h +++ b/wpilibc/Athena/include/VictorSP.h @@ -21,6 +21,7 @@ class VictorSP : public SafePWM, public SpeedController { virtual void Set(float value, uint8_t syncGroup = 0) override; virtual float Get() const override; virtual void Disable() override; + virtual void StopMotor() override; virtual void PIDWrite(float output) override; diff --git a/wpilibc/Athena/src/CANJaguar.cpp b/wpilibc/Athena/src/CANJaguar.cpp index c267cd5aae..7dcca19031 100644 --- a/wpilibc/Athena/src/CANJaguar.cpp +++ b/wpilibc/Athena/src/CANJaguar.cpp @@ -247,10 +247,10 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) { uint8_t dataSize; if (m_safetyHelper) m_safetyHelper->Feed(); - + if (m_stopped) { EnableControl(); - m_stopped = false; + m_stopped = false; } if (m_controlEnabled) { diff --git a/wpilibc/Athena/src/CANTalon.cpp b/wpilibc/Athena/src/CANTalon.cpp index e59982cff6..9adc66c37f 100644 --- a/wpilibc/Athena/src/CANTalon.cpp +++ b/wpilibc/Athena/src/CANTalon.cpp @@ -134,12 +134,12 @@ float CANTalon::Get() const { void CANTalon::Set(float value, uint8_t syncGroup) { /* feed safety helper since caller just updated our output */ m_safetyHelper->Feed(); - + if (m_stopped) { EnableControl(); - m_stopped = false; + m_stopped = false; } - + if (m_controlEnabled) { m_setPoint = value; /* cache set point for GetSetpoint() */ CTR_Code status = CTR_OKAY; @@ -1898,7 +1898,7 @@ bool CANTalon::GetInverted() const { return m_isInverted; } * * @deprecated Call Disable instead. */ -void CANTalon::StopMotor() { +void CANTalon::StopMotor() { Disable(); m_stopped = true; } diff --git a/wpilibc/Athena/src/Jaguar.cpp b/wpilibc/Athena/src/Jaguar.cpp index 4f285c4422..b2bd57aacb 100644 --- a/wpilibc/Athena/src/Jaguar.cpp +++ b/wpilibc/Athena/src/Jaguar.cpp @@ -77,3 +77,8 @@ bool Jaguar::GetInverted() const { return m_isInverted; } * @param output Write out the PWM value as was found in the PIDController */ void Jaguar::PIDWrite(float output) { Set(output); } + +/** + * Common interface to stop the motor until Set is called again. + */ +void Jaguar::StopMotor() { this->SafePWM::StopMotor(); } \ No newline at end of file diff --git a/wpilibc/Athena/src/RobotDrive.cpp b/wpilibc/Athena/src/RobotDrive.cpp index 8fd0f0ea3f..69184e3463 100644 --- a/wpilibc/Athena/src/RobotDrive.cpp +++ b/wpilibc/Athena/src/RobotDrive.cpp @@ -745,9 +745,9 @@ void RobotDrive::GetDescription(std::ostringstream& desc) const { } void RobotDrive::StopMotor() { - if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable(); - if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable(); - if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable(); - if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable(); + if (m_frontLeftMotor != nullptr) m_frontLeftMotor->StopMotor(); + if (m_frontRightMotor != nullptr) m_frontRightMotor->StopMotor(); + if (m_rearLeftMotor != nullptr) m_rearLeftMotor->StopMotor(); + if (m_rearRightMotor != nullptr) m_rearRightMotor->StopMotor(); m_safetyHelper->Feed(); } diff --git a/wpilibc/Athena/src/SD540.cpp b/wpilibc/Athena/src/SD540.cpp index f88893b190..9d2ad1eb0d 100644 --- a/wpilibc/Athena/src/SD540.cpp +++ b/wpilibc/Athena/src/SD540.cpp @@ -86,3 +86,8 @@ void SD540::Disable() { SetRaw(kPwmDisabled); } * @param output Write out the PWM value as was found in the PIDController */ void SD540::PIDWrite(float output) { Set(output); } + +/** + * Common interface to stop the motor until Set is called again. + */ +void SD540::StopMotor() { this->SafePWM::StopMotor(); } diff --git a/wpilibc/Athena/src/Spark.cpp b/wpilibc/Athena/src/Spark.cpp index 343472170b..35171220b0 100644 --- a/wpilibc/Athena/src/Spark.cpp +++ b/wpilibc/Athena/src/Spark.cpp @@ -86,3 +86,8 @@ void Spark::Disable() { SetRaw(kPwmDisabled); } * @param output Write out the PWM value as was found in the PIDController */ void Spark::PIDWrite(float output) { Set(output); } + +/** + * Common interface to stop the motor until Set is called again. + */ +void Spark::StopMotor() { this->SafePWM::StopMotor(); } \ No newline at end of file diff --git a/wpilibc/Athena/src/Talon.cpp b/wpilibc/Athena/src/Talon.cpp index 0d7e18e4c8..64f9db83ea 100644 --- a/wpilibc/Athena/src/Talon.cpp +++ b/wpilibc/Athena/src/Talon.cpp @@ -82,3 +82,8 @@ bool Talon::GetInverted() const { return m_isInverted; } * @param output Write out the PWM value as was found in the PIDController */ void Talon::PIDWrite(float output) { Set(output); } + +/** + * Common interface to stop the motor until Set is called again. + */ +void Talon::StopMotor() { this->SafePWM::StopMotor(); } diff --git a/wpilibc/Athena/src/TalonSRX.cpp b/wpilibc/Athena/src/TalonSRX.cpp index 2bee06d422..e06f8ca2aa 100644 --- a/wpilibc/Athena/src/TalonSRX.cpp +++ b/wpilibc/Athena/src/TalonSRX.cpp @@ -79,3 +79,8 @@ bool TalonSRX::GetInverted() const { return m_isInverted; } * @param output Write out the PWM value as was found in the PIDController */ void TalonSRX::PIDWrite(float output) { Set(output); } + +/** + * Common interface to stop the motor until Set is called again. + */ +void TalonSRX::StopMotor() { this->SafePWM::StopMotor(); } \ No newline at end of file diff --git a/wpilibc/Athena/src/Victor.cpp b/wpilibc/Athena/src/Victor.cpp index 059a47dca9..81c863c1ea 100644 --- a/wpilibc/Athena/src/Victor.cpp +++ b/wpilibc/Athena/src/Victor.cpp @@ -82,3 +82,8 @@ bool Victor::GetInverted() const { return m_isInverted; } * @param output Write out the PWM value as was found in the PIDController */ void Victor::PIDWrite(float output) { Set(output); } + +/** + * Common interface to stop the motor until Set is called again. + */ +void Victor::StopMotor() { this->SafePWM::StopMotor(); } \ No newline at end of file diff --git a/wpilibc/Athena/src/VictorSP.cpp b/wpilibc/Athena/src/VictorSP.cpp index c09a73de77..57dddea9a2 100644 --- a/wpilibc/Athena/src/VictorSP.cpp +++ b/wpilibc/Athena/src/VictorSP.cpp @@ -86,3 +86,8 @@ void VictorSP::Disable() { SetRaw(kPwmDisabled); } * @param output Write out the PWM value as was found in the PIDController */ void VictorSP::PIDWrite(float output) { Set(output); } + +/** + * Common interface to stop the motor until Set is called again. + */ +void VictorSP::StopMotor() { this->SafePWM::StopMotor(); } \ No newline at end of file diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java index 9b013c8456..cabcaba6fe 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java @@ -375,15 +375,14 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController { byte[] data = new byte[8]; byte dataSize; - if (m_safetyHelper != null) - m_safetyHelper.feed(); - - if (m_stopped) - { - enableControl(); - m_stopped = false; - } - + if (m_safetyHelper != null) + m_safetyHelper.feed(); + + if (m_stopped) { + enableControl(); + m_stopped = false; + } + if (m_controlEnabled) { switch (m_controlMode) { case PercentVbus: @@ -2251,10 +2250,9 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController { * @deprecated Use disableControl instead. */ @Override - @Deprecated public void stopMotor() { disableControl(); - m_stopped = true; + m_stopped = true; } /* diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java index 09bc46f0f1..b60de8676d 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java @@ -424,11 +424,10 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont public void set(double outputValue) { /* feed safety helper since caller just updated our output */ m_safetyHelper.feed(); - if(m_stopped) - { - enableControl(); - m_stopped = false; - } + if(m_stopped) { + enableControl(); + m_stopped = false; + } if (m_controlEnabled) { m_setPoint = outputValue; /* cache set point for getSetpoint() */ switch (m_controlMode) { @@ -1218,10 +1217,9 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont * @deprecated Use disableControl instead. */ @Override - @Deprecated public void stopMotor() { disableControl(); - m_stopped = true; + m_stopped = true; } @Override diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java similarity index 97% rename from wpilibj/src/shared/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java rename to wpilibj/src/athena/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java index 73d7976f42..fb3ba24c5d 100644 --- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java @@ -92,7 +92,7 @@ public class MotorSafetyHelper { if (!m_enabled || RobotState.isDisabled() || RobotState.isTest()) return; if (m_stopTime < Timer.getFPGATimestamp()) { - System.err.println(m_safeObject.getDescription() + "... Output not updated often enough."); + DriverStation.reportError(m_safeObject.getDescription() + "... Output not updated often enough.", false); m_safeObject.stopMotor(); } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java index 5d04b52ece..97338d297d 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java @@ -794,16 +794,16 @@ public class RobotDrive implements MotorSafety { public void stopMotor() { if (m_frontLeftMotor != null) { - m_frontLeftMotor.set(0.0); + m_frontLeftMotor.stopMotor(); } if (m_frontRightMotor != null) { - m_frontRightMotor.set(0.0); + m_frontRightMotor.stopMotor(); } if (m_rearLeftMotor != null) { - m_rearLeftMotor.set(0.0); + m_rearLeftMotor.stopMotor(); } if (m_rearRightMotor != null) { - m_rearRightMotor.set(0.0); + m_rearRightMotor.stopMotor(); } if (m_safetyHelper != null) m_safetyHelper.feed(); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SpeedController.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SpeedController.java index e06ee3fca4..e886590f74 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SpeedController.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SpeedController.java @@ -51,10 +51,14 @@ public interface SpeedController extends PIDOutput { */ boolean getInverted(); - - /** * Disable the speed controller */ void disable(); + + /** + * Stops motor movement. Motor can be moved again by calling set without having + * to re-enable the motor. + */ + void stopMotor(); } diff --git a/wpilibj/src/sim/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java b/wpilibj/src/sim/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java new file mode 100644 index 0000000000..fb3ba24c5d --- /dev/null +++ b/wpilibj/src/sim/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java @@ -0,0 +1,132 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2016. 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.wpilibj.Timer; + +/** + * The MotorSafetyHelper object is constructed for every object that wants to + * implement the Motor Safety protocol. The helper object has the code to + * actually do the timing and call the motors Stop() method when the timeout + * expires. The motor object is expected to call the Feed() method whenever the + * motors value is updated. + * + * @author brad + */ +public class MotorSafetyHelper { + + double m_expiration; + boolean m_enabled; + double m_stopTime; + MotorSafety m_safeObject; + MotorSafetyHelper m_nextHelper; + static MotorSafetyHelper m_headHelper = null; + + /** + * The constructor for a MotorSafetyHelper object. The helper object is + * constructed for every object that wants to implement the Motor Safety + * protocol. The helper object has the code to actually do the timing and call + * the motors Stop() method when the timeout expires. The motor object is + * expected to call the Feed() method whenever the motors value is updated. + * + * @param safeObject a pointer to the motor object implementing MotorSafety. + * This is used to call the Stop() method on the motor. + */ + public MotorSafetyHelper(MotorSafety safeObject) { + m_safeObject = safeObject; + m_enabled = false; + m_expiration = MotorSafety.DEFAULT_SAFETY_EXPIRATION; + m_stopTime = Timer.getFPGATimestamp(); + m_nextHelper = m_headHelper; + m_headHelper = this; + } + + /** + * Feed the motor safety object. Resets the timer on this object that is used + * to do the timeouts. + */ + public void feed() { + m_stopTime = Timer.getFPGATimestamp() + m_expiration; + } + + /** + * Set the expiration time for the corresponding motor safety object. + *$ + * @param expirationTime The timeout value in seconds. + */ + public void setExpiration(double expirationTime) { + m_expiration = expirationTime; + } + + /** + * Retrieve the timeout value for the corresponding motor safety object. + *$ + * @return the timeout value in seconds. + */ + public double getExpiration() { + return m_expiration; + } + + /** + * Determine of the motor is still operating or has timed out. + *$ + * @return a true value if the motor is still operating normally and hasn't + * timed out. + */ + public boolean isAlive() { + return !m_enabled || m_stopTime > Timer.getFPGATimestamp(); + } + + /** + * Check if this motor has exceeded its timeout. This method is called + * periodically to determine if this motor has exceeded its timeout value. If + * it has, the stop method is called, and the motor is shut down until its + * value is updated again. + */ + public void check() { + if (!m_enabled || RobotState.isDisabled() || RobotState.isTest()) + return; + if (m_stopTime < Timer.getFPGATimestamp()) { + DriverStation.reportError(m_safeObject.getDescription() + "... Output not updated often enough.", false); + + m_safeObject.stopMotor(); + } + } + + /** + * Enable/disable motor safety for this device Turn on and off the motor + * safety option for this PWM object. + *$ + * @param enabled True if motor safety is enforced for this object + */ + public void setSafetyEnabled(boolean enabled) { + m_enabled = enabled; + } + + /** + * Return the state of the motor safety enabled flag Return if the motor + * safety is currently enabled for this devicce. + *$ + * @return True if motor safety is enforced for this device + */ + public boolean isSafetyEnabled() { + return m_enabled; + } + + /** + * Check the motors to see if any have timed out. This static method is called + * periodically to poll all the motors and stop any that have timed out. + */ + // TODO: these should be synchronized with the setting methods in case it's + // called from a different thread + public static void checkMotors() { + for (MotorSafetyHelper msh = m_headHelper; msh != null; msh = msh.m_nextHelper) { + msh.check(); + } + } +}