diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 962dedb..f7e73de 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 14; - public static final String GIT_SHA = "eb0d9de8d8342dc2b039ff2c80c2750776fa088f"; - public static final String GIT_DATE = "2026-01-19 19:42:29 MST"; - public static final String GIT_BRANCH = "Autorotate"; - public static final String BUILD_DATE = "2026-01-20 19:19:56 MST"; - public static final long BUILD_UNIX_TIME = 1768961996020L; + public static final int GIT_REVISION = 15; + public static final String GIT_SHA = "ba16f53d6d8155249e7ca767b54fbfa1f17ce68e"; + public static final String GIT_DATE = "2026-01-20 19:23:14 MST"; + public static final String GIT_BRANCH = "Intake-boilerplate"; + public static final String BUILD_DATE = "2026-01-27 18:56:30 MST"; + public static final long BUILD_UNIX_TIME = 1769565390510L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 29e2ba2..1aac079 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -25,20 +25,38 @@ public class Intake extends SubsystemBase { this.m_swervePoseSupplier = swervePoseSupplier; } - public enum FieldZone { - // The robot should aim at the hub - InShootZone, - // The robot should aim towards the wall - AimAtWall, - + public enum IntakeMode { + Up, + Down, } - // Calculate what should be done based off of the position of the robot - // TODO: Implement field zones - public FieldZone getTarget(Pose2d position) { - return FieldZone.InShootZone; + public void setMode(IntakeMode mode) { + switch (mode) { + case Up: + io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + break; + case Down: + io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); + io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); + break; + } } + + // public enum FieldZone { + // // The robot should aim at the hub + // InShootZone, + // // The robot should aim towards the wall + // AimAtWall, + + // } + + // // Calculate what should be done based off of the position of the robot + // // TODO: Implement field zones + // public FieldZone getTarget(Pose2d position) { + // return FieldZone.InShootZone; + // } + @Override public void periodic() { @@ -56,3 +74,5 @@ public class Intake extends SubsystemBase { } } + + diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index f21fe8f..2e3477d 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -1,50 +1,49 @@ package frc4388.robot.subsystems.intake; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import frc4388.utility.status.CanDevice; public class IntakeConstants { // Motor conversions - // public static final double ANGLE_MOTOR_GEAR_RATIO = 1.; - public static final double PITCH_MOTOR_GEAR_RATIO = 1.; - public static final double FLYWHEEL_GEAR_RATIO = 1.; - public static final double FEEDER_INCHES_PER_ROT = 1.; + + public static final double ARM_MOTOR_GEAR_RATIO = 1.; + public static final double ROLLER_MOTOR_GEAR_RATIO = 1.; // Limits // 0 is the forward angle on the robot. // negative is left, positive is right - // public static final Angle ANGLE_LIMIT_LEFT = Degrees.of(-180); - // public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180); + public static final Angle ARM_LIMIT_LOWER = Degrees.of(-180); + public static final Angle ARM_LIMIT_UPPER = Degrees.of(180); + public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(0.0); + + public static final Slot0Configs ARM_PID = new Slot0Configs() + .withKP(2.0) + .withKI(0.0) + .withKD(10.0); + + public static final Slot1Configs ROLLER_PID = new Slot1Configs() + .withKP(2.0) + .withKI(0.0) + .withKD(10.0); // 0 is paralell to the ground, 90 is directly up - public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); - public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); + // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); // Motor configs - // public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); - - public static final class IDs { - public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); - } - - public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? @@ -54,17 +53,21 @@ public class IntakeConstants { .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(40) // TODO: tune??? - .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny - .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - ); - public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration() + + public static final class IDs { + public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); + } + // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 35d82e1..92ba274 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -16,27 +16,27 @@ import edu.wpi.first.units.measure.LinearVelocity; public interface IntakeIO { @AutoLog public class IntakeState { - // Angle IntakeAngle = Rotations.of(0); - // Angle IntakeTargetAngle = Rotations.of(0); - // Current angleMotorCurrent = Amps.of(0); + Angle armAngle = Rotations.of(0); + Angle armTargetAngle = Rotations.of(0); + Current armMotorCurrent = Amps.of(0); - // Angle IntakePitch = Rotations.of(0); - // Angle IntakeTargetPitch = Rotations.of(0); + // Angle shooterPitch = Rotations.of(0); + // Angle shooterTargetPitch = Rotations.of(0); // Current pitchMotorCurrent = Amps.of(0); - AngularVelocity flywheelVelocity = RotationsPerSecond.of(0); - AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0); - Current flywheelMotorCurrent = Amps.of(0); + AngularVelocity rollerVelocity = RotationsPerSecond.of(0); + AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0); + Current rollerMotorCurrent = Amps.of(0); - LinearVelocity feederVelocity = InchesPerSecond.of(0); - LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); - Current feederMotorCurrent = Amps.of(0); + // LinearVelocity feederVelocity = InchesPerSecond.of(0); + // LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); + // Current feederMotorCurrent = Amps.of(0); } - // public default void setIntakeAngle(IntakeState state, Angle angle) {} - // public default void setIntakePitch(IntakeState state, Angle angle) {} - public default void setFlywheelVelocity(IntakeState state, AngularVelocity angularVelocity) {} - public default void setFeederVelocity(IntakeState state, LinearVelocity linearVelocity) {} + // public default void setShooterAngle(ShooterState state, Angle angle) {} + // public default void setShooterPitch(ShooterState state, Angle angle) {} + public default void setArmAngle(IntakeState state, Angle angle) {} + public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {} public default void updateInputs(IntakeState state) {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index febf17a..c19d527 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -4,35 +4,37 @@ import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.configurable.ConfigurableDouble; public class IntakeReal implements IntakeIO { - // TalonFX m_angleMotor; - // TalonFX m_pitchMotor; - TalonFX m_flywheelMotor; - TalonFX m_feederMotor; + + TalonFX m_armMotor; + TalonFX m_rollerMotor; public IntakeReal( - // TalonFX angleMotor, - // TalonFX pitchMotor, - TalonFX flywheelMotor, - TalonFX feederMotor + + TalonFX armMotor, + TalonFX rollerMotor ) { // m_angleMotor = angleMotor; // m_pitchMotor = pitchMotor; - m_flywheelMotor = flywheelMotor; - m_feederMotor = feederMotor; + m_armMotor = armMotor; + m_rollerMotor = rollerMotor; // Apply the configs - // m_angleMotor.getConfigurator().apply(IntakeConstants.ANGLE_MOTOR_CONFIG); - // m_pitchMotor.getConfigurator().apply(IntakeConstants.PITCH_MOTOR_CONFIG); - m_flywheelMotor.getConfigurator().apply(IntakeConstants.FLYWHEEL_MOTOR_CONFIG); - m_feederMotor.getConfigurator().apply(IntakeConstants.FEEDER_MOTOR_CONFIG); + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); } private Angle clampAng(Angle x, Angle min, Angle max){ @@ -45,67 +47,42 @@ public class IntakeReal implements IntakeIO { } } - // // TODO: Test - // @Override - // public void setIntakeAngle(IntakeState state, Angle angle) { - // state.IntakeTargetAngle = angle; - // // Assume that the angle is always accurate, because I think we will use a shaft encoder - // // Assume that 0 degrees = forwards. Might need an offset here - // Angle boundedAngle = clampAng(angle, IntakeConstants.ANGLE_LIMIT_LEFT, IntakeConstants.ANGLE_LIMIT_RIGHT); - // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - // double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.ANGLE_MOTOR_GEAR_RATIO; - // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - // m_angleMotor.setControl(posRequest); - // } - - - // TODO: Test - // @Override - // public void setIntakePitch(IntakeState state, Angle angle) { - // state.IntakeTargetPitch = angle; - // // TODO: Test - // // This assumes that the 0 is paralell to the ground. Might need an offset here - - - // Angle boundedAngle = clampAng(angle, IntakeConstants.PITCH_LIMIT_UPPER, IntakeConstants.PITCH_LIMIT_LOWER); - // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - // double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.PITCH_MOTOR_GEAR_RATIO; - // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - // m_pitchMotor.setControl(posRequest); - // } @Override - public void setFlywheelVelocity(IntakeState state, AngularVelocity angularVelocity) { - state.flywheelTargetVelocity = angularVelocity; + public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) { + state.rollerTargetVelocity = angularVelocity; // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) - double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.FLYWHEEL_GEAR_RATIO; + double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.ROLLER_MOTOR_GEAR_RATIO; VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velocity); + m_rollerMotor.setControl(velocity); } @Override - public void setFeederVelocity(IntakeState state, LinearVelocity linearVelocity) { - state.feederTargetVelocity = linearVelocity; - // (IN / SEC) * (ROT / IN) = (ROT / SEC) - double motorSpeed = linearVelocity.in(InchesPerSecond) / IntakeConstants.FEEDER_INCHES_PER_ROT; - VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velRequest); + public void setArmAngle(IntakeState state, Angle angle) { + state.armTargetAngle = angle; + // Assume that the angle is always accurate, because I think we will use a shaft encoder + // Assume that 0 degrees = forwards. Might need an offset here + + Angle boundedAngle = clampAng(angle, IntakeConstants.ARM_LIMIT_LOWER, IntakeConstants.ARM_LIMIT_UPPER); + // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT + double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.ARM_MOTOR_GEAR_RATIO; + PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); + m_armMotor.setControl(posRequest); } @Override public void updateInputs(IntakeState state) { - // state.IntakeAngle = m_angleMotor.getPosition().getValue().times(IntakeConstants.ANGLE_MOTOR_GEAR_RATIO); - // state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue(); + state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue(); - // state.IntakePitch = m_pitchMotor.getPosition().getValue().times(IntakeConstants.PITCH_MOTOR_GEAR_RATIO); + // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); - state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue(); - state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue(); + // state.armAngle = m_armMotor.getPosition().getValue(); + // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); - state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * IntakeConstants.FEEDER_INCHES_PER_ROT); - state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue(); + state.rollerVelocity = m_rollerMotor.getVelocity().getValue(); + state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); } - }