diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c6cb8e0..5fe82b3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -67,6 +67,10 @@ public static final class ShooterConstants { new Constraints(kFlywheelVelocity, kFlywheelAcceleration)); } + public static final class IntakeConstants { + public static final double kPivotGearRatio = 36.0 / 16; + } + public static final class Ports { public static final int kShooterPivotLeader = 0; public static final int kShooterPivotFollower = 0; diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIO.java b/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIO.java new file mode 100644 index 0000000..30f1d26 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIO.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.intake.pivot; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface IntakePivotIO { + public static class IntakePivotInputs { + public double curAngle; + public double[] curVelocity; + public double[] curVoltage; + public double[] curAmps; + } + + public void setVoltage(double voltage); + + public Rotation2d getCurrentAngle(); +} diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIONeo.java b/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIONeo.java new file mode 100644 index 0000000..21b3b73 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/IntakePivotIONeo.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.intake.pivot; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; + +import frc.robot.Constants.IntakeConstants; + +public class IntakePivotIONeo implements IntakePivotIO { + private CANSparkMax m_motor; + private AbsoluteEncoder m_encoder; + + public IntakePivotIONeo(int port) { + m_motor = new CANSparkMax(port, CANSparkMax.MotorType.kBrushless); + m_encoder = m_motor.getAbsoluteEncoder(); + m_encoder.setPositionConversionFactor(IntakeConstants.kPivotGearRatio); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java new file mode 100644 index 0000000..4aeaf47 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.intake.roller; + +import org.littletonrobotics.junction.AutoLog; + +import frc.lib.advantagekit.LoggedIO; + +public interface RollerIO extends LoggedIO { + @AutoLog + public static class RollerInputs { + public double curVelocity; + public double curVoltage; + public double curAmps; + } + + public void setVoltage(double voltage); +} diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOKraken.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOKraken.java new file mode 100644 index 0000000..abb202b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOKraken.java @@ -0,0 +1,39 @@ +package frc.robot.subsystems.intake.roller; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.util.Units; + +public class RollerIOKraken implements RollerIO { + private TalonFX m_motor; + private StatusSignal m_voltageSignal; + private StatusSignal m_currentSignal; + private StatusSignal m_velocitySignal; + + public RollerIOKraken(int port) { + m_motor = new TalonFX(port); + m_voltageSignal = m_motor.getMotorVoltage(); + m_currentSignal = m_motor.getSupplyCurrent(); + m_velocitySignal = m_motor.getVelocity(); + } + + @Override + public void updateInputs(RollerInputs inputs) { + BaseStatusSignal.refreshAll( + m_voltageSignal, + m_currentSignal, + m_velocitySignal + ); + + inputs.curVoltage = m_voltageSignal.getValueAsDouble(); + inputs.curAmps = m_currentSignal.getValueAsDouble(); + inputs.curVelocity = Units.rotationsToRadians(m_velocitySignal.getValueAsDouble()); + } + + @Override + public void setVoltage(double voltage) { + m_motor.setVoltage(voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java new file mode 100644 index 0000000..fae079f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems.intake.roller; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class RollerIOSim implements RollerIO { + private DCMotorSim m_sim; + private double m_voltage; + + public RollerIOSim() { + m_sim = new DCMotorSim( + DCMotor.getKrakenX60(1), + 3, + 1 + ); + m_voltage = 0.0; + } + + @Override + public void updateInputs(RollerInputs inputs) { + m_sim.update(0.02); + + inputs.curVoltage = m_voltage; + inputs.curAmps = m_sim.getCurrentDrawAmps(); + inputs.curVelocity = m_sim.getAngularVelocityRadPerSec(); + } + + @Override + public void setVoltage(double voltage) { + m_sim.setInputVoltage(voltage); + m_voltage = voltage; + } +}