diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4ae2df3..3190ee7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -149,8 +149,17 @@ public static class SystemConstants { } public static class WinchConstants { - public static final double WINCH_POWER = -0.25; // Adjust this constant as needed, 15% according to Ryker - public static final double WINCH_POWER_BOOST = -0.35; // Adjust this constant as needed, 15% according to Ryker + public static final double WINCH_POWER = -0.25; + public static final double WINCH_POWER_BOOST = -0.35; + + public static final double WINCH_MAX_VEL = 10; // TODO 10 rps cruise velocity + public static final double WINCH_MAX_ACCEL = 80; // TODO + public static final double WINCH_JERK = 0; // TODO + public static final double WINCH_FWD_LIMIT = 20; // TODO + public static final double WINCH_REV_LIMIT = 0; + public static final double WINCH_STATOR_LIMIT = 15; // TODO + public static final double WINCH_SUPPLY_LIMIT = 15; // TODO + } public static class CANIDs { diff --git a/src/main/java/frc/robot/subsystems/WinchSubsystem3.java b/src/main/java/frc/robot/subsystems/WinchSubsystem3.java index e69de29..b4620a2 100644 --- a/src/main/java/frc/robot/subsystems/WinchSubsystem3.java +++ b/src/main/java/frc/robot/subsystems/WinchSubsystem3.java @@ -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 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 \ No newline at end of file