From 1316deaf5b0f16a3f190f22f93f6a11d3dc111fb Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Mon, 27 Jul 2015 17:17:31 -0400 Subject: [PATCH] Added LiveWindow support for PID control for CAN speed controllers. Change-Id: Id32e27ee7074ffa23824d5d8c0e9509059001284 --- wpilibc/wpilibC++Devices/include/CANJaguar.h | 1 + .../include/CANSpeedController.h | 16 +++ wpilibc/wpilibC++Devices/include/CANTalon.h | 1 + wpilibc/wpilibC++Devices/src/CANJaguar.cpp | 74 ++++++---- wpilibc/wpilibC++Devices/src/CANTalon.cpp | 31 +++- wpilibj/wpilibJava/pom.xml | 4 +- wpilibj/wpilibJavaDevices/pom.xml | 4 +- .../java/edu/wpi/first/wpilibj/CANJaguar.java | 98 ++++++------- .../wpi/first/wpilibj/CANSpeedController.java | 136 +++++++++++++----- .../java/edu/wpi/first/wpilibj/CANTalon.java | 46 +++--- 10 files changed, 277 insertions(+), 134 deletions(-) diff --git a/wpilibc/wpilibC++Devices/include/CANJaguar.h b/wpilibc/wpilibC++Devices/include/CANJaguar.h index 555557d338..2c451bd303 100644 --- a/wpilibc/wpilibC++Devices/include/CANJaguar.h +++ b/wpilibc/wpilibC++Devices/include/CANJaguar.h @@ -101,6 +101,7 @@ class CANJaguar : public MotorSafety, virtual double GetP() const override; virtual double GetI() const override; virtual double GetD() const override; + virtual bool IsModePID(CANSpeedController::ControlMode mode) const override; virtual float GetBusVoltage() const override; virtual float GetOutputVoltage() const override; virtual float GetOutputCurrent() const override; diff --git a/wpilibc/wpilibC++Devices/include/CANSpeedController.h b/wpilibc/wpilibC++Devices/include/CANSpeedController.h index 376aaad754..73ed379417 100644 --- a/wpilibc/wpilibC++Devices/include/CANSpeedController.h +++ b/wpilibc/wpilibC++Devices/include/CANSpeedController.h @@ -23,6 +23,22 @@ class CANSpeedController : public SpeedController { kFollower = 5 // Not supported in Jaguar. }; + // Helper function for the ControlMode enum + std::string GetModeName(ControlMode mode) { + switch(mode) { + case kPercentVbus: return "PercentVbus"; + case kCurrent: return "Current"; + case kSpeed: return "Speed"; + case kPosition: return "Position"; + case kVoltage: return "Voltage"; + case kFollower: return "Follower"; + default: return "[unknown control mode]"; + } + } + + // Helper function for the ControlMode enum + virtual bool IsModePID(ControlMode mode) const = 0; + enum Faults { kCurrentFault = 1, kTemperatureFault = 2, diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h index 3263174958..498d067709 100644 --- a/wpilibc/wpilibC++Devices/include/CANTalon.h +++ b/wpilibc/wpilibC++Devices/include/CANTalon.h @@ -80,6 +80,7 @@ class CANTalon : public MotorSafety, virtual double GetI() const override; virtual double GetD() const override; virtual double GetF() const; + virtual bool IsModePID(CANSpeedController::ControlMode mode) const override; virtual float GetBusVoltage() const override; virtual float GetOutputVoltage() const override; virtual float GetOutputCurrent() const override; diff --git a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp index ff0a53cb05..27a8a6066f 100644 --- a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp +++ b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp @@ -1938,29 +1938,6 @@ uint8_t CANJaguar::GetDeviceID() const { return m_deviceNumber; } */ void CANJaguar::StopMotor() { DisableControl(); } -void CANJaguar::ValueChanged(ITable* source, llvm::StringRef key, - std::shared_ptr value, bool isNew) { - if (value->type() == NT_DOUBLE) Set(value->GetDouble()); -} - -void CANJaguar::UpdateTable() { - if (m_table != nullptr) { - m_table->PutNumber("Value", Get()); - } -} - -void CANJaguar::StartLiveWindowMode() { - if (m_table != nullptr) { - m_table->AddTableListener("Value", this, true); - } -} - -void CANJaguar::StopLiveWindowMode() { - if (m_table != nullptr) { - m_table->RemoveTableListener(this); - } -} - /** * Common interface for inverting direction of a speed controller. * Only works in PercentVbus, speed, and Voltage modes. @@ -1976,8 +1953,57 @@ void CANJaguar::SetInverted(bool isInverted) { m_isInverted = isInverted; } */ bool CANJaguar::GetInverted() const { return m_isInverted; } +void CANJaguar::ValueChanged(ITable* source, llvm::StringRef key, + std::shared_ptr value, bool isNew) { + if(key == "Mode" && value->IsDouble()) SetControlMode(static_cast(value->GetDouble())); + if(IsModePID(m_controlMode) && value->IsDouble()) { + if(key == "p") SetP(value->GetDouble()); + if(key == "i") SetI(value->GetDouble()); + if(key == "d") SetD(value->GetDouble()); + } + if(key == "Enabled" && value->IsBoolean()) { + if (value->GetBoolean()) { + EnableControl(); + } else { + DisableControl(); + } + } + if(key == "Value" && value->IsDouble()) Set(value->GetDouble()); +} + +bool CANJaguar::IsModePID(CANSpeedController::ControlMode mode) const { + return mode == kCurrent || mode == kSpeed || mode == kPosition; +} + +void CANJaguar::UpdateTable() { + if (m_table != nullptr) { + m_table->PutString("~TYPE~", "CANSpeedController"); + m_table->PutString("Type", "CANJaguar"); + m_table->PutString("Mode", GetModeName(m_controlMode)); + if (IsModePID(m_controlMode)) { + m_table->PutNumber("p", GetP()); + m_table->PutNumber("i", GetI()); + m_table->PutNumber("d", GetD()); + } + m_table->PutBoolean("Enabled", m_controlEnabled); + m_table->PutNumber("Value", Get()); + } +} + +void CANJaguar::StartLiveWindowMode() { + if (m_table != nullptr) { + m_table->AddTableListener(this, true); + } +} + +void CANJaguar::StopLiveWindowMode() { + if (m_table != nullptr) { + m_table->RemoveTableListener(this); + } +} + std::string CANJaguar::GetSmartDashboardType() const { - return "Speed Controller"; + return "CANSpeedController"; } void CANJaguar::InitTable(std::shared_ptr subTable) { diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp index 509d7a9e49..466a8b1d93 100644 --- a/wpilibc/wpilibC++Devices/src/CANTalon.cpp +++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp @@ -1278,19 +1278,42 @@ void CANTalon::StopMotor() { Disable(); } void CANTalon::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) { - if (!value->IsDouble()) return; - Set(value->GetDouble()); + if(key == "Mode" && value->IsDouble()) SetControlMode(static_cast(value->GetDouble())); + if(key == "p" && value->IsDouble()) SetP(value->GetDouble()); + if(key == "i" && value->IsDouble()) SetI(value->GetDouble()); + if(key == "d" && value->IsDouble()) SetD(value->GetDouble()); + if(key == "f" && value->IsDouble()) SetF(value->GetDouble()); + if(key == "Enabled" && value->IsBoolean()) { + if (value->GetBoolean()) { + Enable(); + } else { + Disable(); + } + } + if(key == "Value" && value->IsDouble()) Set(value->GetDouble()); +} + +bool CANTalon::IsModePID(CANSpeedController::ControlMode mode) const { + return mode == kCurrent || mode == kSpeed || mode == kPosition; } void CANTalon::UpdateTable() { if (m_table != nullptr) { + m_table->PutString("~TYPE~", "CANSpeedController"); + m_table->PutString("Type", "CANTalon"); + m_table->PutString("Mode", GetModeName(m_controlMode)); + m_table->PutNumber("p", GetP()); + m_table->PutNumber("i", GetI()); + m_table->PutNumber("d", GetD()); + m_table->PutNumber("f", GetF()); + m_table->PutBoolean("Enabled", IsControlEnabled()); m_table->PutNumber("Value", Get()); } } void CANTalon::StartLiveWindowMode() { if (m_table != nullptr) { - m_table->AddTableListener("Value", this, true); + m_table->AddTableListener(this, true); } } @@ -1301,7 +1324,7 @@ void CANTalon::StopLiveWindowMode() { } std::string CANTalon::GetSmartDashboardType() const { - return "Speed Controller"; + return "CANSpeedController"; } void CANTalon::InitTable(std::shared_ptr subTable) { diff --git a/wpilibj/wpilibJava/pom.xml b/wpilibj/wpilibJava/pom.xml index de8ced712f..bbe7aa8bdc 100644 --- a/wpilibj/wpilibJava/pom.xml +++ b/wpilibj/wpilibJava/pom.xml @@ -55,8 +55,8 @@ maven-compiler-plugin 3.1 - 1.7 - 1.7 + 1.8 + 1.8 diff --git a/wpilibj/wpilibJavaDevices/pom.xml b/wpilibj/wpilibJavaDevices/pom.xml index 1c3b849327..61cf8c8839 100644 --- a/wpilibj/wpilibJavaDevices/pom.xml +++ b/wpilibj/wpilibJavaDevices/pom.xml @@ -60,8 +60,8 @@ maven-compiler-plugin 3.1 - 1.7 - 1.7 + 1.8 + 1.8 edu/wpi/first/wpilibj/camera/ 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 7812e0a242..f9818dd52c 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 @@ -13,7 +13,6 @@ import java.nio.ByteOrder; import edu.wpi.first.wpilibj.can.CANExceptionFactory; import edu.wpi.first.wpilibj.can.CANJNI; import edu.wpi.first.wpilibj.can.CANMessageNotFoundException; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; import edu.wpi.first.wpilibj.tables.ITable; import edu.wpi.first.wpilibj.tables.ITableListener; import edu.wpi.first.wpilibj.util.AllocationException; @@ -24,8 +23,7 @@ import edu.wpi.first.wpilibj.util.CheckedAllocationException; *$ * @author Thomas Clark */ -public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeedController, - LiveWindowSendable { +public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController { public static final int kMaxMessageDataSize = 8; @@ -73,8 +71,19 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed /** * Mode determines how the Jaguar is controlled, used internally. */ - public enum ControlMode { + public enum JaguarControlMode implements CANSpeedController.ControlMode { PercentVbus, Current, Speed, Position, Voltage; + + @Override + public boolean isPID() { + return this == Current || this == Speed || this == Position; + } + + @Override + public int getValue() { + return ordinal(); + } + } public static final int kCurrentFault = 1; @@ -191,7 +200,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed } m_deviceNumber = (byte) deviceNumber; - m_controlMode = ControlMode.PercentVbus; + m_controlMode = JaguarControlMode.PercentVbus; m_safetyHelper = new MotorSafetyHelper(this); @@ -503,7 +512,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed m_maxOutputVoltageVerified = false; m_faultTimeVerified = false; - if (m_controlMode == ControlMode.PercentVbus || m_controlMode == ControlMode.Voltage) { + if (m_controlMode == JaguarControlMode.PercentVbus || m_controlMode == JaguarControlMode.Voltage) { m_voltageRampRateVerified = false; } else { m_pVerified = false; @@ -545,7 +554,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed try { getMessage(CANJNI.LM_API_STATUS_CMODE, CANJNI.CAN_MSGID_FULL_M, data); - ControlMode mode = ControlMode.values()[data[0]]; + JaguarControlMode mode = JaguarControlMode.values()[data[0]]; if (m_controlMode == mode) { m_controlModeVerified = true; @@ -837,7 +846,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed } if (!m_voltageRampRateVerified) { - if (m_controlMode == ControlMode.PercentVbus) { + if (m_controlMode == JaguarControlMode.PercentVbus) { try { getMessage(CANJNI.LM_API_VOLT_SET_RAMP, CANJNI.CAN_MSGID_FULL_M, data); @@ -855,7 +864,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed requestMessage(CANJNI.LM_API_VOLT_SET_RAMP); } } - } else if (m_controlMode == ControlMode.Voltage) { + } else if (m_controlMode == JaguarControlMode.Voltage) { try { getMessage(CANJNI.LM_API_VCOMP_COMP_RAMP, CANJNI.CAN_MSGID_FULL_M, data); @@ -922,7 +931,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed // PIDOutput interface @Override public void pidWrite(double output) { - if (m_controlMode == ControlMode.PercentVbus) { + if (m_controlMode == JaguarControlMode.PercentVbus) { set(output); } else { throw new IllegalStateException("PID only supported in PercentVbus mode"); @@ -1070,7 +1079,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @return The proportional gain. */ public double getP() { - if (m_controlMode.equals(ControlMode.PercentVbus) || m_controlMode.equals(ControlMode.Voltage)) { + if (m_controlMode.equals(JaguarControlMode.PercentVbus) || m_controlMode.equals(JaguarControlMode.Voltage)) { throw new IllegalStateException("PID does not apply in Percent or Voltage control modes"); } return m_p; @@ -1082,7 +1091,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @return The integral gain. */ public double getI() { - if (m_controlMode.equals(ControlMode.PercentVbus) || m_controlMode.equals(ControlMode.Voltage)) { + if (m_controlMode.equals(JaguarControlMode.PercentVbus) || m_controlMode.equals(JaguarControlMode.Voltage)) { throw new IllegalStateException("PID does not apply in Percent or Voltage control modes"); } return m_i; @@ -1094,7 +1103,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @return The derivative gain. */ public double getD() { - if (m_controlMode.equals(ControlMode.PercentVbus) || m_controlMode.equals(ControlMode.Voltage)) { + if (m_controlMode.equals(JaguarControlMode.PercentVbus) || m_controlMode.equals(JaguarControlMode.Voltage)) { throw new IllegalStateException("PID does not apply in Percent or Voltage control modes"); } return m_d; @@ -1189,7 +1198,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * {@link CANJaguar#enableControl(double)} to enable the device. */ public void setPercentMode() { - changeControlMode(ControlMode.PercentVbus); + changeControlMode(JaguarControlMode.PercentVbus); setPositionReference(CANJNI.LM_REF_NONE); setSpeedReference(CANJNI.LM_REF_NONE); } @@ -1204,7 +1213,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param codesPerRev The counts per revolution on the encoder */ public void setPercentMode(EncoderTag tag, int codesPerRev) { - changeControlMode(ControlMode.PercentVbus); + changeControlMode(JaguarControlMode.PercentVbus); setPositionReference(CANJNI.LM_REF_NONE); setSpeedReference(CANJNI.LM_REF_ENCODER); configEncoderCodesPerRev(codesPerRev); @@ -1220,7 +1229,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param codesPerRev The counts per revolution on the encoder */ public void setPercentMode(QuadEncoderTag tag, int codesPerRev) { - changeControlMode(ControlMode.PercentVbus); + changeControlMode(JaguarControlMode.PercentVbus); setPositionReference(CANJNI.LM_REF_ENCODER); setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); configEncoderCodesPerRev(codesPerRev); @@ -1235,7 +1244,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param tag The constant {@link CANJaguar#kPotentiometer} */ public void setPercentMode(PotentiometerTag tag) { - changeControlMode(ControlMode.PercentVbus); + changeControlMode(JaguarControlMode.PercentVbus); setPositionReference(CANJNI.LM_REF_POT); setSpeedReference(CANJNI.LM_REF_NONE); configPotentiometerTurns(1); @@ -1251,7 +1260,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param d The differential gain of the Jaguar's PID controller. */ public void setCurrentMode(double p, double i, double d) { - changeControlMode(ControlMode.Current); + changeControlMode(JaguarControlMode.Current); setPositionReference(CANJNI.LM_REF_NONE); setSpeedReference(CANJNI.LM_REF_NONE); setPID(p, i, d); @@ -1269,7 +1278,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param d The differential gain of the Jaguar's PID controller. */ public void setCurrentMode(EncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(ControlMode.Current); + changeControlMode(JaguarControlMode.Current); setPositionReference(CANJNI.LM_REF_NONE); setSpeedReference(CANJNI.LM_REF_NONE); configEncoderCodesPerRev(codesPerRev); @@ -1288,7 +1297,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param d The differential gain of the Jaguar's PID controller. */ public void setCurrentMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(ControlMode.Current); + changeControlMode(JaguarControlMode.Current); setPositionReference(CANJNI.LM_REF_ENCODER); setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); configEncoderCodesPerRev(codesPerRev); @@ -1307,7 +1316,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param d The differential gain of the Jaguar's PID controller. */ public void setCurrentMode(PotentiometerTag tag, double p, double i, double d) { - changeControlMode(ControlMode.Current); + changeControlMode(JaguarControlMode.Current); setPositionReference(CANJNI.LM_REF_POT); setSpeedReference(CANJNI.LM_REF_NONE); configPotentiometerTurns(1); @@ -1327,7 +1336,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param d The differential gain of the Jaguar's PID controller. */ public void setSpeedMode(EncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(ControlMode.Speed); + changeControlMode(JaguarControlMode.Speed); setPositionReference(CANJNI.LM_REF_NONE); setSpeedReference(CANJNI.LM_REF_ENCODER); configEncoderCodesPerRev(codesPerRev); @@ -1347,7 +1356,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param d The differential gain of the Jaguar's PID controller. */ public void setSpeedMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(ControlMode.Speed); + changeControlMode(JaguarControlMode.Speed); setPositionReference(CANJNI.LM_REF_ENCODER); setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); configEncoderCodesPerRev(codesPerRev); @@ -1367,7 +1376,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * */ public void setPositionMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(ControlMode.Position); + changeControlMode(JaguarControlMode.Position); setPositionReference(CANJNI.LM_REF_ENCODER); configEncoderCodesPerRev(codesPerRev); setPID(p, i, d); @@ -1384,7 +1393,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param d The differential gain of the Jaguar's PID controller. */ public void setPositionMode(PotentiometerTag tag, double p, double i, double d) { - changeControlMode(ControlMode.Position); + changeControlMode(JaguarControlMode.Position); setPositionReference(CANJNI.LM_REF_POT); configPotentiometerTurns(1); setPID(p, i, d); @@ -1397,7 +1406,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * {@link CANJaguar#enableControl(double)} to enable the device. */ public void setVoltageMode() { - changeControlMode(ControlMode.Voltage); + changeControlMode(JaguarControlMode.Voltage); setPositionReference(CANJNI.LM_REF_NONE); setSpeedReference(CANJNI.LM_REF_NONE); } @@ -1412,7 +1421,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param codesPerRev The counts per revolution on the encoder */ public void setVoltageMode(EncoderTag tag, int codesPerRev) { - changeControlMode(ControlMode.Voltage); + changeControlMode(JaguarControlMode.Voltage); setPositionReference(CANJNI.LM_REF_NONE); setSpeedReference(CANJNI.LM_REF_ENCODER); configEncoderCodesPerRev(codesPerRev); @@ -1428,7 +1437,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param codesPerRev The counts per revolution on the encoder */ public void setVoltageMode(QuadEncoderTag tag, int codesPerRev) { - changeControlMode(ControlMode.Voltage); + changeControlMode(JaguarControlMode.Voltage); setPositionReference(CANJNI.LM_REF_ENCODER); setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); configEncoderCodesPerRev(codesPerRev); @@ -1441,7 +1450,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @param tag The constant {@link CANJaguar#kPotentiometer} */ public void setVoltageMode(PotentiometerTag tag) { - changeControlMode(ControlMode.Voltage); + changeControlMode(JaguarControlMode.Voltage); setPositionReference(CANJNI.LM_REF_POT); setSpeedReference(CANJNI.LM_REF_NONE); configPotentiometerTurns(1); @@ -1475,7 +1484,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * @see CANJaguar#setVoltageMode(EncoderTag, int) * @see CANJaguar#setVoltageMode(QuadEncoderTag, int) */ - private void changeControlMode(ControlMode controlMode) { + private void changeControlMode(JaguarControlMode controlMode) { // Disable the previous mode disableControl(); @@ -1489,12 +1498,16 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed * * Ask the Jagaur what mode it is in. * - * @return ControlMode that the Jag is in. + * @return JaguarControlMode that the Jag is in. */ - public ControlMode getControlMode() { + public JaguarControlMode getControlMode() { return m_controlMode; } + public void setControlMode(int mode) { + changeControlMode(JaguarControlMode.values()[mode]); + } + /** * Get the voltage at the battery input terminals of the Jaguar. * @@ -1829,7 +1842,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed double m_value = 0.0f; // Parameters/configuration - ControlMode m_controlMode; + JaguarControlMode m_controlMode; int m_speedReference = CANJNI.LM_REF_NONE; int m_positionReference = CANJNI.LM_REF_NONE; double m_p = 0.0; @@ -2268,10 +2281,6 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed /* * Live Window code, only does anything if live window is activated. */ - @Override - public String getSmartDashboardType() { - return "Speed Controller"; - } private ITable m_table = null; private ITableListener m_table_listener = null; @@ -2290,9 +2299,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed */ @Override public void updateTable() { - if (m_table != null) { - m_table.putNumber("Value", get()); - } + CANSpeedController.super.updateTable(); } /** @@ -2309,13 +2316,8 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed @Override public void startLiveWindowMode() { set(0); // Stop for safety - m_table_listener = new ITableListener() { - @Override - public void valueChanged(ITable itable, String key, Object value, boolean bln) { - set(((Double) value).doubleValue()); - } - }; - m_table.addTableListener("Value", m_table_listener, true); + m_table_listener = createTableListener(); + m_table.addTableListener(m_table_listener, true); } /** @@ -2324,7 +2326,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed @Override public void stopLiveWindowMode() { set(0); // Stop for safety - // TODO: Broken, should only remove the listener from "Value" only. + // TODO: See if this is still broken m_table.removeTableListener(m_table_listener); } } diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANSpeedController.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANSpeedController.java index 2b6856738e..e4a3471bf6 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANSpeedController.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANSpeedController.java @@ -1,50 +1,48 @@ package edu.wpi.first.wpilibj; -public interface CANSpeedController extends SpeedController { +import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.tables.ITable; +import edu.wpi.first.wpilibj.tables.ITableListener; + +public interface CANSpeedController extends SpeedController, PIDInterface, LiveWindowSendable { /** - * Mode determines how the motor is controlled, used internally. + * Mode determines how the motor is controlled, used internally. This is meant + * to be subclassed by enums + * (see {@link CANTalon.TalonControlMode CANTalon.TalonControlMode}). + * * * Note that the Jaguar does not support follower mode. */ - public enum ControlMode { - PercentVbus((byte) 0), Current((byte) 1), Speed((byte) 2), Position((byte) 3), Voltage((byte) 4), Follower( - (byte) 5); // Not supported by Jaguar. - - public byte value; - - public static ControlMode valueOf(byte value) { - for (ControlMode mode : values()) { - if (mode.value == value) { - return mode; - } - } - - return null; - } - - private ControlMode(byte value) { - this.value = value; - } + public interface ControlMode { + /** + * Gets the name of this control mode. Since this interface should only be + * subclassed by enumerations, this will be overridden by the builtin + * name() method. + */ + public String name(); + /** + * Checks if this control mode is PID-compatible. + */ + public boolean isPID(); + /** + * Gets the integer value of this control mode. + */ + public int getValue(); } /** - * Return the current setpoint. + * Gets the current control mode. * - * @return the current setpoint, as passed to {@link #set}. + * @return the current control mode */ - public double get(); + public ControlMode getControlMode(); /** - * Set the output for whichever mode we are currently in. + * Sets the control mode of this speed controller. * - * @param value the setpoint, in some units depending on the mode. + * @param mode the the new mode */ - public void set(double value); - - /** - * Disables to motor controller output. - */ - public void disable(); + public void setControlMode(int mode); /** * Set the proportional PID constant. @@ -61,6 +59,20 @@ public interface CANSpeedController extends SpeedController { */ public void setD(double d); + /** + * Set the feed-forward PID constant. This method is optional to implement. + */ + public default void setF(double f) { + } + + /** + * Gets the feed-forward PID constant. This method is optional to implement. + * If a subclass does not implement this, it will always return zero. + */ + public default double getF() { + return 0.0; + } + /** * Get the current input (battery) voltage. * @@ -115,4 +127,64 @@ public interface CANSpeedController extends SpeedController { * @param rampRate the maximum rate of change of the votlage, in Volts / sec. */ public void setVoltageRampRate(double rampRate); + + /** + * All CAN Speed Controllers have the same SmartDashboard type: "CANSpeedController". + */ + String SMART_DASHBOARD_TYPE = "CANSpeedController"; + + @Override + public default void updateTable() { + ITable table = getTable(); + if(table != null) { + table.putString("~TYPE~", SMART_DASHBOARD_TYPE); + table.putString("Type", getClass().getSimpleName()); // "CANTalon", "CANJaguar" + table.putNumber("Mode", getControlMode().getValue()); + if (getControlMode().isPID()) { + // CANJaguar throws an exception if you try to get its PID constants + // when it's not in a PID-compatible mode + table.putNumber("p", getP()); + table.putNumber("i", getI()); + table.putNumber("d", getD()); + table.putNumber("f", getF()); + } + table.putBoolean("Enabled", isEnabled()); + table.putNumber("Value", get()); + } + } + + @Override + public default String getSmartDashboardType() { + return SMART_DASHBOARD_TYPE; + } + + /** + * Creates an ITableListener for the LiveWindow table for this CAN speed + * controller. + */ + public default ITableListener createTableListener() { + return (table, key, value, isNew) -> { + switch(key) { + case "Enabled": + if ((Boolean) value) { + enable(); + } else { + disable(); + } + break; + case "Value": set((Double) value); break; + case "Mode": setControlMode(((Double) value).intValue()); break; + } + if(getControlMode().isPID()) { + switch(key) { + case "p": setP((Double) value); break; + case "i": setI((Double) value); break; + case "d": setD((Double) value); break; + case "f": setF((Double) value); break; + } + } + }; + } + + } 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 4485ee541d..689f3ca8e3 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 @@ -6,22 +6,19 @@ import edu.wpi.first.wpilibj.communication.UsageReporting; import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType; import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_double; import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_int; -import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_uint32_t; import edu.wpi.first.wpilibj.hal.SWIGTYPE_p_CTR_Code; -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, PIDInterface, - CANSpeedController, LiveWindowSendable { +public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedController { private MotorSafetyHelper m_safetyHelper; private boolean isInverted = false; protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement; - public enum TalonControlMode { - PercentVbus(0), Follower(5), Voltage(4), Position(1), Speed(2), Current(3), Disabled(15); + public enum TalonControlMode implements CANSpeedController.ControlMode { + PercentVbus(0), Position(1), Speed(2), Current(3), Voltage(4), Follower(5), Disabled(15); - public int value; + public final int value; public static TalonControlMode valueOf(int value) { for (TalonControlMode mode : values()) { @@ -36,6 +33,16 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface private TalonControlMode(int value) { this.value = value; } + + @Override + public boolean isPID() { + return this == Current || this == Speed || this == Position; + } + + @Override + public int getValue() { + return value; + } } public enum FeedbackDevice { @@ -546,6 +553,12 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface return m_controlMode; } + public void setControlMode(int mode) { + TalonControlMode tcm = TalonControlMode.valueOf(mode); + if(tcm != null) + changeControlMode(tcm); + } + /** * Fixup the m_controlMode so set() serializes the correct demand value. Also * fills the modeSelecet in the control frame to disabled. @@ -1132,10 +1145,6 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface /* * Live Window code, only does anything if live window is activated. */ - @Override - public String getSmartDashboardType() { - return "Speed Controller"; - } private ITable m_table = null; private ITableListener m_table_listener = null; @@ -1154,9 +1163,7 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface */ @Override public void updateTable() { - if (m_table != null) { - m_table.putNumber("Value", get()); - } + CANSpeedController.super.updateTable(); } /** @@ -1173,13 +1180,8 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface @Override public void startLiveWindowMode() { set(0); // Stop for safety - m_table_listener = new ITableListener() { - @Override - public void valueChanged(ITable itable, String key, Object value, boolean bln) { - set(((Double) value).doubleValue()); - } - }; - m_table.addTableListener("Value", m_table_listener, true); + m_table_listener = createTableListener(); + m_table.addTableListener(m_table_listener, true); } /** @@ -1188,7 +1190,7 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface @Override public void stopLiveWindowMode() { set(0); // Stop for safety - // TODO: Broken, should only remove the listener from "Value" only. + // TODO: See if this is still broken m_table.removeTableListener(m_table_listener); }