From 3216c4e31c1f1d2bd75dc34b5ee3f1504c599f09 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Mon, 9 Mar 2026 18:19:11 -0400 Subject: [PATCH 1/2] cha cha slide --- src/main/java/frc/robot/Constants.java | 10 ++++++++++ src/main/java/frc/robot/subsystems/Intake.java | 10 ++++++++++ 2 files changed, 20 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c0a54d8..4d77bdb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -61,8 +61,14 @@ 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.Encoder; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.lib.W8.devices.AbsoluteEncoder; +import frc.lib.W8.io.absoluteencoder.AbsoluteEncoderIOCANCoder; import frc.lib.W8.mechanisms.linear.LinearMechanism.LinearMechCharacteristics; import frc.lib.W8.mechanisms.rotary.RotaryMechanism.RotaryMechCharacteristics; import frc.lib.W8.util.Device; @@ -618,6 +624,10 @@ 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..6476285 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; @@ -15,6 +16,7 @@ import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; +import frc.robot.Robot; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; import org.littletonrobotics.junction.Logger; @@ -32,6 +34,8 @@ public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { _rollerIO = rollerIO; _pivotIO = pivotIO; + if (Robot.isReal()) tunePivotPosition(); + simBalls = 0; } @@ -107,6 +111,12 @@ 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)) From 60245004c6fefaae72842b05578d8223bbb3357f Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Mon, 9 Mar 2026 18:20:54 -0400 Subject: [PATCH 2/2] spotless cha cha slide --- src/main/java/frc/robot/Constants.java | 8 +++----- src/main/java/frc/robot/subsystems/Intake.java | 5 ++--- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4d77bdb..582317f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -64,11 +64,8 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DutyCycle; import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import frc.lib.W8.devices.AbsoluteEncoder; -import frc.lib.W8.io.absoluteencoder.AbsoluteEncoderIOCANCoder; import frc.lib.W8.mechanisms.linear.LinearMechanism.LinearMechCharacteristics; import frc.lib.W8.mechanisms.rotary.RotaryMechanism.RotaryMechCharacteristics; import frc.lib.W8.util.Device; @@ -625,8 +622,9 @@ public class IntakePivotConstants { 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); + 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 = diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 6476285..e5476ce 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -16,9 +16,9 @@ import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; -import frc.robot.Robot; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; +import frc.robot.Robot; import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { @@ -111,8 +111,7 @@ private Command setStowAngle(Angle stowAngle) { PIDSlot.SLOT_0)); } - public void tunePivotPosition() - { + public void tunePivotPosition() { System.out.println(IntakePivotConstants.ENCODER1.get()); _pivotIO.setEncoderPosition(Rotations.of(IntakePivotConstants.ENCODER1.get())); }