diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c0a54d8..582317f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -61,6 +61,9 @@ import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DutyCycle; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.lib.W8.mechanisms.linear.LinearMechanism.LinearMechCharacteristics; @@ -618,6 +621,11 @@ public class IntakePivotConstants { public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.25); + public static final int ENCODER_CHANNEL = 9; + public static final DutyCycleEncoder ENCODER1 = + new DutyCycleEncoder(new DutyCycle(new DigitalInput(ENCODER_CHANNEL))); + // public static final Encoder ENCODER = new Encoder(ENCODER_CHANNEL, ENCODER_CHANNEL); + // Positional PID public static final Slot0Configs SLOT_0_CONFIG = new Slot0Configs().withKP(100.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 224ea29..e5476ce 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import edu.wpi.first.math.geometry.Pose3d; @@ -17,6 +18,7 @@ import frc.lib.W8.mechanisms.rotary.RotaryMechanism; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; +import frc.robot.Robot; import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { @@ -32,6 +34,8 @@ public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { _rollerIO = rollerIO; _pivotIO = pivotIO; + if (Robot.isReal()) tunePivotPosition(); + simBalls = 0; } @@ -107,6 +111,11 @@ private Command setStowAngle(Angle stowAngle) { PIDSlot.SLOT_0)); } + public void tunePivotPosition() { + System.out.println(IntakePivotConstants.ENCODER1.get()); + _pivotIO.setEncoderPosition(Rotations.of(IntakePivotConstants.ENCODER1.get())); + } + @Override public void periodic() { // if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree))