11 #include "CANSpeedController.h"
12 #include "PIDOutput.h"
13 #include "PIDSource.h"
14 #include "PIDInterface.h"
15 #include "HAL/CanTalonSRX.h"
16 #include "MotorSafetyHelper.h"
17 #include "LiveWindow/LiveWindowSendable.h"
18 #include "tables/ITable.h"
51 enum StatusFrameRate {
52 StatusFrameRateGeneral = 0,
53 StatusFrameRateFeedback = 1,
54 StatusFrameRateQuadEncoder = 2,
55 StatusFrameRateAnalogTempVbat = 3,
56 StatusFrameRatePulseWidthMeas = 4,
96 SetValueMotionProfileDisable = 0,
97 SetValueMotionProfileEnable = 1,
98 SetValueMotionProfileHold = 2,
203 explicit CANTalon(
int deviceNumber);
204 explicit CANTalon(
int deviceNumber,
int controlPeriodMs);
209 virtual void PIDWrite(
float output)
override;
212 virtual double PIDGet()
override;
215 virtual void SetExpiration(
float timeout)
override;
216 virtual float GetExpiration()
const override;
217 virtual bool IsAlive()
const override;
219 virtual void SetSafetyEnabled(
bool enabled)
override;
220 virtual bool IsSafetyEnabled()
const override;
221 virtual void GetDescription(std::ostringstream& desc)
const override;
224 virtual float Get()
const override;
225 virtual void Set(
float value, uint8_t syncGroup = 0)
override;
226 virtual void Reset()
override;
228 virtual void Disable()
override;
230 virtual void Enable()
override;
231 virtual void SetP(
double p)
override;
232 virtual void SetI(
double i)
override;
233 virtual void SetD(
double d)
override;
236 virtual void SetPID(
double p,
double i,
double d)
override;
237 virtual void SetPID(
double p,
double i,
double d,
double f);
238 virtual double GetP()
const override;
239 virtual double GetI()
const override;
240 virtual double GetD()
const override;
241 virtual double GetF()
const;
242 virtual bool IsModePID(CANSpeedController::ControlMode mode)
const override;
249 virtual double GetSpeed()
const override;
253 virtual void SetAnalogPosition(
int newPosition);
257 virtual void SetEncPosition(
int);
264 int GetNumberOfQuadIdxRises()
const;
265 void SetNumberOfQuadIdxRises(
int rises);
266 virtual int GetPulseWidthPosition()
const;
267 virtual void SetPulseWidthPosition(
int newpos);
268 virtual int GetPulseWidthVelocity()
const;
269 virtual int GetPulseWidthRiseToFallUs()
const;
270 virtual int GetPulseWidthRiseToRiseUs()
const;
274 virtual uint16_t
GetFaults()
const override;
275 uint16_t GetStickyFaults()
const;
276 void ClearStickyFaults();
278 virtual void SetVoltageCompensationRampRate(
double rampRate);
284 double reverseLimitPosition)
override;
315 void ConfigPeakOutputVoltage(
double forwardVoltage,
double reverseVoltage);
316 void ConfigNominalOutputVoltage(
double forwardVoltage,
double reverseVoltage);
326 bool GetParameter(uint32_t paramEnum,
double & dvalue)
const;
411 std::shared_ptr<nt::Value> value,
bool isNew)
override;
416 void InitTable(std::shared_ptr<ITable> subTable)
override;
417 std::shared_ptr<ITable>
GetTable()
const override;
420 virtual void SetInverted(
bool isInverted)
override;
425 enum TalonControlMode {
432 kMotionProfileMode = 6,
437 std::unique_ptr<CanTalonSRX> m_impl;
438 std::unique_ptr<MotorSafetyHelper> m_safetyHelper;
442 bool m_controlEnabled =
true;
443 bool m_stopped =
false;
444 ControlMode m_controlMode = kPercentVbus;
445 TalonControlMode m_sendMode;
447 double m_setPoint = 0;
454 uint32_t m_codesPerRev = 0;
463 uint32_t m_numPotTurns = 0;
470 static const unsigned int kDelayForSolicitedSignalsUs = 4000;
480 double GetNativeUnitsPerRotationScalar(
FeedbackDevice devToLookup)
const;
487 void ApplyControlMode(CANSpeedController::ControlMode mode);
497 int32_t ScaleRotationsToNativeUnits(
FeedbackDevice devToLookup,
double fullRotations)
const;
507 int32_t ScaleVelocityToNativeUnits(
FeedbackDevice devToLookup,
double rpm)
const;
516 double ScaleNativeUnitsToRotations(
FeedbackDevice devToLookup, int32_t nativePos)
const;
525 double ScaleNativeUnitsToRpm(
FeedbackDevice devToLookup, int32_t nativeVel)
const;
528 std::shared_ptr<ITable> m_table;
virtual double PIDGet() override
Retrieve the current sensor value.
Definition: CANTalon.cpp:86
FeedbackDevice
Definition: CANTalon.h:33
void ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, bool bReverseLimitSwitchEn)
Overrides the forward and reverse limit switch enables.
Definition: CANTalon.cpp:1221
SetValueMotionProfile outputEnable
The current output mode of the motion profile executer (disabled, enabled, or hold).
Definition: CANTalon.h:201
virtual float GetOutputVoltage() const override
Definition: CANTalon.cpp:490
virtual void ConfigMaxOutputVoltage(double voltage) override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:1404
void SelectProfileSlot(int slotIdx)
SRX has two available slots for PID.
Definition: CANTalon.cpp:301
A table whose values can be read and written to.
Definition: ITable.h:43
virtual void Reset() override
Resets the integral term and disables the controller.
Definition: CANTalon.cpp:199
virtual void SetPID(double p, double i, double d) override
Sets control values for closed loop control.
Definition: CANTalon.cpp:318
virtual ControlMode GetControlMode() const
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:1533
int GetPinStateQuadA() const
Definition: CANTalon.cpp:809
virtual double GetI() const override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:386
void SetIzone(unsigned iz)
Set the Izone to a nonzero value to auto clear the integral accumulator when the absolute value of Cl...
Definition: CANTalon.cpp:291
void ConfigRevLimitSwitchNormallyOpen(bool normallyOpen)
Change the rev limit switch setting to normally open or closed.
Definition: CANTalon.cpp:1382
void StartLiveWindowMode() override
Start having this sendable object automatically respond to value changes reflect the value on the tab...
Definition: CANTalon.cpp:1941
virtual void EnableControl()
Enables control of the Talon, allowing the motor to move.
Definition: CANTalon.cpp:217
Definition: MotorSafety.h:14
virtual void Set(float value, uint8_t syncGroup=0) override
Sets the appropriate output on the talon, depending on the mode.
Definition: CANTalon.cpp:134
Live Window Sendable is a special type of object sendable to the live window.
Definition: LiveWindowSendable.h:18
virtual void ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) override
Definition: CANTalon.cpp:1200
bool hasUnderrun
Set if isUnderrun ever gets set.
Definition: CANTalon.h:180
virtual double GetF() const
Definition: CANTalon.cpp:429
CTRE Talon SRX Speed Controller with CAN Control.
Definition: CANTalon.h:25
virtual bool GetInverted() const override
Common interface for the inverting direction of a speed controller.
Definition: CANTalon.cpp:1893
void ConfigFwdLimitSwitchNormallyOpen(bool normallyOpen)
Change the fwd limit switch setting to normally open or closed.
Definition: CANTalon.cpp:1364
TrajectoryPoint activePoint
The number of points in the low level Talon buffer.
Definition: CANTalon.h:195
int IsRevLimitSwitchClosed() const
Definition: CANTalon.cpp:856
virtual void SetInverted(bool isInverted) override
Common interface for inverting direction of a speed controller.
Definition: CANTalon.cpp:1885
virtual void DisableSoftPositionLimits() override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:1210
void UpdateTable() override
Update the table for this sendable object with the latest values.
Definition: CANTalon.cpp:1927
virtual double GetD() const override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:408
virtual void ConfigNeutralMode(NeutralMode mode) override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:1135
void ChangeMotionControlFramePeriod(int periodMs)
Calling application can opt to speed up the handshaking between the robot API and the Talon to increa...
Definition: CANTalon.cpp:1756
void SetStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs)
Select the feedback device to use in closed-loop.
Definition: CANTalon.cpp:352
virtual void ConfigForwardLimit(double forwardLimitPosition) override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:1320
virtual void SetSetpoint(float value) override
Sets the setpoint to value.
Definition: CANTalon.cpp:194
virtual void SetI(double i) override
Set the integration constant of the currently selected profile.
Definition: CANTalon.cpp:254
void ClearMotionProfileHasUnderrun()
Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is ready for another point...
Definition: CANTalon.cpp:1876
int GetPinStateQuadB() const
Definition: CANTalon.cpp:820
void ClearIaccum()
Clear the accumulator for I gain.
Definition: CANTalon.cpp:1125
int GetBrakeEnableDuringNeutral() const
Definition: CANTalon.cpp:1161
Motion Profile Status This is simply a data transer object.
Definition: CANTalon.h:157
bool IsMotionProfileTopLevelBufferFull()
Definition: CANTalon.cpp:1804
Sensor is present and working okay.
Definition: CANTalon.h:48
virtual double GetPosition() const override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:550
int GetPinStateQuadIdx() const
Definition: CANTalon.cpp:831
virtual int GetClosedLoopError() const
Returns the current error in the controller.
Definition: CANTalon.cpp:591
int IsFwdLimitSwitchClosed() const
Definition: CANTalon.cpp:843
virtual float GetBusVoltage() const override
Returns the voltage coming in from the battery.
Definition: CANTalon.cpp:478
Base class for most objects.
Definition: ErrorBase.h:66
virtual void ConfigReverseLimit(double reverseLimitPosition) override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:1393
virtual FeedbackDeviceStatus IsSensorPresent(FeedbackDevice feedbackDevice) const
Definition: CANTalon.cpp:770
virtual void Enable() override
Enables control of the Talon, allowing the motor to move.
Definition: CANTalon.cpp:225
int GetIaccum() const
Definition: CANTalon.cpp:1109
virtual void StopMotor() override
Common interface for stopping the motor until the next Set() call Part of the MotorSafety interface...
Definition: CANTalon.cpp:1901
bool isLastPoint
Set to true to signal Talon that this is the final point, so do not attempt to pop another trajectory...
Definition: CANTalon.h:140
void SetClosedLoopOutputDirection(bool reverseOutput)
Flips the sign (multiplies by negative one) the throttle values going into the motor on the talon in ...
Definition: CANTalon.cpp:580
int GetMotionProfileTopLevelBufferCount()
Retrieve just the buffer count for the api-level (top) buffer.
Definition: CANTalon.cpp:1777
SetValueMotionProfile
Enumerated types for Motion Control Set Values.
Definition: CANTalon.h:95
virtual int GetEncPosition() const
Get the position of whatever is in the analog pin of the Talon, regardless of whether it is actually ...
Definition: CANTalon.cpp:699
void InitTable(std::shared_ptr< ITable > subTable) override
Initializes a table for this sendable object.
Definition: CANTalon.cpp:1957
Sensor is not present, not plugged in, not powered, etc...
Definition: CANTalon.h:49
bool zeroPos
Set to true to signal Talon to zero the selected sensor.
Definition: CANTalon.h:151
virtual void SetControlMode(ControlMode mode)
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:1522
bool GetParameter(uint32_t paramEnum, double &dvalue) const
General get frame.
Definition: CANTalon.cpp:1452
virtual int GetEncVel() const
Get the position of whatever is in the analog pin of the Talon, regardless of whether it is actually ...
Definition: CANTalon.cpp:720
bool velocityOnly
Set to true to only perform the velocity feed-forward and not perform position servo.
Definition: CANTalon.h:133
virtual void ConfigEncoderCodesPerRev(uint16_t codesPerRev) override
Configure how many codes per revolution are generated by your encoder.
Definition: CANTalon.cpp:1174
unsigned int profileSlotSelect
Which slot to get PIDF gains.
Definition: CANTalon.h:120
virtual void SetP(double p) override
Definition: CANTalon.cpp:241
virtual void ConfigPotentiometerTurns(uint16_t turns) override
Configure the number of turns on the potentiometer.
Definition: CANTalon.cpp:1188
A listener that listens to changes in values in a ITable.
Definition: ITableListener.h:18
virtual double GetP() const override
Get the current proportional constant.
Definition: CANTalon.cpp:365
int GetIzone() const
Definition: CANTalon.cpp:449
bool activePointValid
True if the active trajectory point has not empty, false otherwise.
Definition: CANTalon.h:191
void EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge)
Enables Talon SRX to automatically zero the Sensor Position whenever an edge is detected on the index...
Definition: CANTalon.cpp:1737
void ValueChanged(ITable *source, llvm::StringRef key, std::shared_ptr< nt::Value > value, bool isNew) override
Called when a key-value pair is changed in a ITable.
Definition: CANTalon.cpp:1906
void SetFeedbackDevice(FeedbackDevice device)
Select the feedback device to use in closed-loop.
Definition: CANTalon.cpp:340
virtual int GetAnalogIn() const
Get the position of whatever is in the analog pin of the Talon, regardless of whether it is actually ...
Definition: CANTalon.cpp:656
virtual uint16_t GetFaults() const override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:931
virtual float Get() const override
Gets the current status of the Talon (usually a sensor value).
Definition: CANTalon.cpp:98
PIDSource interface is a generic sensor source for the PID class.
Definition: PIDSource.h:18
virtual void Disable() override
Disables control of the talon, causing the motor to brake or coast depending on its mode (see the Tal...
Definition: CANTalon.cpp:209
virtual void ConfigLimitMode(LimitMode mode) override
Configures the soft limit enable (wear leveled persistent memory).
Definition: CANTalon.cpp:1255
CANTalon(int deviceNumber)
Constructor for the CANTalon device.
Definition: CANTalon.cpp:34
unsigned int timeDurMs
Time in milliseconds to process this point.
Definition: CANTalon.h:112
Cross The Road Electronics Magnetic Encoder in Absolute/PulseWidth Mode.
Definition: CANTalon.h:39
Definition: PIDInterface.h:14
void ConfigSetParameter(uint32_t paramEnum, double value)
General set frame.
Definition: CANTalon.cpp:1440
Sensor status could not be determined. Not all sensors can do this.
Definition: CANTalon.h:47
virtual double GetSpeed() const override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:638
virtual void ConfigFaultTime(float faultTime) override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:1474
bool PushMotionProfileTrajectory(const TrajectoryPoint &trajPt)
Push another trajectory point into the top level buffer (which is emptied into the Talon's bottom buf...
Definition: CANTalon.cpp:1789
bool isUnderrun
This is set if Talon needs to shift a point from its buffer into the active trajectory point however ...
Definition: CANTalon.h:186
unsigned int topBufferRem
The available empty slots in the trajectory buffer.
Definition: CANTalon.h:165
double position
The position to servo to.
Definition: CANTalon.h:105
virtual float GetOutputCurrent() const override
Returns the current going through the Talon, in Amperes.
Definition: CANTalon.cpp:503
virtual int GetAnalogInVel() const
Get the position of whatever is in the analog pin of the Talon, regardless of whether it is actually ...
Definition: CANTalon.cpp:684
bool IsControlEnabled() const
Definition: CANTalon.cpp:230
void SetCloseLoopRampRate(double rampRate)
Sets a voltage change rate that applies only when a close loop contorl mode is enabled.
Definition: CANTalon.cpp:1074
void ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn)
Set the Forward Soft Limit Enable.
Definition: CANTalon.cpp:1334
LimitMode
Definition: CANSpeedController.h:56
virtual void PIDWrite(float output) override
Write out the PID value as seen in the PIDOutput base object.
Definition: CANTalon.cpp:72
void SetSensorDirection(bool reverseSensor)
If sensor and motor are out of phase, sensor can be inverted (position and velocity multiplied by -1)...
Definition: CANTalon.cpp:563
virtual float GetTemperature() const override
Returns temperature of Talon, in degrees Celsius.
Definition: CANTalon.cpp:517
std::shared_ptr< ITable > GetTable() const override
Definition: CANTalon.cpp:1962
unsigned int topBufferCnt
The number of points in the top trajectory buffer.
Definition: CANTalon.h:169
Motion Profile Trajectory Point This is simply a data transer object.
Definition: CANTalon.h:104
Cross The Road Electronics Magnetic Encoder in Relative/Quadrature Mode.
Definition: CANTalon.h:40
void SetPosition(double pos)
Set the position value of the selected sensor.
Definition: CANTalon.cpp:532
virtual void SetD(double d) override
Set the derivative constant of the currently selected profile.
Definition: CANTalon.cpp:267
std::string GetSmartDashboardType() const override
Definition: CANTalon.cpp:1953
bool IsEnabled() const override
Definition: CANTalon.cpp:235
double velocity
The velocity to feed-forward.
Definition: CANTalon.h:106
void StopLiveWindowMode() override
Stop having this sendable object automatically respond to value changes.
Definition: CANTalon.cpp:1947
void ProcessMotionProfileBuffer()
This must be called periodically to funnel the trajectory points from the API's top level buffer to t...
Definition: CANTalon.cpp:1815
void SetF(double f)
Set the feedforward value of the currently selected profile.
Definition: CANTalon.cpp:279
virtual void SetVoltageRampRate(double rampRate) override
Set the maximum voltage change rate.
Definition: CANTalon.cpp:1047
unsigned int btmBufferCnt
The number of points in the low level Talon buffer.
Definition: CANTalon.h:173
double GetSetpoint() const override
Definition: CANTalon.cpp:471
StringRef - Represent a constant reference to a string, i.e.
Definition: StringRef.h:39
virtual void SetAllowableClosedLoopErr(uint32_t allowableCloseLoopError)
Set the allowable closed loop error.
Definition: CANTalon.cpp:606
virtual uint32_t GetFirmwareVersion() const override
Definition: CANTalon.cpp:1085
void ClearMotionProfileTrajectories()
Clear the buffered motion profile in both Talon RAM (bottom), and in the API (top).
Definition: CANTalon.cpp:1765
virtual int GetAnalogInRaw() const
Get the position of whatever is in the analog pin of the Talon, regardless of whether it is actually ...
Definition: CANTalon.cpp:677
FeedbackDeviceStatus
Depending on the sensor type, Talon can determine if sensor is plugged in ot not. ...
Definition: CANTalon.h:46
void GetMotionProfileStatus(MotionProfileStatus &motionProfileStatus)
Retrieve all status information.
Definition: CANTalon.cpp:1825
Interface for "smart" CAN-based speed controllers.
Definition: CANSpeedController.h:17
virtual bool GetForwardLimitOK() const override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:893
void ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn)
Set the Reverse Soft Limit Enable.
Definition: CANTalon.cpp:1347
NeutralMode
Definition: CANSpeedController.h:46
virtual bool GetReverseLimitOK() const override
TODO documentation (see CANJaguar.cpp)
Definition: CANTalon.cpp:912