diff --git a/wpilibc/wpilibC++/include/PIDController.h b/wpilibc/wpilibC++/include/PIDController.h index 8bc6c24039..a224632c95 100644 --- a/wpilibc/wpilibC++/include/PIDController.h +++ b/wpilibc/wpilibC++/include/PIDController.h @@ -9,6 +9,7 @@ #include "Controller.h" #include "LiveWindow/LiveWindow.h" #include "HAL/Semaphore.hpp" +#include "PIDInterface.h" class PIDOutput; class PIDSource; @@ -21,8 +22,9 @@ class Notifier; * care of the integral calculations, as well as writing the given * PIDOutput */ -class PIDController : public LiveWindowSendable, public Controller, public ITableListener -{ +class PIDController : public LiveWindowSendable, + public PIDInterface, + public ITableListener { public: PIDController(float p, float i, float d, PIDSource *source, PIDOutput *output, float period = 0.05); @@ -33,15 +35,15 @@ public: virtual void SetContinuous(bool continuous = true); virtual void SetInputRange(float minimumInput, float maximumInput); virtual void SetOutputRange(float minimumOutput, float maximumOutput); - virtual void SetPID(float p, float i, float d); - virtual void SetPID(float p, float i, float d, float f); - virtual float GetP() const; - virtual float GetI() const; - virtual float GetD() const; - virtual float GetF() const; + virtual void SetPID(double p, double i, double d) override; + virtual void SetPID(double p, double i, double d, double f); + virtual double GetP() const override; + virtual double GetI() const override; + virtual double GetD() const override; + virtual double GetF() const; - virtual void SetSetpoint(float setpoint); - virtual float GetSetpoint() const; + virtual void SetSetpoint(float setpoint) override; + virtual double GetSetpoint() const override; virtual float GetError() const; @@ -52,9 +54,9 @@ public: virtual void Enable() override; virtual void Disable() override; - virtual bool IsEnabled() const; + virtual bool IsEnabled() const override; - virtual void Reset(); + virtual void Reset() override; virtual void InitTable(ITable* table); diff --git a/wpilibc/wpilibC++/include/PIDInterface.h b/wpilibc/wpilibC++/include/PIDInterface.h new file mode 100644 index 0000000000..3bde03b590 --- /dev/null +++ b/wpilibc/wpilibC++/include/PIDInterface.h @@ -0,0 +1,22 @@ +#pragma once + +#include "Base.h" +#include "Controller.h" +#include "LiveWindow/LiveWindow.h" +#include "HAL/Semaphore.hpp" + +class PIDInterface : public Controller { + virtual void SetPID(double p, double i, double d)=0; + virtual double GetP() const=0; + virtual double GetI() const=0; + virtual double GetD() const=0; + + virtual void SetSetpoint(float setpoint)=0; + virtual double GetSetpoint() const=0; + + virtual void Enable()=0; + virtual void Disable()=0; + virtual bool IsEnabled() const=0; + + virtual void Reset()=0; +}; diff --git a/wpilibc/wpilibC++/include/PIDSource.h b/wpilibc/wpilibC++/include/PIDSource.h index 8880f1bd35..de7a228f6d 100644 --- a/wpilibc/wpilibC++/include/PIDSource.h +++ b/wpilibc/wpilibC++/include/PIDSource.h @@ -11,7 +11,7 @@ * returns a standard value that will be used in the PID code. */ class PIDSource -{ +{ public: enum PIDSourceParameter {kDistance, kRate, kAngle}; virtual double PIDGet() const = 0; diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h index 38590711f7..520e1b676d 100644 --- a/wpilibc/wpilibC++Devices/include/CANTalon.h +++ b/wpilibc/wpilibC++Devices/include/CANTalon.h @@ -8,6 +8,8 @@ #include "SafePWM.h" #include "CANSpeedController.h" #include "PIDOutput.h" +#include "PIDSource.h" +#include "PIDInterface.h" #include "MotorSafetyHelper.h" #include "LiveWindow/LiveWindowSendable.h" #include "tables/ITable.h" @@ -21,7 +23,9 @@ class CANTalon : public MotorSafety, public CANSpeedController, public ErrorBase, public LiveWindowSendable, - public ITableListener + public ITableListener, + public PIDSource, + public PIDInterface { public: enum FeedbackDevice { @@ -41,9 +45,12 @@ public: explicit CANTalon(int deviceNumber,int controlPeriodMs); virtual ~CANTalon(); - // PIDController interface + // PIDOutput interface virtual void PIDWrite(float output) override; + // PIDSource interface + virtual double PIDGet() const override; + // MotorSafety interface virtual void SetExpiration(float timeout) override; virtual float GetExpiration() const override; @@ -56,19 +63,22 @@ public: // CANSpeedController interface virtual float Get() const override; virtual void Set(float value, uint8_t syncGroup=0) override; + virtual void Reset() override; + virtual void SetSetpoint(float value) override; virtual void Disable() override; virtual void EnableControl(); + virtual void Enable(); virtual void SetP(double p) override; virtual void SetI(double i) override; virtual void SetD(double d) override; void SetF(double f); void SetIzone(unsigned iz); virtual void SetPID(double p, double i, double d) override; - void SetPID(double p, double i, double d, double f); + virtual void SetPID(double p, double i, double d, double f); virtual double GetP() const override; virtual double GetI() const override; virtual double GetD() const override; - double GetF() const; + virtual double GetF() const; virtual float GetBusVoltage() const override; virtual float GetOutputVoltage() const override; virtual float GetOutputCurrent() const override; @@ -141,7 +151,8 @@ public: int GetBrakeEnableDuringNeutral() const; bool IsControlEnabled() const; - double GetSetpoint() const; + bool IsEnabled() const override; + double GetSetpoint() const override; // LiveWindow stuff. void ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) override; @@ -151,6 +162,7 @@ public: std::string GetSmartDashboardType() const override; void InitTable(ITable *subTable) override; ITable * GetTable() const override; + private: // Values for various modes as is sent in the CAN packets for the Talon. enum TalonControlMode { diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp index f703441748..575a262762 100644 --- a/wpilibc/wpilibC++Devices/src/CANTalon.cpp +++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp @@ -66,16 +66,25 @@ void CANTalon::PIDWrite(float output) } } - /** - * Gets the current status of the Talon (usually a sensor value). - * - * In Current mode: returns output current. - * In Speed mode: returns current speed. - * In Position mode: returns current sensor position. - * In PercentVbus and Follower modes: returns current applied throttle. - * - * @return The current sensor value of the Talon. - */ +/** + * Retrieve the current sensor value. Equivalent to Get(). + * + * @return The current sensor value of the Talon. + */ +double CANTalon::PIDGet() const { + return Get(); +} + +/** + * Gets the current status of the Talon (usually a sensor value). + * + * In Current mode: returns output current. + * In Speed mode: returns current speed. + * In Position mode: returns current sensor position. + * In PercentVbus and Follower modes: returns current applied throttle. + * + * @return The current sensor value of the Talon. + */ float CANTalon::Get() const { int value; @@ -165,7 +174,24 @@ void CANTalon::Set(float value, uint8_t syncGroup) } /** - * TODO documentation (see CANJaguar.cpp) + * Sets the setpoint to value. Equivalent to Set(). + */ +void CANTalon::SetSetpoint(float value) { + Set(value); +} + +/** + * Resets the integral term and disables the controller. + */ +void CANTalon::Reset() { + ClearIaccum(); + Disable(); +} + +/** + * Disables control of the talon, causing the motor to brake or coast + * depending on its mode (see the Talon SRX Software Reference manual + * for more information). */ void CANTalon::Disable() { @@ -174,13 +200,20 @@ void CANTalon::Disable() } /** - * TODO documentation (see CANJaguar.cpp) + * Enables control of the Talon, allowing the motor to move. */ void CANTalon::EnableControl() { SetControlMode(m_controlMode); m_controlEnabled = true; } +/** + * Enables control of the Talon, allowing the motor to move. + */ +void CANTalon::Enable() { + EnableControl(); +} + /** * @return Whether the Talon is currently enabled. */ @@ -188,6 +221,13 @@ bool CANTalon::IsControlEnabled() const { return m_controlEnabled; } +/** + * @return Whether the Talon is currently enabled. + */ +bool CANTalon::IsEnabled() const { + return IsControlEnabled(); +} + /** * @param p Proportional constant to use in PID loop. * @see SelectProfileSlot to choose between the two sets of gains. diff --git a/wpilibc/wpilibC++Devices/src/PIDController.cpp b/wpilibc/wpilibC++Devices/src/PIDController.cpp index cd1e0cdff6..e051d05964 100644 --- a/wpilibc/wpilibC++Devices/src/PIDController.cpp +++ b/wpilibc/wpilibC++Devices/src/PIDController.cpp @@ -63,7 +63,7 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, float period) { m_table = NULL; - + m_semaphore = initializeMutexNormal(); m_controlLoop = new Notifier(PIDController::CallCalculate, this); @@ -209,7 +209,7 @@ void PIDController::Calculate() * @param i Integral coefficient * @param d Differential coefficient */ -void PIDController::SetPID(float p, float i, float d) +void PIDController::SetPID(double p, double i, double d) { CRITICAL_REGION(m_semaphore) { @@ -234,7 +234,7 @@ void PIDController::SetPID(float p, float i, float d) * @param d Differential coefficient * @param f Feed forward coefficient */ -void PIDController::SetPID(float p, float i, float d, float f) +void PIDController::SetPID(double p, double i, double d, double f) { CRITICAL_REGION(m_semaphore) { @@ -257,7 +257,7 @@ void PIDController::SetPID(float p, float i, float d, float f) * Get the Proportional coefficient * @return proportional coefficient */ -float PIDController::GetP() const +double PIDController::GetP() const { CRITICAL_REGION(m_semaphore) { @@ -270,7 +270,7 @@ float PIDController::GetP() const * Get the Integral coefficient * @return integral coefficient */ -float PIDController::GetI() const +double PIDController::GetI() const { CRITICAL_REGION(m_semaphore) { @@ -283,7 +283,7 @@ float PIDController::GetI() const * Get the Differential coefficient * @return differential coefficient */ -float PIDController::GetD() const +double PIDController::GetD() const { CRITICAL_REGION(m_semaphore) { @@ -296,7 +296,7 @@ float PIDController::GetD() const * Get the Feed forward coefficient * @return Feed forward coefficient */ -float PIDController::GetF() const +double PIDController::GetF() const { CRITICAL_REGION(m_semaphore) { @@ -348,7 +348,7 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput) CRITICAL_REGION(m_semaphore) { m_minimumInput = minimumInput; - m_maximumInput = maximumInput; + m_maximumInput = maximumInput; } END_REGION; @@ -393,8 +393,8 @@ void PIDController::SetSetpoint(float setpoint) m_setpoint = setpoint; } } - END_REGION; - + END_REGION; + if (m_table != NULL) { m_table->PutNumber("setpoint", m_setpoint); } @@ -404,7 +404,7 @@ void PIDController::SetSetpoint(float setpoint) * Returns the current setpoint of the PIDController * @return the current setpoint */ -float PIDController::GetSetpoint() const +double PIDController::GetSetpoint() const { float setpoint; CRITICAL_REGION(m_semaphore) @@ -512,11 +512,11 @@ bool PIDController::OnTarget() const void PIDController::Enable() { CRITICAL_REGION(m_semaphore) - { + { m_enabled = true; } - END_REGION; - + END_REGION; + if (m_table != NULL) { m_table->PutBoolean("enabled", true); } @@ -533,7 +533,7 @@ void PIDController::Disable() m_enabled = false; } END_REGION; - + if (m_table != NULL) { m_table->PutBoolean("enabled", false); } diff --git a/wpilibc/wpilibC++Sim/src/PIDController.cpp b/wpilibc/wpilibC++Sim/src/PIDController.cpp index d3e3ade778..97f5b233b9 100644 --- a/wpilibc/wpilibC++Sim/src/PIDController.cpp +++ b/wpilibc/wpilibC++Sim/src/PIDController.cpp @@ -66,7 +66,7 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, m_I = Ki; m_D = Kd; m_F = Kf; - + m_maximumOutput = 1.0; m_minimumOutput = -1.0; @@ -87,14 +87,14 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, m_pidOutput = output; m_period = period; - + m_semaphore = initializeMutexRecursive(); m_controlLoop = new Notifier(PIDController::CallCalculate, this); m_controlLoop->StartPeriodic(m_period); static int32_t instances = 0; instances++; - + m_toleranceType = kNoTolerance; } @@ -125,7 +125,7 @@ void PIDController::CallCalculate(void *controller) * Read the input, calculate the output accordingly, and write to the output. * This should only be called by the Notifier indirectly through CallCalculate * and is created during initialization. - */ + */ void PIDController::Calculate() { bool enabled; @@ -163,7 +163,7 @@ void PIDController::Calculate() } } } - + if(m_I != 0) { double potentialIGain = (m_totalError + m_error) * m_I; @@ -201,7 +201,7 @@ void PIDController::Calculate() * @param i Integral coefficient * @param d Differential coefficient */ -void PIDController::SetPID(float p, float i, float d) +void PIDController::SetPID(double p, double i, double d) { CRITICAL_REGION(m_semaphore) { @@ -226,7 +226,7 @@ void PIDController::SetPID(float p, float i, float d) * @param d Differential coefficient * @param f Feed forward coefficient */ -void PIDController::SetPID(float p, float i, float d, float f) +void PIDController::SetPID(double p, double i, double d, double f) { CRITICAL_REGION(m_semaphore) { @@ -249,7 +249,7 @@ void PIDController::SetPID(float p, float i, float d, float f) * Get the Proportional coefficient * @return proportional coefficient */ -float PIDController::GetP() const +double PIDController::GetP() const { CRITICAL_REGION(m_semaphore) { @@ -262,7 +262,7 @@ float PIDController::GetP() const * Get the Integral coefficient * @return integral coefficient */ -float PIDController::GetI() const +double PIDController::GetI() const { CRITICAL_REGION(m_semaphore) { @@ -275,7 +275,7 @@ float PIDController::GetI() const * Get the Differential coefficient * @return differential coefficient */ -float PIDController::GetD() const +double PIDController::GetD() const { CRITICAL_REGION(m_semaphore) { @@ -288,7 +288,7 @@ float PIDController::GetD() const * Get the Feed forward coefficient * @return Feed forward coefficient */ -float PIDController::GetF() const +double PIDController::GetF() const { CRITICAL_REGION(m_semaphore) { @@ -332,7 +332,7 @@ void PIDController::SetContinuous(bool continuous) /** * Sets the maximum and minimum values expected from the input. - * + * * @param minimumInput the minimum value expected from the input * @param maximumInput the maximum value expected from the output */ @@ -341,7 +341,7 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput) CRITICAL_REGION(m_semaphore) { m_minimumInput = minimumInput; - m_maximumInput = maximumInput; + m_maximumInput = maximumInput; } END_REGION; @@ -350,7 +350,7 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput) /** * Sets the minimum and maximum values to write. - * + * * @param minimumOutput the minimum value to write to the output * @param maximumOutput the maximum value to write to the output */ @@ -386,8 +386,8 @@ void PIDController::SetSetpoint(float setpoint) m_setpoint = setpoint; } } - END_REGION; - + END_REGION; + if (m_table != NULL) { m_table->PutNumber("setpoint", m_setpoint); } @@ -397,7 +397,7 @@ void PIDController::SetSetpoint(float setpoint) * Returns the current setpoint of the PIDController * @return the current setpoint */ -float PIDController::GetSetpoint() const +double PIDController::GetSetpoint() const { float setpoint; CRITICAL_REGION(m_semaphore) @@ -501,11 +501,11 @@ bool PIDController::OnTarget() const void PIDController::Enable() { CRITICAL_REGION(m_semaphore) - { + { m_enabled = true; } - END_REGION; - + END_REGION; + if (m_table != NULL) { m_table->PutBoolean("enabled", true); } @@ -522,7 +522,7 @@ void PIDController::Disable() m_enabled = false; } END_REGION; - + if (m_table != NULL) { m_table->PutBoolean("enabled", false); } @@ -598,7 +598,7 @@ void PIDController::ValueChanged(ITable* source, const std::string& key, EntryVa } void PIDController::UpdateTable() { - + } void PIDController::StartLiveWindowMode() { @@ -606,5 +606,5 @@ void PIDController::StartLiveWindowMode() { } void PIDController::StopLiveWindowMode() { - + } diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java index 96c65cd52d..f84211ed6d 100644 --- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java +++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java @@ -1,4 +1,3 @@ -/*----------------------------------------------------------------------------*/ /* Copyright (c) FIRST 2008-2012. 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 */ @@ -20,7 +19,7 @@ import edu.wpi.first.wpilibj.util.BoundaryException; * care of the integral calculations, as well as writing the given * PIDOutput */ -public class PIDController implements LiveWindowSendable, Controller { +public class PIDController implements PIDInterface, LiveWindowSendable, Controller { public static final double kDefaultPeriod = .05; private static int instances = 0; @@ -47,15 +46,27 @@ public class PIDController implements LiveWindowSendable, Controller { private boolean m_freed = false; private boolean m_usingPercentTolerance; - /** - * Tolerance is the type of tolerance used to specify if the PID controller is on target. - * The various implementations of this class such as PercentageTolerance and AbsoluteTolerance - * specify types of tolerance specifications to use. - */ + /** + * Tolerance is the type of tolerance used to specify if the PID controller + * is on target. + * + * The various implementations of this class such as PercentageTolerance and + * AbsoluteTolerance specify types of tolerance specifications to use. + */ public interface Tolerance { public boolean onTarget(); } + /** + * Used internally for when Tolerance hasn't been set. + */ + public class NullTolerance implements Tolerance { + @Override + public boolean onTarget() { + throw new RuntimeException("No tolerance value set when calling onTarget()."); + } + } + public class PercentageTolerance implements Tolerance { double percentage; @@ -64,7 +75,7 @@ public class PIDController implements LiveWindowSendable, Controller { } @Override - public boolean onTarget() { + public boolean onTarget() { return (Math.abs(getError()) < percentage / 100 * (m_maximumInput - m_minimumInput)); } @@ -78,19 +89,11 @@ public class PIDController implements LiveWindowSendable, Controller { } @Override - public boolean onTarget() { + public boolean onTarget() { return Math.abs(getError()) < value; } } - public class NullTolerance implements Tolerance { - - @Override - public boolean onTarget() { - throw new RuntimeException("No tolerance value set when using PIDController.onTarget()"); - } - } - private class PIDTask extends TimerTask { private PIDController m_controller; @@ -464,7 +467,7 @@ public class PIDController implements LiveWindowSendable, Controller { * @param tolerance a tolerance object of the right type, e.g. PercentTolerance * or AbsoluteTolerance */ - private synchronized void setTolerance(Tolerance tolerance) { + public void setTolerance(Tolerance tolerance) { m_tolerance = tolerance; } @@ -523,9 +526,19 @@ public class PIDController implements LiveWindowSendable, Controller { /** * Return true if PIDController is enabled. + * + * @deprecated Call {@link #isEnabled()} instead. */ + @Deprecated public synchronized boolean isEnable() { - return m_enabled; + return isEnabled(); + } + + /** + * Return true if PIDController is enabled. + */ + public boolean isEnabled() { + return m_enabled; } /** diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java new file mode 100644 index 0000000000..ab8b334ce0 --- /dev/null +++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java @@ -0,0 +1,16 @@ +package edu.wpi.first.wpilibj; + +public interface PIDInterface extends Controller { + + public void setPID(double p, double i, double d); + public double getP(); + public double getI(); + public double getD(); + public void setSetpoint(double setpoint); + public double getSetpoint(); + public double getError(); + public void enable(); + public void disable(); + public boolean isEnabled(); + public void reset(); +} diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java index 22a1ff3862..a378400e64 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java @@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj.util.CheckedAllocationException; * Texas Instruments Jaguar Speed Controller as a CAN device. * @author Thomas Clark */ -public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController, LiveWindowSendable { +public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeedController, LiveWindowSendable { public static final int kMaxMessageDataSize = 8; @@ -304,6 +304,27 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController, Li return m_value; } + /** + * Equivalent to {@link #get()}. + */ + @Override + public double getSetpoint() { + return get(); + } + + /** + * Get the difference between the setpoint and goal in closed loop modes. + * + * Outside of position and velocity modes the return value of getError() + * has relatively little meaning. + * + * @return The difference between the setpoint and the current position. + */ + @Override + public double getError() { + return get() - getPosition(); + } + /** * Sets the output set-point value. * @@ -387,6 +408,20 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController, Li set(value, (byte)0); } + /** + * Equivalent to {@link #set(double)}. Implements PIDInterface. + */ + @Override + public void setSetpoint(double value) { + set(value); + } + + @Override + public void reset() { + set(m_value); + disableControl(); + } + /** * Check all unverified params and make sure they're equal to their local * cached versions. If a value isn't available, it gets requested. If a value @@ -834,6 +869,11 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController, Li disableControl(); } + // PIDInterface interface + public void enable() { + enableControl(); + } + // PIDOutput interface @Override public void pidWrite(double output) { @@ -1061,6 +1101,15 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController, Li enableControl(0.0); } + /** + * Return whether the controller is enabled. + * + * @returns true if enabled. + */ + public boolean isEnabled() { + return m_controlEnabled; + } + /** * Disable the closed loop controller. * 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 6f8a0f5bd7..e2d0ff0745 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 @@ -1,11 +1,3 @@ -/* ---------------------------------------------------------------------------- - * This file was automatically generated by SWIG (http://www.swig.org). - * Version 2.0.11 - * - * Do not make changes to this file unless you know what you are doing--modify - * the SWIG interface file instead. - * ----------------------------------------------------------------------------- */ - package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.CanTalonSRX; @@ -20,7 +12,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; import edu.wpi.first.wpilibj.tables.ITable; import edu.wpi.first.wpilibj.tables.ITableListener; -public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedController, LiveWindowSendable { +public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface, CANSpeedController, LiveWindowSendable { private MotorSafetyHelper m_safetyHelper; public enum TalonControlMode { PercentVbus(0), Follower(5), Voltage(4), Position(1), Speed(2), Current(3), Disabled(15); @@ -79,8 +71,10 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont private CanTalonSRX m_impl; - TalonControlMode m_controlMode; + private TalonControlMode m_controlMode; private static double kDelayForSolicitedSignals = 0.004; + private double m_minimumInput; + private double m_maximumInput; int m_deviceNumber; boolean m_controlEnabled; @@ -98,6 +92,7 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont setProfile(m_profile); applyControlMode(TalonControlMode.PercentVbus); } + public CANTalon(int deviceNumber,int controlPeriodMs) { m_deviceNumber = deviceNumber; m_impl = new CanTalonSRX(deviceNumber,controlPeriodMs); /* bound period to be within [1 ms,95 ms] */ @@ -183,6 +178,40 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont set(outputValue); } + /** + * Resets the accumulated integral error and disables the controller. + * + * The only difference between this and {@link PIDController#reset} is that + * the PIDController also resets the previous error for the D term, but the + * difference should have minimal effect as it will only last one cycle. + */ + public void reset() { + disable(); + clearIAccum(); + } + + /** + * Return true if Talon is enabled. + * + * @return true if the Talon is enabled and may be applying power to the motor + */ + public boolean isEnabled() { + return isControlEnabled(); + } + + /** + * Returns the difference between the setpoint and the current position. + * + * @return The error in units corresponding to whichever mode we are in. + * @see #set(double) set() for a detailed description of the various units. + */ + public double getError() {return getClosedLoopError();} + + /** + * Calls {@link #set(double)}. + */ + public void setSetpoint(double setpoint) { set(setpoint); } + /** * Flips the sign (multiplies by negative one) the sensor values going into *the talon. @@ -487,6 +516,9 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont changeControlMode(m_controlMode); m_controlEnabled = true; } + public void enable() { + enableControl(); + } public void disableControl() { m_impl.SetModeSelect(TalonControlMode.Disabled.value); @@ -652,13 +684,6 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont return CanTalonJNI.intp_value(fp); } - /** - * Clear the accumulator for I gain. - */ - public void ClearIaccum() - { - SWIGTYPE_p_CTR_Code status = m_impl.SetParam(CanTalonSRX.param_t.ePidIaccum, 0); - } /** * Set the proportional value of the currently selected profile. * @@ -743,6 +768,14 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont m_impl.SetRampThrottle(rate); } + /** + * Clear the accumulator for I gain. + */ + public void ClearIaccum() + { + SWIGTYPE_p_CTR_Code status = m_impl.SetParam(CanTalonSRX.param_t.ePidIaccum, 0); + } + /** * Sets control values for closed loop control. * @@ -816,6 +849,9 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont } // TODO: Documentation for all these accessors/setters for misc. stuff. + public void clearIAccum() { + SWIGTYPE_p_CTR_Code status = m_impl.SetParam(CanTalonSRX.param_t.ePidIaccum, 0); + } public void setForwardSoftLimit(int forwardLimit) { m_impl.SetForwardSoftLimit(forwardLimit); }