From e21a751aaad6e3ca003108b08a014daf74747564 Mon Sep 17 00:00:00 2001 From: mixxlto Date: Mon, 9 Feb 2026 15:52:27 -0800 Subject: [PATCH] a --- .../java/frc/robot/constants/IdConstants.java | 5 ++++ .../frc/robot/subsystems/intake/Intake.java | 5 ++-- .../subsystems/intake/IntakeConstants.java | 23 +++++++++++++++++-- .../frc/robot/subsystems/intake/IntakeIO.java | 1 - 4 files changed, 28 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index 19c5838..ad7ef14 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -36,4 +36,9 @@ public class IdConstants { // Spindexer 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_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 9835af3..d72340f 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -28,11 +28,10 @@ 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); + 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); double basePower; double flyWheelPower; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index c4b0c5d..7df91ad 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,5 +1,24 @@ 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 MASS = 1.5753263; // kilograms: mass of arm specifically + public static final double CENTER_OF_MASS_LENGTH = 0.199608903192622; // meters + public static final double LENGTH = 0.245750; // meters + + public static final double MOI = 0.0644; // kg*m^2 + 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; + 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); + +} \ 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 e72793b..01eb981 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -8,7 +8,6 @@ public interface IntakeIO { public double measuredAngle = IntakeConstants.START_ANGLE; public double currentAmps = 0.0; public double flyWheelVelocity = 0.0; - } public void updateInputs(); -- 2.39.5