From af1dba8088125001fc4ebadc3d883eae8b94c2f7 Mon Sep 17 00:00:00 2001 From: mixxlto Date: Mon, 9 Feb 2026 16:02:59 -0800 Subject: [PATCH] done --- .../java/frc/robot/constants/IdConstants.java | 5 +-- .../frc/robot/subsystems/intake/Intake.java | 36 ++++++++++++------- .../subsystems/intake/IntakeConstants.java | 8 ++--- .../frc/robot/subsystems/intake/IntakeIO.java | 3 +- 4 files changed, 32 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index ad7ef14..0a82dec 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -38,7 +38,8 @@ public class IdConstants { public static final int SPINDEXER_ID = 12; // Intake - public static final int INTAKE_BASE_ID = 13; - public static final int INTAKE_FLYWHEEL_ID = 14; + public static final int INTAKE_BASE_LEFT_ID = 13; + public static final int INTAKE_BASE_RIGHT_ID = 14; + public static final int INTAKE_FLYWHEEL_ID = 15; public static final int INTAKE_ENCODER_ID = 2; } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index d72340f..47363dd 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -31,7 +31,8 @@ import frc.robot.constants.IdConstants; public class Intake extends SubsystemBase implements IntakeIO{ private TalonFX flyWheelMotor = new TalonFX(IdConstants.INTAKE_FLYWHEEL_ID, Constants.SUBSYSTEM_CANIVORE_CAN); - private TalonFX baseMotor = new TalonFX(IdConstants.INTAKE_BASE_ID, Constants.SUBSYSTEM_CANIVORE_CAN); + private TalonFX baseMotorLeft = new TalonFX(IdConstants.INTAKE_BASE_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); + private TalonFX baseMotorRight = new TalonFX(IdConstants.INTAKE_BASE_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_CAN); double basePower; double flyWheelPower; @@ -59,7 +60,7 @@ public class Intake extends SubsystemBase implements IntakeIO{ public Intake() { updateInputs(); - encoderSim = baseMotor.getSimState(); + encoderSim = baseMotorLeft.getSimState(); intakeSim = new SingleJointedArmSim( baseIntakeMotorSim, @@ -71,12 +72,14 @@ public class Intake extends SubsystemBase implements IntakeIO{ Units.degreesToRadians(0), Units.degreesToRadians(360), true, - Units.degreesToRadians(IntakeConstants.START_ANGLE) + Units.degreesToRadians(IntakeConstants.STOW_ANGLE) ); double absoluteAngleDegrees = getAbsoluteEncoderAngle() - IntakeConstants.ABSOLUTE_OFFSET_ANGLE; - baseMotor.setPosition(Units.degreesToRotations(absoluteAngleDegrees * IntakeConstants.PIVOT_GEAR_RATIO)); - baseMotor.setNeutralMode(NeutralModeValue.Coast); + baseMotorLeft.setPosition(Units.degreesToRotations(absoluteAngleDegrees * IntakeConstants.PIVOT_GEAR_RATIO)); + baseMotorLeft.setNeutralMode(NeutralModeValue.Coast); + baseMotorRight.setPosition(Units.degreesToRotations(absoluteAngleDegrees * IntakeConstants.PIVOT_GEAR_RATIO)); + baseMotorRight.setNeutralMode(NeutralModeValue.Coast); TalonFXConfiguration config = new TalonFXConfiguration(); config.Slot0.kS = 0.1; // Static friction compensation (should be >0 if friction exists) @@ -93,14 +96,19 @@ public class Intake extends SubsystemBase implements IntakeIO{ config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - baseMotor.getConfigurator().apply(config); + baseMotorLeft.getConfigurator().apply(config); + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + baseMotorRight.getConfigurator().apply(config); CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); limitConfig.StatorCurrentLimit = 30; // 120 limitConfig.StatorCurrentLimitEnable = true; - baseMotor.getConfigurator().apply(limitConfig); + baseMotorLeft.getConfigurator().apply(limitConfig); + baseMotorRight.getConfigurator().apply(limitConfig); flyWheelMotor.getConfigurator().apply( new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) @@ -115,7 +123,8 @@ public class Intake extends SubsystemBase implements IntakeIO{ 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))); + baseMotorLeft.setControl(voltageRequest.withPosition(Units.degreesToRotations(clampedSetpoint) * IntakeConstants.PIVOT_GEAR_RATIO).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0))); + baseMotorRight.setControl(voltageRequest.withPosition(Units.degreesToRotations(clampedSetpoint) * IntakeConstants.PIVOT_GEAR_RATIO).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0))); } @AutoLogOutput @@ -133,7 +142,7 @@ public class Intake extends SubsystemBase implements IntakeIO{ // if(RobotBase.isSimulation()){ // return position; // } - return inputs.measuredAngle; + return inputs.measuredAngleLeft; } public double getFlyWheelVelocity() { @@ -154,12 +163,12 @@ public class Intake extends SubsystemBase implements IntakeIO{ } public double getAppliedVoltage() { - return baseMotor.getMotorVoltage().getValueAsDouble(); + return baseMotorLeft.getMotorVoltage().getValueAsDouble(); } @Override public void simulationPeriodic() { - double voltsMotor = baseMotor.getMotorVoltage().getValueAsDouble(); + double voltsMotor = baseMotorLeft.getMotorVoltage().getValueAsDouble(); intakeSim.setInputVoltage(voltsMotor); intakeSim.update(Constants.LOOP_TIME); @@ -194,8 +203,9 @@ public class Intake extends SubsystemBase implements IntakeIO{ @Override public void updateInputs() { - inputs.measuredAngle = Units.rotationsToDegrees(baseMotor.getPosition().getValueAsDouble()/ IntakeConstants.PIVOT_GEAR_RATIO); - inputs.currentAmps = baseMotor.getStatorCurrent().getValueAsDouble(); + inputs.measuredAngleLeft = Units.rotationsToDegrees(baseMotorLeft.getPosition().getValueAsDouble()/ IntakeConstants.PIVOT_GEAR_RATIO); + inputs.measuredAngleRight = Units.rotationsToDegrees(baseMotorLeft.getPosition().getValueAsDouble()/ IntakeConstants.PIVOT_GEAR_RATIO); + inputs.currentAmps = baseMotorLeft.getStatorCurrent().getValueAsDouble(); inputs.flyWheelVelocity = Units.rotationsPerMinuteToRadiansPerSecond(flyWheelMotor.getVelocity().getValueAsDouble() * 60); Logger.processInputs("Intake", inputs); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 7df91ad..ed9a578 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.intake; public class IntakeConstants { - public static final double PIVOT_GEAR_RATIO = 1/(18.0*18.0*10.0/54.0/60.0/38.0); //38 + public static final double PIVOT_GEAR_RATIO = 3.0; //38 public static final double MASS = 1.5753263; // kilograms: mass of arm specifically public static final double CENTER_OF_MASS_LENGTH = 0.199608903192622; // meters @@ -11,14 +11,14 @@ public class IntakeConstants { public static final double MAX_VELOCITY = 15.0; // rad/s public static final double MAX_ACCELERATION = 100.0; // rad/s^2 - public static final double START_ANGLE = 101.7539148; + // TODO: Find these + public static final double STOW_ANGLE = 101.7539148; public static final double INTAKE_ANGLE = 142.48; - public static final double STOW_ANGLE = 107.0; //TODO: find this public static final double FLYWHEEL_SPEED = 1.0; //TODO: find this - public static final double ABSOLUTE_OFFSET_ANGLE = (139.1748046875 - START_ANGLE + 30); + public static final double ABSOLUTE_OFFSET_ANGLE = 0.0; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 01eb981..9d0c9a6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -5,7 +5,8 @@ import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { @AutoLog public static class IntakeIOInputs{ - public double measuredAngle = IntakeConstants.START_ANGLE; + public double measuredAngleLeft = IntakeConstants.STOW_ANGLE; + public double measuredAngleRight = IntakeConstants.STOW_ANGLE; public double currentAmps = 0.0; public double flyWheelVelocity = 0.0; } -- 2.39.5