Package com.thethriftybot
Class ThriftyNova
java.lang.Object
com.thethriftybot.ThriftyNova
- All Implemented Interfaces:
MotorController
,AutoCloseable
API for interfacing with the Thrifty Nova motor controller.
URL to docs here:
https://docs.home.thethriftybot.com
-
Nested Class Summary
Nested ClassesModifier and TypeClassDescriptionstatic final class
An internal CAN frame frequency configuration structure.static enum
Different types of current control.static enum
Different encoder types.static enum
Different types of configuration errors.static enum
Different types of external encoder that can be used.static enum
static enum
Different types of motors that can be driven by the controller.static final class
An internal PID controller configuration structure.static enum
The various PID configuration slots. -
Field Summary
FieldsModifier and TypeFieldDescriptionprotected int
protected int
protected final NetworkTable
protected boolean
final ThriftyNova.PIDConfig
final ThriftyNova.PIDConfig
-
Constructor Summary
ConstructorsConstructorDescriptionThriftyNova
(int canID) Instantiate a new ThriftyMotor objectThriftyNova
(int canID, ThriftyNova.MotorType motorType) Instantiate a new ThriftyMotor object using a CAN identifier. -
Method Summary
Modifier and TypeMethodDescriptionboolean
Gets whether hard limits are enabled.boolean
Gets whether soft limits are enabled.void
Clear the error buffer.void
close()
void
disable()
enableHardLimits
(boolean enable) Enable / disable hard limits.enableSoftLimits
(boolean enable) Enable / disable soft limits.void
Factory resets the motor controller to its default state.void
follow
(int followerID) Drives the motor using the parameters of the configured followed motor controller.double
get()
Returns the last set point specified by the user in any control method.int
Gets the absolute encoder offset value.boolean
Get the absolute wrapping state.Gets the alterante quadrature index state.boolean
Get the brake mode status of the motor.double
Returns the error to the last set point that was input, based on last used control mode (position or velocity), and encoder type (internal or quaderature).Gets the current type being used for limiting.Returns a list of the errors that have populated in the buffer.Gets the forward limit switch state.double
Gets the forward soft limit.int
getID()
Returns the CAN ID the device was initialized with.boolean
Get the inversion status of the motor.boolean
Gets the inversion of the motor.boolean[]
Gets the IO state of the motor controller.double
Gets the maximum current limit.double
Get the maximum forward percent output.double
Get the maximum reverse percent output.Gets the motor type being used for the motor controller.double
Gets the encoder position measurement.double
Gets the absolute encoder position.double
Gets the internal encoder position.double
Gets the quadrature encoder position.getQuadA()
Gets the Quadrature encoder A state.getQuadB()
Gets the Quadrature encoder B state.Gets the Quadrature encoder index state.double
Gets the ramp down time in seconds.double
Gets the ramp up time in seconds.Gets the reverse limit switch state.double
Gets the reverse soft limit.double
Returns the last set point specified by the user in any control method.double
Gets the stator current measurement.double
Gets the supply current measurement.double
Gets the temperature of the motor controller.boolean
Gets the temperature throttle enable state.double
Gets the encoder velocity measurement.double
Gets the internal encoder velocity.double
Gets the quadrature encoder velocity.double
Gets the voltage measurement.double
Get the voltage compensation value.boolean
Checks if there is a specific error present in the buffer.protected boolean
protected double
protected int
protected String
protected int
receiveStatusFrame
(int index, int startByte, int endByte) protected int
receiveStatusFrameByBit
(int index, int startBit, int endBit) void
set
(double percentOutput) Sets the percent output of the motor from between 1.0 (full forward) and -1.0 (full reverse), where 0.0 is full stop.void
set
(double percentOutput, double feedForward) Sets the percent output of the motor from between 1.0 (full forward) and -1.0 (full reverse), where 0.0 is full stop.setAbsOffset
(int offset) Sets the absolute encoder offset.setAbsoluteWrapping
(boolean enabled) Set the absolute wrapping state.setBrakeMode
(boolean brakeMode) Set the brake mode status of the motor.void
setEncoderPosition
(double position) Sets the encoder position.setInversion
(boolean inverted) Set the inversion status of the motor.void
setInverted
(boolean inverted) Set the inversion status of the motor.setMaxCurrent
(ThriftyNova.CurrentType currentType, double maxCurrent) Set the max current the motor can draw in amps.setMaxOutput
(double maxOutput) Set the maximum percent output.setMaxOutput
(double maxFwd, double maxRev) Set the maximum forward and reverse percent output.setMotorType
(ThriftyNova.MotorType motorType) Sets the motor type to use for the motor controller.void
setNTLogging
(boolean enabled) void
setPercent
(double percentOutput) Sets the percent output of the motor from between 1.0 (full forward) and -1.0 (full reverse), where 0.0 is full stop.void
setPercent
(double percentOutput, double feedForward) Sets the percent output of the motor from between 1.0 (full forward) and -1.0 (full reverse), where 0.0 is full stop.void
setPosition
(double targetPosition) Drives the motor towards the given position using the configured PID controller.void
setPosition
(double targetPosition, double feedForward) Drives the motor towards the given position using the configured PID controller.void
setPositionAbs
(double targetPosition) Sets the target position using the absolute encoder.void
setPositionAbs
(double targetPosition, double feedForward) Sets the target position using the absolute encoder.void
setPositionInternal
(double targetPosition) Sets the target position using the internal encoder.void
setPositionInternal
(double targetPosition, double feedForward) Sets the target position using the internal encoder with feed forward.void
setPositionQuad
(double targetPosition) Sets the target position using the quadrature encoder.void
setPositionQuad
(double targetPosition, double feedForward) Sets the target position using the quadrature encoder with feed forward.setRampDown
(double rampDown) Sets the time to ramp down in seconds from 0 to 10.setRampUp
(double rampUp) Sets the time to ramp up in seconds from 0 to 10.setSoftLimits
(double revLimit, double fwdLimit) Sets the soft limits that the motor will not cross if soft limits are enabled.setTempThrottleEnable
(boolean enabled) Sets the temperature throttle enable state.void
setVelocity
(double targetVelocity) Drives the motor at the given velocity using the configured PID controller.void
setVelocity
(double targetVelocity, double feedForward) Drives the motor at the given velocity using the configured PID controller.void
setVelocityInternal
(double targetVelocity) Sets the target velocity using the internal encoder.void
setVelocityInternal
(double targetVelocity, double feedForward) Sets the target velocity using the internal encoder with feed forward.void
setVelocityQuad
(double targetVelocity) Sets the target velocity using the quadrature encoder.void
setVelocityQuad
(double targetVelocity, double feedForward) Sets the target velocity using the quadrature encoder with feed forward.setVoltageCompensation
(double vcomp) Sets the voltage compensation value.void
void
Calls every status/feedback getter that has a network table entrystatic void
Updates the network tables for all ThriftyNova motors.useEncoderType
(ThriftyNova.EncoderType encoderType) Set the encoder type to use for feedback control.usePIDSlot
(ThriftyNova.PIDSlot pidSlot) Set the PID slot to use for feedback control.Zeros the absolute encoder reading at the current position.Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
Methods inherited from interface edu.wpi.first.wpilibj.motorcontrol.MotorController
setVoltage, setVoltage
-
Field Details
-
errors
-
pid0
-
pid1
-
canFreq
-
canID
protected int canID -
deviceHandle
protected int deviceHandle -
deviceTable
-
doNTLogging
protected boolean doNTLogging
-
-
Constructor Details
-
ThriftyNova
public ThriftyNova(int canID) Instantiate a new ThriftyMotor object- Parameters:
canID
- The CAN identifier for the particular device.
-
ThriftyNova
Instantiate a new ThriftyMotor object using a CAN identifier.- Parameters:
canID
- The CAN identifier for the particular device.motorType
- The type of motor being controlled.
-
-
Method Details
-
getErrors
Returns a list of the errors that have populated in the buffer.- Returns:
- A list of errors.
-
clearErrors
public void clearErrors()Clear the error buffer. -
hasError
Checks if there is a specific error present in the buffer.- Parameters:
err
- The error to check for.- Returns:
- If the error exists or not.
-
setInverted
public void setInverted(boolean inverted) Set the inversion status of the motor. Same as setInversion, but does not return a value to satisfy the MotorController interface.- Specified by:
setInverted
in interfaceMotorController
- Parameters:
inverted
- If the motor be inverted or not.
-
setInversion
Set the inversion status of the motor.- Parameters:
inverted
- If the motor be inverted or not.
-
getInversion
public boolean getInversion()Get the inversion status of the motor.- Returns:
- The inversion status.
-
getInverted
public boolean getInverted()Gets the inversion of the motor.- Specified by:
getInverted
in interfaceMotorController
- Returns:
- The inversion of the motor.
-
setBrakeMode
Set the brake mode status of the motor.- Parameters:
brakeMode
- If the motor should be in brake mode or not.
-
getBrakeMode
public boolean getBrakeMode()Get the brake mode status of the motor.- Returns:
- The brake mode status.
-
setMaxOutput
Set the maximum percent output.- Parameters:
maxOutput
- Max forward [0, 1].
-
setMaxOutput
Set the maximum forward and reverse percent output.- Parameters:
maxFwd
- Max forward output [0, 1].maxRev
- Max reverse output [0, 1].
-
getMaxForwardOutput
public double getMaxForwardOutput()Get the maximum forward percent output.- Returns:
- The maximum forward percent output.
-
getMaxReverseOutput
public double getMaxReverseOutput()Get the maximum reverse percent output.- Returns:
- The maximum reverse percent output.
-
setRampUp
Sets the time to ramp up in seconds from 0 to 10. For example, an input of 0.5 will ramp the motor from idle to 100% over the course of 0.5 seconds.- Parameters:
rampUp
- The ramp up time.
-
setRampDown
Sets the time to ramp down in seconds from 0 to 10. For example, an input of 0.5 will ramp the motor from 100% to idle over the course of 0.5 seconds.- Parameters:
rampDown
- The ramp up time.
-
getRampUp
public double getRampUp()Gets the ramp up time in seconds.- Returns:
- The ramp up time.
-
getRampDown
public double getRampDown()Gets the ramp down time in seconds.- Returns:
- The ramp down time.
-
setMaxCurrent
Set the max current the motor can draw in amps. Motor speed will be capped to satisfy the max current. Also set the what current reading is used for limiting calculations, between: - Stator: Uses the total draw of phase a, b, and c. - Supply: Uses stator plus the draw of the controller itself.- Parameters:
currentType
- The current reading used for limiting calculations.maxCurrent
- The max current.
-
setSoftLimits
Sets the soft limits that the motor will not cross if soft limits are enabled.- Parameters:
revLimit
- The reverse position the motor will not go past.fwdLimit
- The forward position the motor will not go past.
-
enableSoftLimits
Enable / disable soft limits.- Parameters:
enable
- If soft limits should be enabled.
-
enableHardLimits
Enable / disable hard limits.- Parameters:
enable
- If hard limits should be enabled.
-
getMaxCurrent
public double getMaxCurrent()Gets the maximum current limit.- Returns:
- The maximum current limit in amps.
-
getCurrentType
Gets the current type being used for limiting.- Returns:
- The current type.
-
getForwardSoftLimit
public double getForwardSoftLimit()Gets the forward soft limit.- Returns:
- The forward soft limit position.
-
getReverseSoftLimit
public double getReverseSoftLimit()Gets the reverse soft limit.- Returns:
- The reverse soft limit position.
-
areSoftLimitsEnabled
public boolean areSoftLimitsEnabled()Gets whether soft limits are enabled.- Returns:
- True if soft limits are enabled.
-
areHardLimitsEnabled
public boolean areHardLimitsEnabled()Gets whether hard limits are enabled.- Returns:
- True if hard limits are enabled.
-
setAbsOffset
Sets the absolute encoder offset.- Parameters:
offset
- The offset value to set.
-
getAbsOffset
public int getAbsOffset()Gets the absolute encoder offset value.- Returns:
- The current abs encoder offset.
-
zeroAbsEncoder
Zeros the absolute encoder reading at the current position. -
setTempThrottleEnable
Sets the temperature throttle enable state.- Parameters:
enabled
- True to enable temperature throttling, false to disable.
-
getTempThrottleEnable
public boolean getTempThrottleEnable()Gets the temperature throttle enable state.- Returns:
- True if temperature throttling is enabled, false otherwise.
-
setMotorType
Sets the motor type to use for the motor controller.- Parameters:
motorType
-- Returns:
-
getMotorType
Gets the motor type being used for the motor controller.- Returns:
- The motor type.
-
setVoltageCompensation
Sets the voltage compensation value.- Parameters:
vcomp
- The voltage compensation value.
-
getVoltageCompensationEnabled
public double getVoltageCompensationEnabled()Get the voltage compensation value.- Returns:
- The voltage compensation value.
-
useEncoderType
Set the encoder type to use for feedback control.- Parameters:
encoderType
- The encoder type to use.
-
setExternalEncoder
-
setAbsoluteWrapping
Set the absolute wrapping state.- Parameters:
enabled
- True to enable absolute wrapping, false to disable.
-
getAbsoluteWrapping
public boolean getAbsoluteWrapping()Get the absolute wrapping state.- Returns:
- True if absolute wrapping is enabled, false otherwise.
-
usePIDSlot
Set the PID slot to use for feedback control.- Parameters:
pidSlot
- The PID slot to use.
-
setNTLogging
public void setNTLogging(boolean enabled) -
setPercent
public void setPercent(double percentOutput) Sets the percent output of the motor from between 1.0 (full forward) and -1.0 (full reverse), where 0.0 is full stop.- Parameters:
percentOutput
- The percent output of the motor from [-1, 1].
-
setPercent
public void setPercent(double percentOutput, double feedForward) Sets the percent output of the motor from between 1.0 (full forward) and -1.0 (full reverse), where 0.0 is full stop.- Parameters:
percentOutput
- The percent output of the motor from [-1, 1].
-
set
public void set(double percentOutput) Sets the percent output of the motor from between 1.0 (full forward) and -1.0 (full reverse), where 0.0 is full stop.- Specified by:
set
in interfaceMotorController
- Parameters:
percentOutput
- The percent output of the motor from [-1, 1].
-
set
public void set(double percentOutput, double feedForward) Sets the percent output of the motor from between 1.0 (full forward) and -1.0 (full reverse), where 0.0 is full stop.- Parameters:
percentOutput
- The percent output of the motor from [-1, 1].feedForward
- The feed forward value to use (0-12v).
-
disable
public void disable()- Specified by:
disable
in interfaceMotorController
-
stopMotor
public void stopMotor()- Specified by:
stopMotor
in interfaceMotorController
-
setPosition
public void setPosition(double targetPosition, double feedForward) Drives the motor towards the given position using the configured PID controller.- Parameters:
targetPosition
- The position to target.feedForward
- The feed forward value to use (0-12v).`
-
setPosition
public void setPosition(double targetPosition) Drives the motor towards the given position using the configured PID controller.- Parameters:
targetPosition
- The position to target.
-
setPositionInternal
public void setPositionInternal(double targetPosition, double feedForward) Sets the target position using the internal encoder with feed forward.- Parameters:
targetPosition
- The target position to set.feedForward
- The feed forward value to set (0-12v).
-
setPositionInternal
public void setPositionInternal(double targetPosition) Sets the target position using the internal encoder.- Parameters:
targetPosition
- The target position to set.
-
setPositionQuad
public void setPositionQuad(double targetPosition, double feedForward) Sets the target position using the quadrature encoder with feed forward.- Parameters:
targetPosition
- The target position to set.feedForward
- The feed forward value to set (0-12v).
-
setPositionQuad
public void setPositionQuad(double targetPosition) Sets the target position using the quadrature encoder.- Parameters:
targetPosition
- The target position to set.
-
setPositionAbs
public void setPositionAbs(double targetPosition, double feedForward) Sets the target position using the absolute encoder.- Parameters:
targetPosition
- The target position to set.feedForward
- The feed forward value to set (0-12v).
-
setPositionAbs
public void setPositionAbs(double targetPosition) Sets the target position using the absolute encoder.- Parameters:
targetPosition
- The target position to set.
-
setVelocity
public void setVelocity(double targetVelocity, double feedForward) Drives the motor at the given velocity using the configured PID controller.- Parameters:
targetVelocity
- The velocity to target.feedForward
- The feed forward value to use (0-12v).
-
setVelocity
public void setVelocity(double targetVelocity) Drives the motor at the given velocity using the configured PID controller.- Parameters:
targetVelocity
- The velocity to target.
-
setVelocityInternal
public void setVelocityInternal(double targetVelocity, double feedForward) Sets the target velocity using the internal encoder with feed forward.- Parameters:
targetVelocity
- The target velocity to set.feedForward
- The feed forward value to set (0-12v).
-
setVelocityInternal
public void setVelocityInternal(double targetVelocity) Sets the target velocity using the internal encoder.- Parameters:
targetVelocity
- The target velocity to set.
-
setVelocityQuad
public void setVelocityQuad(double targetVelocity, double feedForward) Sets the target velocity using the quadrature encoder with feed forward.- Parameters:
targetVelocity
- The target velocity to set.feedForward
- The feed forward value to set (0-12v).
-
setVelocityQuad
public void setVelocityQuad(double targetVelocity) Sets the target velocity using the quadrature encoder.- Parameters:
targetVelocity
- The target velocity to set.
-
follow
public void follow(int followerID) Drives the motor using the parameters of the configured followed motor controller. Follower mode is dependant on the fault status frame. The slower that frame is sent, the slower the follower motor will update.- Parameters:
followerID
- The target ID to follow.
-
updateStatusNT
public void updateStatusNT()Calls every status/feedback getter that has a network table entry -
getPosition
public double getPosition()Gets the encoder position measurement.- Returns:
- The position measurement.
-
getPositionInternal
public double getPositionInternal()Gets the internal encoder position.- Returns:
- The internal encoder position.
-
getPositionQuad
public double getPositionQuad()Gets the quadrature encoder position.- Returns:
- The quadrature encoder position.
-
getPositionAbs
public double getPositionAbs()Gets the absolute encoder position.- Returns:
- The absolute encoder position.
-
getVelocity
public double getVelocity()Gets the encoder velocity measurement.- Returns:
- The velocity measurement.
-
getVelocityInternal
public double getVelocityInternal()Gets the internal encoder velocity.- Returns:
- The internal encoder velocity.
-
getVelocityQuad
public double getVelocityQuad()Gets the quadrature encoder velocity.- Returns:
- The quadrature encoder velocity.
-
getVoltage
public double getVoltage()Gets the voltage measurement.- Returns:
- The voltage measurement in volts.
-
getIOState
public boolean[] getIOState()Gets the IO state of the motor controller.Each element in the array corresponds to a specific I/O signal:
- Index 0: FWD_LIMIT -
true
if the forward limit sensor is active. - Index 1: QUAD_INDEX -
true
if the quad index sensor is active. - Index 2: REV_LIMIT -
true
if the reverse limit sensor is active. - Index 3: ALT_QUAD_INDEX -
true
if the alternate quad index sensor is active. - Index 4: QUAD_A -
true
if the QUAD_A signal is high. - Index 5: QUAD_B -
true
if the QUAD_B signal is high.
- Returns:
- The array containing the IO state.
- Index 0: FWD_LIMIT -
-
getReverseLimit
Gets the reverse limit switch state.- Returns:
- The reverse limit switch state.
-
getForwardLimit
Gets the forward limit switch state.- Returns:
- The forward limit switch state.
-
getQuadIndex
Gets the Quadrature encoder index state.- Returns:
- The Quadrature encoder index state.
-
getQuadA
Gets the Quadrature encoder A state.- Returns:
- The Quadrature encoder A state.
-
getQuadB
Gets the Quadrature encoder B state.- Returns:
- The Quadrature encoder B state.
-
getAltQuadIndex
Gets the alterante quadrature index state.- Returns:
- The alternate quadrature index state.
-
getStatorCurrent
public double getStatorCurrent()Gets the stator current measurement.- Returns:
- The stator current measurement.
-
getSupplyCurrent
public double getSupplyCurrent()Gets the supply current measurement.- Returns:
- The supply current measurement.
-
getTemperature
public double getTemperature()Gets the temperature of the motor controller.- Returns:
- The temperature of the motor controller.
-
getSetPoint
public double getSetPoint()Returns the last set point specified by the user in any control method. Percent output set point becomes zero.- Returns:
- The current set point.
-
get
public double get()Returns the last set point specified by the user in any control method. Percent output set point becomes zero.- Specified by:
get
in interfaceMotorController
- Returns:
- The current set point.
-
getClosedLoopError
public double getClosedLoopError()Returns the error to the last set point that was input, based on last used control mode (position or velocity), and encoder type (internal or quaderature).- Returns:
- The closed loop error.
-
setEncoderPosition
public void setEncoderPosition(double position) Sets the encoder position.- Parameters:
position
- The position to set the encoder to.
-
factoryReset
public void factoryReset()Factory resets the motor controller to its default state. -
close
- Specified by:
close
in interfaceAutoCloseable
- Throws:
Exception
-
updateStatusNTGlobal
public static void updateStatusNTGlobal()Updates the network tables for all ThriftyNova motors. -
getID
public int getID()Returns the CAN ID the device was initialized with.- Returns:
- The device's CAN ID.
-
receiveStatusFrame
protected int receiveStatusFrame(int index, int startByte, int endByte) -
receiveStatusFrameByBit
protected int receiveStatusFrameByBit(int index, int startBit, int endBit) -
logNT
-
logNT
-
logNT
-
logNT
-