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 new file mode 100644 index 0000000..1aac079 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -0,0 +1,78 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Rotation; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.status.FaultReporter; + +public class Intake extends SubsystemBase { + IntakeIO io; + IntakeStateAutoLogged state = new IntakeStateAutoLogged(); + + Supplier m_swervePoseSupplier; + + public Intake( + IntakeIO io, + Supplier swervePoseSupplier + ) { + this.io = io; + this.m_swervePoseSupplier = swervePoseSupplier; + } + + public enum IntakeMode { + Up, + Down, + } + + 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() { + + + + // FaultReporter.register(this); // TODO Implement fault reporter + + + Logger.processInputs("Intake", state); + + Pose2d pose = m_swervePoseSupplier.get(); + Angle robotRot = pose.getRotation().getMeasure(); + + io.updateInputs(state); + + } +} + + diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..2e3477d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,80 @@ +package frc4388.robot.subsystems.intake; + +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 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 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); + + // Motor configs + public static final TalonFXConfiguration ARM_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 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??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..92ba274 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,42 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Amps; +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 org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.LinearVelocity; + +public interface IntakeIO { + @AutoLog + public class IntakeState { + Angle armAngle = Rotations.of(0); + Angle armTargetAngle = Rotations.of(0); + Current armMotorCurrent = Amps.of(0); + + // Angle shooterPitch = Rotations.of(0); + // Angle shooterTargetPitch = Rotations.of(0); + // Current pitchMotorCurrent = 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); + } + + // 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 new file mode 100644 index 0000000..1f8b5b3 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -0,0 +1,88 @@ +package frc4388.robot.subsystems.intake; + +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_armMotor; + TalonFX m_rollerMotor; + + public IntakeReal( + + TalonFX armMotor, + TalonFX rollerMotor + ) { + // m_angleMotor = angleMotor; + // m_pitchMotor = pitchMotor; + m_armMotor = armMotor; + m_rollerMotor = rollerMotor; + + // Apply the configs + 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){ + if(x.gt(max)) { + return max; + }else if(x.lt(min)) { + return min; + }else{ + return x; + } + } + + + + @Override + 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.ROLLER_MOTOR_GEAR_RATIO; + VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); + m_rollerMotor.setControl(velocity); + } + + @Override + 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.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue(); + + // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); + // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + + // state.armAngle = m_armMotor.getPosition().getValue(); + // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); + + state.rollerVelocity = m_rollerMotor.getVelocity().getValue(); + state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); + } +} \ No newline at end of file