From 9ecaa655dc73d8ede88336c96d73b21d64e49fc3 Mon Sep 17 00:00:00 2001 From: mixxlto Date: Mon, 9 Feb 2026 15:47:36 -0800 Subject: [PATCH] a --- .../frc/robot/subsystems/intake/Intake.java | 204 ++++++++++++++++++ .../subsystems/intake/IntakeConstants.java | 5 + 2 files changed, 209 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..9835af3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,204 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.Logger; + +import org.littletonrobotics.junction.AutoLogOutput; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; +import frc.robot.constants.IntakeConstants; + +public class Intake extends SubsystemBase implements IntakeIO{ + private TalonFX flyWheelMotor = new TalonFX(IdConstants.FLYWHEEL_MOTOR_ID, Constants.SUBSYSTEM_CANIVORE_CAN); + private TalonFX baseMotor = new TalonFX(IdConstants.BASE_MOTOR_ID, Constants.SUBSYSTEM_CANIVORE_CAN); + + double basePower; + double flyWheelPower; + + // Increase kG until the arm just holds position against gravity (no PID). + // Add kS if you notice stiction (motor doesn’t start moving easily). + // Adjust kV to track moving targets more smoothly. + + private final ArmFeedforward feedforward = new ArmFeedforward(0, IntakeConstants.MASS*IntakeConstants.CENTER_OF_MASS_LENGTH/IntakeConstants.PIVOT_GEAR_RATIO/baseIntakeMotorSim.KtNMPerAmp*baseIntakeMotorSim.rOhms, 0); + + private SingleJointedArmSim intakeSim; + private static final DCMotor baseIntakeMotorSim = DCMotor.getKrakenX60(1); + private TalonFXSimState encoderSim; + + private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0.0 * IntakeConstants.PIVOT_GEAR_RATIO); + private double setpoint; + + Mechanism2d mechanism2d = new Mechanism2d(100, 100); + MechanismRoot2d mechanismRoot = mechanism2d.getRoot("pivot", 50, 50); + MechanismLigament2d ligament2d = mechanismRoot.append(new MechanismLigament2d("baseMotor", 25, 0)); + + private final DutyCycleEncoder absoluteEncoder = new DutyCycleEncoder(IdConstants.INTAKE_ENCODER_ID); + + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + public Intake() { + updateInputs(); + encoderSim = baseMotor.getSimState(); + + intakeSim = new SingleJointedArmSim( + baseIntakeMotorSim, + IntakeConstants.PIVOT_GEAR_RATIO, + //moment of inertia + IntakeConstants.MOI, + //length + IntakeConstants.LENGTH, + Units.degreesToRadians(0), + Units.degreesToRadians(360), + true, + Units.degreesToRadians(IntakeConstants.START_ANGLE) + ); + + double absoluteAngleDegrees = getAbsoluteEncoderAngle() - IntakeConstants.ABSOLUTE_OFFSET_ANGLE; + baseMotor.setPosition(Units.degreesToRotations(absoluteAngleDegrees * IntakeConstants.PIVOT_GEAR_RATIO)); + baseMotor.setNeutralMode(NeutralModeValue.Coast); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kS = 0.1; // Static friction compensation (should be >0 if friction exists) + config.Slot0.kG = IntakeConstants.MASS * IntakeConstants.CENTER_OF_MASS_LENGTH * 9.8 / IntakeConstants.PIVOT_GEAR_RATIO; // Gravity compensation + config.Slot0.kV = 0.12; // Velocity gain: 1 rps -> 0.12V + config.Slot0.kA = 0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters) + config.Slot0.kP = Units.radiansToRotations(0.8 * 12); // If position error is 2.5 rotations, apply 12V (0.5 * 2.5 * 12V) + config.Slot0.kI = Units.radiansToRotations(0.00); // Integral term (usually left at 0 for MotionMagic) + config.Slot0.kD = Units.radiansToRotations(0.00 * 12); // Derivative term (used to dampen oscillations) + + MotionMagicConfigs motionMagicConfigs = config.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = Units.radiansToRotations(IntakeConstants.MAX_VELOCITY * IntakeConstants.PIVOT_GEAR_RATIO); + motionMagicConfigs.MotionMagicAcceleration = Units.radiansToRotations(IntakeConstants.MAX_ACCELERATION * IntakeConstants.PIVOT_GEAR_RATIO); + + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + baseMotor.getConfigurator().apply(config); + + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + + limitConfig.StatorCurrentLimit = 30; // 120 + limitConfig.StatorCurrentLimitEnable = true; + + baseMotor.getConfigurator().apply(limitConfig); + + flyWheelMotor.getConfigurator().apply( + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast) + ); + + SmartDashboard.putData("Set intake stow angle", new InstantCommand(() -> setSetpoint(IntakeConstants.STOW_ANGLE))); + SmartDashboard.putData("Set intake down angle", new InstantCommand(() -> setSetpoint(IntakeConstants.INTAKE_ANGLE))); + + SmartDashboard.putNumber("Absolute Encoder Value", getAbsoluteEncoderAngle()); + } + + public void setSetpoint(double setpoint) { + double clampedSetpoint = MathUtil.clamp(setpoint, 90.0, 180.0); + baseMotor.setControl(voltageRequest.withPosition(Units.degreesToRotations(clampedSetpoint) * IntakeConstants.PIVOT_GEAR_RATIO).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0))); + } + + @AutoLogOutput + public double getAbsoluteEncoderAngle() { + double rotations = absoluteEncoder.get(); + double armRotations = rotations / (IntakeConstants.PIVOT_GEAR_RATIO / 18.0); + return Units.rotationsToDegrees(armRotations); + } + + /** + * Gets the angle of the intake + * @return The angle in degrees + */ + public double getAngle() { + // if(RobotBase.isSimulation()){ + // return position; + // } + return inputs.measuredAngle; + } + + public double getFlyWheelVelocity() { + return inputs.flyWheelVelocity; + } + + public boolean atSetpoint() { + return Math.abs(getAngle() - setpoint) < 3.0; + } + + @Override + public void periodic() { + updateInputs(); + + flyWheelMotor.set(flyWheelPower); + + ligament2d.setAngle(getAngle()); + } + + public double getAppliedVoltage() { + return baseMotor.getMotorVoltage().getValueAsDouble(); + } + + @Override + public void simulationPeriodic() { + double voltsMotor = baseMotor.getMotorVoltage().getValueAsDouble(); + intakeSim.setInputVoltage(voltsMotor); + + intakeSim.update(Constants.LOOP_TIME); + + double simAngle = intakeSim.getAngleRads(); + double simRotations = Units.radiansToRotations(simAngle); + double motorRotations = simRotations * IntakeConstants.PIVOT_GEAR_RATIO; + + encoderSim.setRawRotorPosition(motorRotations); + } + + public boolean flyWheelSpinning() { + return flyWheelMotor.getVelocity().getValueAsDouble() > 0; + } + + public void setFlyWheel() { + flyWheelPower = IntakeConstants.FLYWHEEL_SPEED; + } + + public void outtakeFlyWheel() { + flyWheelPower = -IntakeConstants.FLYWHEEL_SPEED; + } + + public void stopFlyWheel(){ + flyWheelPower = 0; + } + + @AutoLogOutput + public double setpointAngle(){ + return setpoint; + } + + @Override + public void updateInputs() { + inputs.measuredAngle = Units.rotationsToDegrees(baseMotor.getPosition().getValueAsDouble()/ IntakeConstants.PIVOT_GEAR_RATIO); + inputs.currentAmps = baseMotor.getStatorCurrent().getValueAsDouble(); + inputs.flyWheelVelocity = Units.rotationsPerMinuteToRadiansPerSecond(flyWheelMotor.getVelocity().getValueAsDouble() * 60); + + Logger.processInputs("Intake", inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..c4b0c5d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.intake; + +public class IntakeConstants { + +} -- 2.39.5