From 9021f480beaf35912eb54a845ec87522fef5337a Mon Sep 17 00:00:00 2001 From: Shatcar Date: Mon, 30 Mar 2026 18:57:14 -0600 Subject: [PATCH] Intake --- .gitignore | 1 + .../java/frc4388/robot/RobotContainer.java | 18 +++- .../robot/constants/BuildConstants.java | 14 +-- .../robot/subsystems/intake/Intake.java | 2 +- .../subsystems/intake/IntakeConstants.java | 16 ++- .../robot/subsystems/intake/IntakeIO.java | 2 + .../robot/subsystems/intake/IntakeReal.java | 3 + .../utility/compute/JankCoder copy.java.old | 100 ++++++++++++++++++ .../frc4388/utility/compute/JankCoder.java | 27 ++--- .../configurable/ConfigurableDouble.java | 4 + 10 files changed, 159 insertions(+), 28 deletions(-) create mode 100644 src/main/java/frc4388/utility/compute/JankCoder copy.java.old diff --git a/.gitignore b/.gitignore index 9a9ca7b..ffe7811 100644 --- a/.gitignore +++ b/.gitignore @@ -185,3 +185,4 @@ compile_commands.json # Eclipse generated file for annotation processors .factorypath +src/main/java/frc4388/utility/compute/JankCoder copy.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index be215b1..5c0977d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -378,11 +378,14 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Retracting); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); })); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.ExtendingIdle); })) @@ -390,13 +393,24 @@ public class RobotContainer { m_robotIntake.setMode(IntakeMode.Idle); })); - new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270) .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Retracting); })) .onFalse(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); })); + + + new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Bouncing); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + })); + + // .onFalse(new InstantCommand(() -> { // m_robotIntake.setMode(IntakeMode.Idle); // })); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 65c2b66..7f3e6b2 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,15 +5,15 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; + public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 189; - public static final String GIT_SHA = "65c76aca95de81f342df7ff26a77e18856f22a83"; - public static final String GIT_DATE = "2026-03-28 15:56:54 MDT"; + public static final int GIT_REVISION = 190; + public static final String GIT_SHA = "3fe5d0e5a1cfe24710b755b489fa61255e307095"; + public static final String GIT_DATE = "2026-03-30 16:15:43 MDT"; public static final String GIT_BRANCH = "New-Intake"; - public static final String BUILD_DATE = "2026-03-30 15:59:11 MDT"; - public static final long BUILD_UNIX_TIME = 1774907951058L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-03-30 18:15:12 MDT"; + public static final long BUILD_UNIX_TIME = 1774916112363L; + 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 2059e6d..a837396 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -115,7 +115,7 @@ public class Intake extends SubsystemBase { case ExtendingRolling: io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); + io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); break; case Retracting: diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 59b6fba..238786d 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -4,6 +4,7 @@ import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.LimitSwitchConfig.Behavior; import com.revrobotics.spark.config.LimitSwitchConfig.Type; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; @@ -13,7 +14,7 @@ public class IntakeConstants { public static final double ARM_MOTOR_GEAR_RATIO = 125; public static final double ROLLER_MOTOR_GEAR_RATIO = 3; - public static final double ARM_ENCODER_OFFSET = -0.466; + public static final ConfigurableDouble ARM_ENCODER_OFFSET = new ConfigurableDouble("Arm Encoder Offset", 0); @@ -42,11 +43,11 @@ public class IntakeConstants { // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.); - public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.8); - public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.1); - public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.1); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.5); + public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.5); + public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.5); - public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 2.0); + public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 1.17); public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .80); public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .40); @@ -90,6 +91,11 @@ public class IntakeConstants { .reverseLimitSwitchType(Type.kNormallyClosed) .reverseLimitSwitchPosition(0) .reverseLimitSwitchTriggerBehavior(Behavior.kStopMovingMotorAndSetPosition); + + ARM_MOTOR_CONFIG.idleMode(IdleMode.kBrake); + + + ROLELR_MOTOR_CONFIG.idleMode(IdleMode.kCoast); } // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 8a40309..706a37f 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -20,6 +20,8 @@ public interface IntakeIO { boolean retractedLimitSwitch = false; boolean extendedSoftLimit = false; boolean retractedSoftLimit = false; + + boolean encoderConnected = false; Angle intakeEncoder = Rotations.of(0); Angle armAngle = Rotations.of(0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 288b25a..1ccfd72 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -119,6 +119,8 @@ public class IntakeReal implements IntakeIO { state.extendedSoftLimit = extendedLimit(); state.intakeEncoder = m_encoder.getRotations(); + state.encoderConnected = m_encoder.isConnected(); + state.retractedLimitSwitch = m_armMotor.getReverseLimitSwitch().isPressed(); if(state.retractedLimitSwitch) { @@ -128,6 +130,7 @@ public class IntakeReal implements IntakeIO { @Override public void updateGains() { + m_encoder.loadRotations(); // IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get(); // IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get(); diff --git a/src/main/java/frc4388/utility/compute/JankCoder copy.java.old b/src/main/java/frc4388/utility/compute/JankCoder copy.java.old new file mode 100644 index 0000000..8abe01f --- /dev/null +++ b/src/main/java/frc4388/utility/compute/JankCoder copy.java.old @@ -0,0 +1,100 @@ +package frc4388.utility.compute; + +import static edu.wpi.first.units.Units.Rotations; + +import java.io.FileInputStream; +import java.io.FileOutputStream; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +public class JankCoder { + DutyCycleEncoder m_encoder; + + boolean loaded_rotations = false; + double lastRotation = 0; + double offset = 0; + + public JankCoder(int dio_channel) { + m_encoder = new DutyCycleEncoder(dio_channel); + loadRotations(); + } + public JankCoder(int dio_channel, double _offset) { + m_encoder = new DutyCycleEncoder(dio_channel); + loadRotations(); + } + + public void update() { + if(!m_encoder.isConnected()) { + return; + } + + if(!loaded_rotations) { + loadRotations(); + } else { + double curRot = m_encoder.get(); + if(lastRotation - curRot > 0.5) { + offset += 1; + saveRotations(); + } else if (curRot - lastRotation > 0.5) { + offset -= 1; + saveRotations(); + } + + lastRotation = curRot; + } + } + + public double get() { + return (double) offset + m_encoder.get(); + } + + public Angle getRotations() { + return Rotations.of(get()); + } + + public int getRotationCount() { + return (int) offset; + } + + public void resetRotation() { + setRotation(0); + } + + public void setRotation(double rotation) { + offset = rotation - m_encoder.get(); + saveRotations(); + } + + private void saveRotations() { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + stream.write(DataUtils.doubleToByteArray(offset)); + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("ENCODER: Unable to write to trim file `" + m_encoder.getSourceChannel() + "`!?!"); + } + } + + private boolean loadRotations() { + lastRotation = m_encoder.get(); + this.loaded_rotations = true; + + try (FileInputStream stream = new FileInputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + offset = DataUtils.byteArrayToDouble(stream.readNBytes(4)); + + // clampModify(); + // modified = false; + + // if (fileValue != currentValue) { + // // System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); + // // modified = true; + // } + return true; + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("ENCODER: Unable to read encoder `" + m_encoder.getSourceChannel() + "`, using current value..."); + return false; + } + + } +} diff --git a/src/main/java/frc4388/utility/compute/JankCoder.java b/src/main/java/frc4388/utility/compute/JankCoder.java index e04e3a8..63a23d8 100644 --- a/src/main/java/frc4388/utility/compute/JankCoder.java +++ b/src/main/java/frc4388/utility/compute/JankCoder.java @@ -7,6 +7,7 @@ import java.io.FileOutputStream; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc4388.utility.configurable.ConfigurableDouble; public class JankCoder { DutyCycleEncoder m_encoder; @@ -14,23 +15,18 @@ public class JankCoder { boolean loaded_rotations = false; int rotations = 0; double lastRotation = 0; - final double offset; + final ConfigurableDouble offset; - public JankCoder(int dio_channel) { - this.offset = 0; - m_encoder = new DutyCycleEncoder(dio_channel); - loadRotations(); - } - public JankCoder(int dio_channel, double offset) { + public JankCoder(int dio_channel, ConfigurableDouble offset) { this.offset = offset; m_encoder = new DutyCycleEncoder(dio_channel); loadRotations(); } public void update() { - if(!m_encoder.isConnected()) { - return; - } + // if(!m_encoder.isConnected()) { + // return; + // } if(!loaded_rotations) { loadRotations(); @@ -49,7 +45,7 @@ public class JankCoder { } public double get() { - return (double) rotations + m_encoder.get() + offset; + return (double) rotations + m_encoder.get() + offset.get(); } public Angle getRotations() { @@ -61,6 +57,7 @@ public class JankCoder { } public void resetRotations() { + offset.set(-m_encoder.get()); setRotations(0); } @@ -69,7 +66,11 @@ public class JankCoder { saveRotations(); } - private void saveRotations() { + public boolean isConnected() { + return m_encoder.isConnected(); + } + + public void saveRotations() { try (FileOutputStream stream = new FileOutputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { stream.write(DataUtils.intToByteArray(rotations)); } catch (Exception e) { @@ -78,7 +79,7 @@ public class JankCoder { } } - private boolean loadRotations() { + public boolean loadRotations() { lastRotation = m_encoder.get(); this.loaded_rotations = true; diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java index c0384db..a6023bb 100644 --- a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -20,4 +20,8 @@ public class ConfigurableDouble { public double get() { return SmartDashboard.getNumber(name, defualtValue); } + + public void set(double value) { + SmartDashboard.putNumber(name, value); + } }