-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Update winch constants and configurations
- Loading branch information
Showing
2 changed files
with
122 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,111 @@ | ||
| package frc.robot.subsystems; | ||
|
|
||
| import com.ctre.phoenix.motorcontrol.ControlMode; | ||
| import com.ctre.phoenix6.StatusSignal; | ||
| import com.ctre.phoenix6.configs.CurrentLimitsConfigs; | ||
| import com.ctre.phoenix6.configs.MotionMagicConfigs; | ||
| import com.ctre.phoenix6.configs.Slot0Configs; | ||
| import com.ctre.phoenix6.configs.Slot1Configs; | ||
| import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; | ||
| import com.ctre.phoenix6.configs.TalonFXConfiguration; | ||
| import com.ctre.phoenix6.controls.MotionMagicVoltage; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.signals.GravityTypeValue; | ||
| import frc.robot.Constants.CANIDs; | ||
| import frc.robot.Constants.WinchConstants; | ||
| import frc.robot.Constants.WinchConstants.*; | ||
|
|
||
|
|
||
| public class WinchSubsystem3 { | ||
|
|
||
| private TalonFX m_winch; | ||
|
|
||
| // class member variable, Initializes to position 0 | ||
| final MotionMagicVoltage m_winchPose = new MotionMagicVoltage(0); | ||
|
|
||
| public WinchSubsystem3() { | ||
|
|
||
| // Initialize the motor in the constructor with the motor ID and optional canbus | ||
| m_winch = new TalonFX(CANIDs.WINCH_ID); | ||
|
|
||
| /* | ||
| * any unmodified configs in a configuration object are *automatically* | ||
| * factory-defaulted; | ||
| * user can perform a full factory default by passing a new device configuration | ||
| * object. | ||
| * This line should do a full factory reset...? | ||
| */ | ||
| m_winch.getConfigurator().apply(new TalonFXConfiguration()); | ||
|
|
||
| /* Gains or configuration of winch motor for config slot 0 */ | ||
| var winchGains0 = new Slot0Configs(); | ||
| winchGains0.GravityType = GravityTypeValue.Arm_Cosine; /* .Elevator_Static | .Arm_Cosine */ | ||
| m_winch.setInverted(true); // Set to true if you want to invert the motor direction | ||
| winchGains0.kP = 0.50; /* Proportional Gain */ | ||
| winchGains0.kI = 0.00; /* Integral Gain */ | ||
| winchGains0.kD = 0.00; /* Derivative Gain */ | ||
| winchGains0.kV = 0.00; /* Velocity Feed Forward Gain */ | ||
| winchGains0.kS = 0.00; /* | ||
| * Static Feed Forward Gain // NOTE, not MY notes Approximately 0.25V to get the | ||
| * mechanism moving | ||
| */ | ||
| winchGains0.kA = 0.00; /* Acceleration Feedforward */ | ||
| winchGains0.kG = 0.00; /* Gravity Feedfoward */ | ||
|
|
||
| // set Motion Magic settings | ||
| var winchMotionMagic0 = new MotionMagicConfigs(); | ||
| winchMotionMagic0.MotionMagicCruiseVelocity = WinchConstants.WINCH_MAX_VEL; // 80 rps cruise velocity // | ||
| // FIMXE changed for safety | ||
| // testing | ||
| winchMotionMagic0.MotionMagicAcceleration = WinchConstants.WINCH_MAX_ACCEL; // 160 rps/s acceleration (0.5 | ||
| // seconds) // FIMXE changed for | ||
| // safety testing | ||
| winchMotionMagic0.MotionMagicJerk = WinchConstants.WINCH_JERK; // 1600 rps/s^2 jerk (0.1 seconds) | ||
|
|
||
| var winchSoftLimit0 = new SoftwareLimitSwitchConfigs(); | ||
| winchSoftLimit0.ForwardSoftLimitEnable = true; | ||
| winchSoftLimit0.ForwardSoftLimitThreshold = WinchConstants.WINCH_FWD_LIMIT; | ||
| winchSoftLimit0.ReverseSoftLimitEnable = true; | ||
| winchSoftLimit0.ReverseSoftLimitThreshold = WinchConstants.WINCH_REV_LIMIT; | ||
|
|
||
| var winchCurrent0 = new CurrentLimitsConfigs(); | ||
| winchCurrent0.StatorCurrentLimitEnable = true; | ||
| winchCurrent0.StatorCurrentLimit = WinchConstants.WINCH_STATOR_LIMIT; | ||
| winchCurrent0.SupplyCurrentLimitEnable = true; | ||
| winchCurrent0.SupplyCurrentLimit = WinchConstants.WINCH_SUPPLY_LIMIT; | ||
|
|
||
| /* | ||
| * Long form (better for my learning): Applies gains with an optional 50 ms | ||
| * timeout (I think) | ||
| */ | ||
| m_winch.getConfigurator().apply(winchGains0, 0.050); | ||
| m_winch.getConfigurator().apply(winchMotionMagic0, 0.050); | ||
| m_winch.getConfigurator().apply(winchSoftLimit0, 0.050); | ||
| m_winch.getConfigurator().apply(winchCurrent0, 0.050); | ||
|
|
||
| /* | ||
| * Send info about the winch to the Shuffleboard | ||
| * Defaults to percent output | ||
| * Only needed for diagnostics | ||
| */ | ||
| // Shuffleboard.getTab("winch").add("winch Output", m_winch); | ||
|
|
||
| } | ||
|
|
||
| public StatusSignal<Double> getWinchPos() { | ||
| return m_winch.getPosition(); | ||
| } | ||
|
|
||
| public void setWinchPose(double winchPose) { | ||
| m_winch.setControl(m_winchPose.withPosition(winchPose)); | ||
| } | ||
|
|
||
| // FIXME | ||
| /* | ||
| public void moveWinchIncrementally(int winchIncrement) { | ||
| int currentWinchPose = getWinchPos(); | ||
| m_winch.setControl(m_winchPose.withPosition(currentWinchPose + winchIncrement)); | ||
| } | ||
| */ | ||
|
|
||
| } // end of class WinchSubsystem3 |