10 #include "ErrorBase.h"
11 #include "MotorSafety.h"
13 #include "MotorSafetyHelper.h"
14 #include "PIDOutput.h"
15 #include "CANSpeedController.h"
16 #include "HAL/cpp/Semaphore.hpp"
17 #include "HAL/HAL.hpp"
18 #include "LiveWindow/LiveWindowSendable.h"
19 #include "tables/ITableListener.h"
20 #include "NetworkCommunication/CANSessionMux.h"
21 #include "CAN/can_proto.h"
24 #include "HAL/cpp/priority_mutex.h"
39 static const int32_t kControllerRate = 1000;
40 static constexpr
double kApproxBusVoltage = 12.0;
45 static const struct EncoderStruct {
49 static const struct QuadEncoderStruct {
53 static const struct PotentiometerStruct {
63 virtual void PIDWrite(
float output)
override;
75 void SetCurrentMode(EncoderStruct, uint16_t codesPerRev,
double p,
double i,
77 void SetCurrentMode(QuadEncoderStruct, uint16_t codesPerRev,
double p,
79 void SetCurrentMode(PotentiometerStruct,
double p,
double i,
double d);
81 void SetSpeedMode(EncoderStruct, uint16_t codesPerRev,
double p,
double i,
83 void SetSpeedMode(QuadEncoderStruct, uint16_t codesPerRev,
double p,
double i,
88 void SetPositionMode(PotentiometerStruct,
double p,
double i,
double d);
96 virtual float Get()
const override;
97 virtual void Set(
float value, uint8_t syncGroup = 0)
override;
98 virtual void Disable()
override;
99 virtual void SetP(
double p)
override;
100 virtual void SetI(
double i)
override;
101 virtual void SetD(
double d)
override;
102 virtual void SetPID(
double p,
double i,
double d)
override;
103 virtual double GetP()
const override;
104 virtual double GetI()
const override;
105 virtual double GetD()
const override;
106 virtual bool IsModePID(CANSpeedController::ControlMode mode)
const override;
112 virtual double GetSpeed()
const override;
115 virtual uint16_t
GetFaults()
const override;
122 double reverseLimitPosition)
override;
134 void SetExpiration(
float timeout)
override;
135 float GetExpiration()
const override;
136 bool IsAlive()
const override;
138 bool IsSafetyEnabled()
const override;
139 void SetSafetyEnabled(
bool enabled)
override;
140 void GetDescription(std::ostringstream& desc)
const override;
141 uint8_t GetDeviceID()
const;
144 virtual void SetInverted(
bool isInverted)
override;
155 uint8_t packPercentage(uint8_t *buffer,
double value);
156 uint8_t packFXP8_8(uint8_t *buffer,
double value);
157 uint8_t packFXP16_16(uint8_t *buffer,
double value);
158 uint8_t packint16_t(uint8_t *buffer, int16_t value);
159 uint8_t packint32_t(uint8_t *buffer, int32_t value);
160 double unpackPercentage(uint8_t *buffer)
const;
161 double unpackFXP8_8(uint8_t *buffer)
const;
162 double unpackFXP16_16(uint8_t *buffer)
const;
163 int16_t unpackint16_t(uint8_t *buffer)
const;
164 int32_t unpackint32_t(uint8_t *buffer)
const;
166 void sendMessage(uint32_t messageID,
const uint8_t *data, uint8_t dataSize,
167 int32_t period = CAN_SEND_PERIOD_NO_REPEAT);
169 int32_t period = CAN_SEND_PERIOD_NO_REPEAT);
170 bool getMessage(uint32_t messageID, uint32_t mask, uint8_t *data,
171 uint8_t *dataSize)
const;
178 uint8_t m_deviceNumber;
179 float m_value = 0.0f;
182 ControlMode m_controlMode = kPercentVbus;
183 uint8_t m_speedReference = LM_REF_NONE;
184 uint8_t m_positionReference = LM_REF_NONE;
189 uint16_t m_encoderCodesPerRev = 0;
190 uint16_t m_potentiometerTurns = 0;
192 double m_forwardLimit = 0.0;
193 double m_reverseLimit = 0.0;
194 double m_maxOutputVoltage = 30.0;
195 double m_voltageRampRate = 0.0;
196 float m_faultTime = 0.0f;
199 bool m_controlModeVerified =
false;
200 bool m_speedRefVerified =
true;
201 bool m_posRefVerified =
true;
202 bool m_pVerified =
true;
203 bool m_iVerified =
true;
204 bool m_dVerified =
true;
205 bool m_neutralModeVerified =
true;
206 bool m_encoderCodesPerRevVerified =
true;
207 bool m_potentiometerTurnsVerified =
true;
208 bool m_forwardLimitVerified =
true;
209 bool m_reverseLimitVerified =
true;
210 bool m_limitModeVerified =
true;
211 bool m_maxOutputVoltageVerified =
true;
212 bool m_voltageRampRateVerified =
true;
213 bool m_faultTimeVerified =
true;
216 mutable float m_busVoltage = 0.0f;
217 mutable float m_outputVoltage = 0.0f;
218 mutable float m_outputCurrent = 0.0f;
219 mutable float m_temperature = 0.0f;
220 mutable double m_position = 0.0;
221 mutable double m_speed = 0.0;
222 mutable uint8_t m_limits = 0x00;
223 mutable uint16_t m_faults = 0x0000;
224 uint32_t m_firmwareVersion = 0;
225 uint8_t m_hardwareVersion = 0;
228 mutable std::atomic<bool> m_receivedStatusMessage0{
false};
229 mutable std::atomic<bool> m_receivedStatusMessage1{
false};
230 mutable std::atomic<bool> m_receivedStatusMessage2{
false};
232 bool m_controlEnabled =
false;
233 bool m_stopped =
false;
237 std::unique_ptr<MotorSafetyHelper> m_safetyHelper;
240 std::shared_ptr<nt::Value> value,
bool isNew)
override;
245 void InitTable(std::shared_ptr<ITable> subTable)
override;
246 std::shared_ptr<ITable>
GetTable()
const override;
248 std::shared_ptr<ITable> m_table;
251 void InitCANJaguar();
252 bool m_isInverted =
false;
virtual uint32_t GetFirmwareVersion() const override
Get the version of the firmware running on the Jaguar.
Definition: CANJaguar.cpp:1708
void SetVoltageMode()
Enable controlling the motor voltage without any position or speed feedback.
Definition: CANJaguar.cpp:1456
virtual float GetTemperature() const override
Get the internal temperature of the Jaguar.
Definition: CANJaguar.cpp:1590
virtual double GetSpeed() const override
Get the speed of the encoder.
Definition: CANJaguar.cpp:1617
void SetPositionMode(QuadEncoderStruct, uint16_t codesPerRev, double p, double i, double d)
Enable controlling the position with a feedback loop using an encoder.
Definition: CANJaguar.cpp:1424
A table whose values can be read and written to.
Definition: ITable.h:43
virtual double GetD() const override
Get the Differential gain of the controller.
Definition: CANJaguar.cpp:1147
void SetPositionReference(uint8_t reference)
Set the reference source device for position controller mode.
Definition: CANJaguar.cpp:970
virtual void SetVoltageRampRate(double rampRate) override
Set the maximum voltage change rate.
Definition: CANJaguar.cpp:1675
virtual void ConfigFaultTime(float faultTime) override
Configure how long the Jaguar waits in the case of a fault before resuming operation.
Definition: CANJaguar.cpp:1881
Definition: MotorSafety.h:14
void DisableControl()
Disable the closed loop controller.
Definition: CANJaguar.cpp:1207
void UpdateTable() override
Update the table for this sendable object with the latest values.
Definition: CANJaguar.cpp:1984
Live Window Sendable is a special type of object sendable to the live window.
Definition: LiveWindowSendable.h:18
uint8_t GetPositionReference() const
Get the reference source device for position controller mode.
Definition: CANJaguar.cpp:987
virtual void SetP(double p) override
Set the P constant for the closed loop modes.
Definition: CANJaguar.cpp:1007
virtual float GetBusVoltage() const override
Get the voltage at the battery input terminals of the Jaguar.
Definition: CANJaguar.cpp:1554
virtual double GetI() const override
Get the Intregral gain of the controller.
Definition: CANJaguar.cpp:1131
virtual double GetP() const override
Get the Proportional gain of the controller.
Definition: CANJaguar.cpp:1115
virtual void ConfigReverseLimit(double reverseLimitPosition) override
Set the position that if exceeded will disable the reverse direction.
Definition: CANJaguar.cpp:1841
Only use switches for limits.
Definition: CANSpeedController.h:58
void requestMessage(uint32_t messageID, int32_t period=CAN_SEND_PERIOD_NO_REPEAT)
Request a message from the Jaguar, but don't wait for it to arrive.
Definition: CANJaguar.cpp:425
virtual void ConfigPotentiometerTurns(uint16_t turns) override
Configure the number of turns on the potentiometer.
Definition: CANJaguar.cpp:1760
void StopMotor() override
Common interface for stopping the motor until the next Set() command Part of the MotorSafety interfac...
Definition: CANJaguar.cpp:1943
static void UpdateSyncGroup(uint8_t syncGroup)
Update all the motors that have pending sets in the syncGroup.
Definition: CANJaguar.cpp:1903
void SetSpeedReference(uint8_t reference)
Set the reference source device for speed controller mode.
Definition: CANJaguar.cpp:943
virtual void ConfigMaxOutputVoltage(double voltage) override
Configure the maximum voltage that the Jaguar will ever output.
Definition: CANJaguar.cpp:1861
virtual float GetOutputVoltage() const override
Get the voltage being output from the motor terminals of the Jaguar.
Definition: CANJaguar.cpp:1566
void setupPeriodicStatus()
Enables periodic status updates from the Jaguar.
Definition: CANJaguar.cpp:468
virtual float GetOutputCurrent() const override
Get the current through the motor terminals of the Jaguar.
Definition: CANJaguar.cpp:1578
virtual void ConfigEncoderCodesPerRev(uint16_t codesPerRev) override
Configure how many codes per revolution are generated by your encoder.
Definition: CANJaguar.cpp:1741
Class to read quad encoders.
Definition: Encoder.h:40
void SetSpeedMode(EncoderStruct, uint16_t codesPerRev, double p, double i, double d)
Enable controlling the speed with a feedback loop from a non-quadrature encoder.
Definition: CANJaguar.cpp:1382
bool getMessage(uint32_t messageID, uint32_t mask, uint8_t *data, uint8_t *dataSize) const
Get a previously requested message.
Definition: CANJaguar.cpp:442
void EnableControl(double encoderInitialPosition=0.0)
Enable the closed loop controller.
Definition: CANJaguar.cpp:1169
void StopLiveWindowMode() override
Stop having this sendable object automatically respond to value changes.
Definition: CANJaguar.cpp:2005
std::string GetSmartDashboardType() const override
Definition: CANJaguar.cpp:2011
virtual void DisableSoftPositionLimits() override
Disable Soft Position Limits if previously enabled.
Definition: CANJaguar.cpp:1799
Definition: priority_mutex.h:22
void InitTable(std::shared_ptr< ITable > subTable) override
Initializes a table for this sendable object.
Definition: CANJaguar.cpp:2015
void SetPercentMode()
Enable controlling the motor voltage as a percentage of the bus voltage without any position or speed...
Definition: CANJaguar.cpp:1239
virtual void SetControlMode(ControlMode mode)
Used internally.
Definition: CANJaguar.cpp:1520
virtual void ConfigNeutralMode(NeutralMode mode) override
Configure what the controller does to the H-Bridge when neutral (not driving the output).
Definition: CANJaguar.cpp:1726
virtual void ConfigForwardLimit(double forwardLimitPosition) override
Set the position that if exceeded will disable the forward direction.
Definition: CANJaguar.cpp:1824
Base class for most objects.
Definition: ErrorBase.h:66
virtual void SetInverted(bool isInverted) override
Common interface for inverting direction of a speed controller.
Definition: CANJaguar.cpp:1952
virtual void Set(float value, uint8_t syncGroup=0) override
Sets the output set-point value.
Definition: CANJaguar.cpp:244
virtual void Disable() override
Common interface for disabling a motor.
Definition: CANJaguar.cpp:323
A listener that listens to changes in values in a ITable.
Definition: ITableListener.h:18
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: CANJaguar.cpp:1962
Interface for potentiometers.
Definition: Potentiometer.h:16
virtual float Get() const override
Get the recently set outputValue setpoint.
Definition: CANJaguar.cpp:316
virtual void ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) override
Configure Soft Position Limits when in Position Controller mode.
Definition: CANJaguar.cpp:1787
virtual void ConfigLimitMode(LimitMode mode) override
Set the limit mode for position control mode.
Definition: CANJaguar.cpp:1809
std::shared_ptr< ITable > GetTable() const override
Definition: CANJaguar.cpp:2020
virtual ControlMode GetControlMode() const
Get the active control mode from the Jaguar.
Definition: CANJaguar.cpp:1545
void StartLiveWindowMode() override
Start having this sendable object automatically respond to value changes reflect the value on the tab...
Definition: CANJaguar.cpp:1999
uint8_t GetHardwareVersion() const
Get the version of the Jaguar hardware.
Definition: CANJaguar.cpp:1715
LimitMode
Definition: CANSpeedController.h:56
Use the NeutralMode that is set by the jumper wire on the CAN device.
Definition: CANSpeedController.h:48
uint8_t GetSpeedReference() const
Get the reference source device for speed controller mode.
Definition: CANJaguar.cpp:960
void updatePeriodicStatus() const
Check for new periodic status updates and unpack them into local variables.
Definition: CANJaguar.cpp:502
uint8_t getDeviceNumber() const
Definition: CANJaguar.cpp:227
virtual bool GetForwardLimitOK() const override
Get the status of the forward limit switch.
Definition: CANJaguar.cpp:1629
void SetCurrentMode(double p, double i, double d)
Enable controlling the motor current with a PID loop.
Definition: CANJaguar.cpp:1302
virtual void SetD(double d) override
Set the D constant for the closed loop modes.
Definition: CANJaguar.cpp:1079
virtual bool GetInverted() const override
Common interface for the inverting direction of a speed controller.
Definition: CANJaguar.cpp:1960
void sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t period=CAN_SEND_PERIOD_NO_REPEAT)
Send a message to the Jaguar.
Definition: CANJaguar.cpp:408
CANJaguar(uint8_t deviceNumber)
Constructor for the CANJaguar device.
Definition: CANJaguar.cpp:177
void verify()
Check all unverified params and make sure they're equal to their local cached versions.
Definition: CANJaguar.cpp:545
virtual uint16_t GetFaults() const override
Get the status of any faults the Jaguar has detected.
Definition: CANJaguar.cpp:1657
virtual double GetPosition() const override
Get the position of the encoder or potentiometer.
Definition: CANJaguar.cpp:1605
virtual void SetPID(double p, double i, double d) override
Set the P, I, and D constants for the closed loop modes.
Definition: CANJaguar.cpp:996
StringRef - Represent a constant reference to a string, i.e.
Definition: StringRef.h:39
virtual bool GetReverseLimitOK() const override
Get the status of the reverse limit switch.
Definition: CANJaguar.cpp:1641
Luminary Micro / Vex Robotics Jaguar Speed Control.
Definition: CANJaguar.h:32
Interface for "smart" CAN-based speed controllers.
Definition: CANSpeedController.h:17
virtual void SetI(double i) override
Set the I constant for the closed loop modes.
Definition: CANJaguar.cpp:1043
virtual void PIDWrite(float output) override
Write out the PID value as seen in the PIDOutput base object.
Definition: CANJaguar.cpp:333
NeutralMode
Definition: CANSpeedController.h:46