From c6b0d29acdb24728ed566b25d6434a45fbd963c7 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 25 Mar 2026 11:15:46 -0600 Subject: [PATCH 1/9] Add bounce code --- .../robot/constants/BuildConstants.java | 16 +++---- .../robot/subsystems/intake/Intake.java | 47 ++++++++++++++----- .../subsystems/intake/IntakeConstants.java | 12 +++++ .../robot/subsystems/intake/IntakeIO.java | 2 + 4 files changed, 57 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index b2c6e49..57f5e5e 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"; + public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 182; - public static final String GIT_SHA = "75cab187e2225e51259336e5569f15c41f9169d5"; - public static final String GIT_DATE = "2026-03-21 13:38:34 MDT"; - public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-21 15:45:59 MDT"; - public static final long BUILD_UNIX_TIME = 1774129559544L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 183; + public static final String GIT_SHA = "d639b3076d10f39eb5bc18c42c3dc5e707e991ff"; + public static final String GIT_DATE = "2026-03-21 15:49:44 MDT"; + public static final String GIT_BRANCH = "New-Intake"; + public static final String BUILD_DATE = "2026-03-23 10:56:08 MDT"; + public static final long BUILD_UNIX_TIME = 1774284968618L; + public static final int DIRTY = 0; 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 d293bd8..21782aa 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -1,10 +1,15 @@ package frc4388.robot.subsystems.intake; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.Utils; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,16 +30,23 @@ public class Intake extends SubsystemBase { public enum IntakeMode { Extended, Retracted, - Extending, - Retracting, Idle, - RollerStop + Bouncing } private IntakeMode mode = IntakeMode.Idle; public void setMode(IntakeMode mode) { this.mode = mode; + + switch (mode) { + case Bouncing: + // When bounce is enabled: set the bounce timer + this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD; + break; + default: + break; + } } public IntakeMode getMode() { @@ -78,6 +90,8 @@ public class Intake extends SubsystemBase { io.updateInputs(state); + // getCurrentTime + switch (mode) { case Extended: io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); @@ -87,18 +101,27 @@ public class Intake extends SubsystemBase { io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); io.setRollerOutput(state, 0); break; - case Extending: - io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); - break; - case Retracting: - io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); + case Bouncing: io.setRollerOutput(state, 0); + + if( + state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() && + state.armMotorVelocity.in(RotationsPerSecond) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get() + ) { + this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD; + } + + // Get the time delta from the last bounce time update + double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime; + // Get the percentage through the bounce period (0 output means one half period has passed) + double percentOutput = (currentTime / IntakeConstants.BOUNCE_HALF_PERIOD) * IntakeConstants.INTAKE_BOUNCE_OUTPUT; + // Clamp the output of the motor to some value + percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT); + + io.armOutput(percentOutput); break; case Idle: - io.stopArm(); - break; - case RollerStop: + // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); io.setRollerOutput(state, 0); break; } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index dc82e42..7aa88cc 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -1,11 +1,16 @@ package frc4388.robot.subsystems.intake; +import static edu.wpi.first.units.Units.Amps; +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.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; @@ -16,6 +21,13 @@ public class IntakeConstants { public static final double ROLLER_MOTOR_GEAR_RATIO = 3; + public static final double BOUNCE_HALF_PERIOD = 5.; + public static final double INTAKE_BOUNCE_OUTPUT = 0.2; + public static final double INTAKE_BOUNCE_MAX_OUTPUT = 0.5; + public static final ConfigurableDouble INTAKE_BOUNCE_CURRENT_LIMIT = new ConfigurableDouble("Intake Bounce Current Limit", 20); + public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 20); + + //IDs diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 5f99722..8d1e803 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -15,6 +15,8 @@ import edu.wpi.first.units.measure.Current; public interface IntakeIO { @AutoLog public class IntakeState { + double currentBounceTime = 0; + boolean retractedLimit = false; Angle armAngle = Rotations.of(0); Angle armTargetAngle = Rotations.of(0); From 7eba3d8faa208df9222599c53db1e11da82d7a96 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 25 Mar 2026 11:39:55 -0600 Subject: [PATCH 2/9] Add SparkMax and fix arm mode issues --- .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 5 +- .../robot/constants/BuildConstants.java | 12 +- .../robot/subsystems/intake/Intake.java | 10 ++ .../robot/subsystems/intake/IntakeReal.java | 15 +- vendordeps/REVLib.json | 133 ++++++++++++++++++ 6 files changed, 159 insertions(+), 18 deletions(-) create mode 100644 vendordeps/REVLib.json diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2559999..77ee3f6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -134,7 +134,7 @@ public class RobotContainer { private Command RobotRev = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), IntakeExtended, - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake) ); private Command RobotShootDriving = new SequentialCommandGroup( diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index b3dc909..cc680c3 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -13,6 +13,7 @@ import org.photonvision.PhotonCamera; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.wpilibj.DigitalInput; import frc4388.robot.constants.Constants; @@ -98,7 +99,7 @@ public class RobotMap { //Configure Intake 20,21 TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.CANIVORE_CANBUS); - TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.CANIVORE_CANBUS); + SparkMax roller = new SparkMax(IntakeConstants.ROLLER_ID.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); DigitalInput armLimitSwitch = new DigitalInput(IntakeConstants.ARM_LIMIT_SWITCH_CHANNEL); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); @@ -118,7 +119,7 @@ public class RobotMap { FaultTalonFX.addDevice(shooter2, "Shooter2"); FaultTalonFX.addDevice(indexer, "Indexer"); FaultTalonFX.addDevice(arm, "Arm"); - FaultTalonFX.addDevice(roller, "Roller"); + // FaultTalonFX.addDevice(roller, "Roller"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 57f5e5e..d7cb571 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 183; - public static final String GIT_SHA = "d639b3076d10f39eb5bc18c42c3dc5e707e991ff"; - public static final String GIT_DATE = "2026-03-21 15:49:44 MDT"; + public static final int GIT_REVISION = 184; + public static final String GIT_SHA = "c6b0d29acdb24728ed566b25d6434a45fbd963c7"; + public static final String GIT_DATE = "2026-03-25 11:15:46 MDT"; public static final String GIT_BRANCH = "New-Intake"; - public static final String BUILD_DATE = "2026-03-23 10:56:08 MDT"; - public static final long BUILD_UNIX_TIME = 1774284968618L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-03-25 11:17:58 MDT"; + public static final long BUILD_UNIX_TIME = 1774459078868L; + 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 21782aa..b73cb8f 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -30,6 +30,8 @@ public class Intake extends SubsystemBase { public enum IntakeMode { Extended, Retracted, + Extending, + Retracting, Idle, Bouncing } @@ -101,6 +103,14 @@ public class Intake extends SubsystemBase { io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); io.setRollerOutput(state, 0); break; + case Extending: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get())); + io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); + break; + case Retracting: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get())); + io.setRollerOutput(state, 0); + break; case Bouncing: io.setRollerOutput(state, 0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 820ae3e..1e13f86 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -1,11 +1,13 @@ package frc4388.robot.subsystems.intake; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -17,7 +19,7 @@ import edu.wpi.first.wpilibj.DigitalInput; public class IntakeReal implements IntakeIO { TalonFX m_armMotor; - TalonFX m_rollerMotor; + SparkMax m_rollerMotor; DigitalInput m_armLimitSwitch; PositionDutyCycle armPosition = new PositionDutyCycle(0); @@ -26,7 +28,7 @@ public class IntakeReal implements IntakeIO { public IntakeReal( DigitalInput armLimitSwitch, TalonFX armMotor, - TalonFX rollerMotor + SparkMax rollerMotor ) { // m_angleMotor = angleMotor; // m_pitchMotor = pitchMotor; @@ -37,7 +39,7 @@ public class IntakeReal implements IntakeIO { // Apply the configs m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); - m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); + // m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); armPosition.Slot = 0; // rollerVelocity.Slot = 0; @@ -59,11 +61,6 @@ public class IntakeReal implements IntakeIO { public void setRollerOutput(IntakeState state, double rollerOutput) { state.rollerTargetOutput = rollerOutput; - - if(rollerOutput == 0) { - m_rollerMotor.set(0); - return; - } m_rollerMotor.set(rollerOutput); } @@ -133,7 +130,7 @@ public class IntakeReal implements IntakeIO { state.armAngle = m_armMotor.getPosition().getValue().div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); state.rollerOutput = m_rollerMotor.get(); - state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); + state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent()); state.retractedLimit = !m_armLimitSwitch.get(); state.armMotorVelocity = m_armMotor.getVelocity().getValue(); diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..c6b1b9e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,133 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2026.0.5", + "frcYear": "2026", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2026.0.5" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2026.0.5", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.5", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.5", + "libName": "BackendDriver", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.5", + "libName": "REVLibWpi", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From bc933eba9497302a66c4b01a1a47852530b5b39f Mon Sep 17 00:00:00 2001 From: Shatcar Date: Sat, 28 Mar 2026 09:59:30 -0600 Subject: [PATCH 3/9] Sparkmax for arm motor --- src/main/java/frc4388/robot/RobotMap.java | 8 +- .../robot/constants/BuildConstants.java | 12 +- .../robot/subsystems/intake/Intake.java | 12 +- .../subsystems/intake/IntakeConstants.java | 84 +++++++------- .../robot/subsystems/intake/IntakeIO.java | 2 +- .../robot/subsystems/intake/IntakeReal.java | 104 +++++------------- vendordeps/AdvantageKit.json | 6 +- ...enix6-26.1.0.json => Phoenix6-26.1.3.json} | 62 +++++------ vendordeps/photonlib.json | 12 +- 9 files changed, 131 insertions(+), 171 deletions(-) rename vendordeps/{Phoenix6-26.1.0.json => Phoenix6-26.1.3.json} (92%) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index cc680c3..a9f19e9 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -98,9 +98,9 @@ public class RobotMap { TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.CANIVORE_CANBUS); //Configure Intake 20,21 - TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.CANIVORE_CANBUS); + SparkMax arm = new SparkMax(IntakeConstants.ARM_ID.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); SparkMax roller = new SparkMax(IntakeConstants.ROLLER_ID.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); - DigitalInput armLimitSwitch = new DigitalInput(IntakeConstants.ARM_LIMIT_SWITCH_CHANNEL); + // DigitalInput armLimitSwitch = new DigitalInput(IntakeConstants.ARM_LIMIT_SWITCH_CHANNEL); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); @@ -109,7 +109,7 @@ public class RobotMap { // shooterIO = new ShooterIO() {}; shooterIO = new ShooterReal(shooter1, shooter2, indexer); - intakeIO = new IntakeReal(armLimitSwitch, arm, roller); + intakeIO = new IntakeReal(arm, roller); // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); @@ -118,7 +118,7 @@ public class RobotMap { FaultTalonFX.addDevice(shooter1, "Shooter1"); FaultTalonFX.addDevice(shooter2, "Shooter2"); FaultTalonFX.addDevice(indexer, "Indexer"); - FaultTalonFX.addDevice(arm, "Arm"); + // FaultTalonFX.addDevice(arm, "Arm"); // FaultTalonFX.addDevice(roller, "Roller"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index d7cb571..78ac3f6 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,14 +5,14 @@ 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 = 184; - public static final String GIT_SHA = "c6b0d29acdb24728ed566b25d6434a45fbd963c7"; - public static final String GIT_DATE = "2026-03-25 11:15:46 MDT"; + public static final int GIT_REVISION = 185; + public static final String GIT_SHA = "7eba3d8faa208df9222599c53db1e11da82d7a96"; + public static final String GIT_DATE = "2026-03-25 11:39:55 MDT"; public static final String GIT_BRANCH = "New-Intake"; - public static final String BUILD_DATE = "2026-03-25 11:17:58 MDT"; - public static final long BUILD_UNIX_TIME = 1774459078868L; + public static final String BUILD_DATE = "2026-03-28 09:26:32 MDT"; + public static final long BUILD_UNIX_TIME = 1774711592752L; 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 b73cb8f..0246ddd 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -96,19 +96,19 @@ public class Intake extends SubsystemBase { switch (mode) { case Extended: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); - io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); + // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); + // io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); break; case Retracted: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); - io.setRollerOutput(state, 0); + // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + // io.setRollerOutput(state, 0); break; case Extending: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get())); + io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); break; case Retracting: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get())); + io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); io.setRollerOutput(state, 0); break; case Bouncing: diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 7aa88cc..e82ceea 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -1,16 +1,10 @@ package frc4388.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.RotationsPerSecond; +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.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; @@ -33,7 +27,7 @@ public class IntakeConstants { public static final CanDevice ARM_ID = new CanDevice("ARM", 20); public static final CanDevice ROLLER_ID = new CanDevice("INTAKE_ROLLER", 21); - public static final int ARM_LIMIT_SWITCH_CHANNEL = 9; + // public static final int ARM_LIMIT_SWITCH_CHANNEL = 9; // Limits @@ -46,7 +40,7 @@ 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.1); - public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33); + public static final ConfigurableDouble ARM_ssLIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33); public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.4); public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.4); public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .70); @@ -56,16 +50,16 @@ public class IntakeConstants { // public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); - public static final Slot0Configs ARM_PID = new Slot0Configs() - .withKP(0.08) - .withKI(0.0) - .withKD(0.05); + // public static final Slot0Configs ARM_PID = new Slot0Configs() + // .withKP(0.08) + // .withKI(0.0) + // .withKD(0.05); - public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.08); - public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); - public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05); + // public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.08); + // public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); + // public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05); @@ -75,25 +69,37 @@ public class IntakeConstants { // 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(15) // 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 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 - ); + public static final SparkMaxConfig ARM_MOTOR_CONFIG = new SparkMaxConfig(); + public static final SparkMaxConfig ROLELR_MOTOR_CONFIG = new SparkMaxConfig(); + + static { + ARM_MOTOR_CONFIG.limitSwitch + .reverseLimitSwitchType(Type.kNormallyClosed) + .limitSwitchPositionSensor(FeedbackSensor.kPrimaryEncoder) + .forwardLimitSwitchPosition(0) + .forwardLimitSwitchTriggerBehavior(Behavior.kStopMovingMotorAndSetPosition); + } + + // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(15) // 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 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 index 8d1e803..4308c26 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -23,7 +23,7 @@ public interface IntakeIO { Current armMotorCurrent = Amps.of(0); AngularVelocity armMotorVelocity = RotationsPerSecond.of(0); - AngularAcceleration armMotorAcceleration = RotationsPerSecondPerSecond.of(0); + // AngularAcceleration armMotorAcceleration = RotationsPerSecondPerSecond.of(0); // Angle shooterPitch = Rotations.of(0); // Angle shooterTargetPitch = 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 1e13f86..f170880 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -1,7 +1,10 @@ package frc4388.robot.subsystems.intake; import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.PositionDutyCycle; @@ -15,44 +18,24 @@ import edu.wpi.first.units.measure.Acceleration; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DutyCycleEncoder; public class IntakeReal implements IntakeIO { - TalonFX m_armMotor; + SparkMax m_armMotor; SparkMax m_rollerMotor; - DigitalInput m_armLimitSwitch; - - PositionDutyCycle armPosition = new PositionDutyCycle(0); - DutyCycleOut armPercentOutput = new DutyCycleOut(0); + DutyCycleEncoder encoder; public IntakeReal( - DigitalInput armLimitSwitch, - TalonFX armMotor, + // DigitalInput armLimitSwitch, + SparkMax armMotor, SparkMax rollerMotor ) { // m_angleMotor = angleMotor; // m_pitchMotor = pitchMotor; m_armMotor = armMotor; m_rollerMotor = rollerMotor; - m_armLimitSwitch = armLimitSwitch; - - // Apply the configs - m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); - m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); - // m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); - - armPosition.Slot = 0; - // rollerVelocity.Slot = 0; - } - - 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; - } + // m_armLimitSwitch = armLimitSwitch; } @@ -77,40 +60,14 @@ public class IntakeReal implements IntakeIO { Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - m_armMotor.setControl( - armPosition - .withPosition(motorAngle) - .withLimitReverseMotion(!m_armLimitSwitch.get()) - ); + // m_armMotor.setControl( + // armPosition + // .withPosition(motorAngle) + // .withLimitReverseMotion(!m_armLimitSwitch.get()) + // ); } - @Override - public void testSetArmAngle(IntakeState state, Angle angle){ - state.armTargetAngle = angle; - Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); - - final TrapezoidProfile m_profile = new TrapezoidProfile( - new TrapezoidProfile.Constraints(80, 160) - ); - - // Final target of motorAngle rot, 0 rps - // Convert the Angle to a numeric degree value before creating the profile state - TrapezoidProfile.State m_goal = new TrapezoidProfile.State(motorAngle.in(Rotations), 0); - TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); - - // create a position closed-loop request, voltage output, slot 0 configs - final PositionVoltage m_request = new PositionVoltage(0).withSlot(0); - - // calculate the next profile setpoint - m_setpoint = m_profile.calculate(0.020, m_setpoint, m_goal); - - // send the request to the device - m_request.Position = m_setpoint.position; - m_request.Velocity = m_setpoint.velocity; - m_armMotor.setControl(m_request); - } - @Override public void stopArm(){ m_armMotor.set(0); @@ -119,36 +76,33 @@ public class IntakeReal implements IntakeIO { @Override public void armOutput(double percentOutput){ - m_armMotor.setControl( - armPercentOutput.withOutput(percentOutput) - .withLimitReverseMotion(!m_armLimitSwitch.get()) - ); + m_armMotor.set(percentOutput); } @Override public void updateInputs(IntakeState state) { - state.armAngle = m_armMotor.getPosition().getValue().div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); - state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); + state.armAngle = Rotations.of(m_armMotor.getEncoder().getPosition()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + state.armMotorVelocity = RotationsPerSecond.of(m_armMotor.getEncoder().getVelocity()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + // state.armMotorAcceleration = RotationsPerSecondPerSecond.of(m_armMotor.getEncoder().ge); + state.armMotorCurrent = Amps.of(m_armMotor.getOutputCurrent()); + state.rollerOutput = m_rollerMotor.get(); state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent()); - state.retractedLimit = !m_armLimitSwitch.get(); - - state.armMotorVelocity = m_armMotor.getVelocity().getValue(); - state.armMotorAcceleration = m_armMotor.getAcceleration().getValue(); - if(state.retractedLimit) { - // Set the arm motor to be zero if the limit switch is pressed - m_armMotor.setPosition(0., 0); - } + + // if(state.retractedLimit) { + // // Set the arm motor to be zero if the limit switch is pressed + // m_armMotor.setPosition(0., 0); + // } } @Override public void updateGains() { - IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get(); - IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get(); - IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get(); - m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); + // IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get(); + // IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get(); + // IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get(); + // m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); } } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 162ad66..b4e470d 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.0", + "version": "26.0.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2026", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.0" + "version": "26.0.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.0", + "version": "26.0.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.3.json similarity index 92% rename from vendordeps/Phoenix6-26.1.0.json rename to vendordeps/Phoenix6-26.1.3.json index dc5dc62..d5bc4a2 100644 --- a/vendordeps/Phoenix6-26.1.0.json +++ b/vendordeps/Phoenix6-26.1.3.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.0.json", + "fileName": "Phoenix6-26.1.3.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.3", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.3" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.3", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 9699738..a4ec272 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.3.1", + "version": "v2026.3.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", + "version": "v2026.3.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.3.1", + "version": "v2026.3.2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", + "version": "v2026.3.2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.3.1" + "version": "v2026.3.2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.3.1" + "version": "v2026.3.2" } ] } \ No newline at end of file From aead1c165a26a70b81ce74afad41d0963bd3b4f4 Mon Sep 17 00:00:00 2001 From: Shatcar Date: Sat, 28 Mar 2026 10:21:34 -0600 Subject: [PATCH 4/9] Add fault reporting for SparkMax --- src/main/java/frc4388/robot/RobotMap.java | 5 +- .../frc4388/utility/status/FaultSparkMax.java | 97 +++++++++++++++++++ 2 files changed, 100 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/utility/status/FaultSparkMax.java diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a9f19e9..eba05db 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -37,6 +37,7 @@ import frc4388.robot.subsystems.vision.VisionReal; import frc4388.utility.status.FaultCANCoder; import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; +import frc4388.utility.status.FaultSparkMax; import frc4388.utility.status.FaultTalonFX; /** @@ -118,8 +119,8 @@ public class RobotMap { FaultTalonFX.addDevice(shooter1, "Shooter1"); FaultTalonFX.addDevice(shooter2, "Shooter2"); FaultTalonFX.addDevice(indexer, "Indexer"); - // FaultTalonFX.addDevice(arm, "Arm"); - // FaultTalonFX.addDevice(roller, "Roller"); + FaultSparkMax.addDevice(arm, "Arm"); + FaultSparkMax.addDevice(roller, "Roller"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); diff --git a/src/main/java/frc4388/utility/status/FaultSparkMax.java b/src/main/java/frc4388/utility/status/FaultSparkMax.java new file mode 100644 index 0000000..f9035a9 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultSparkMax.java @@ -0,0 +1,97 @@ +package frc4388.utility.status; + +import com.revrobotics.spark.SparkBase.Faults; +import com.revrobotics.spark.SparkBase.Warnings; +import com.revrobotics.spark.SparkMax; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultSparkMax implements Queryable { + private String name; + private SparkMax motor; + + public static void addDevice(SparkMax motor, String name) { + FaultSparkMax p = new FaultSparkMax(); + + p.name = name; + p.motor = motor; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + s.addReport(ReportLevel.INFO, "Firmware Version: " + motor.getFirmwareString()); + s.addReport(ReportLevel.INFO, "Voltage: " + motor.getBusVoltage()); + s.addReport(ReportLevel.INFO, "Current: " + motor.getOutputCurrent()); + s.addReport(ReportLevel.INFO, "Device temp (C): " + motor.getMotorTemperature()); + s.addReport(ReportLevel.INFO, "Position: " + motor.getEncoder().getPosition()); + s.addReport(ReportLevel.INFO, "Velocity: " + motor.getEncoder().getVelocity()); + s.addReport(ReportLevel.INFO, "Forward hard limit: " + motor.getForwardLimitSwitch().isPressed()); + s.addReport(ReportLevel.INFO, "Reverse hard limit: " + motor.getReverseLimitSwitch().isPressed()); + s.addReport(ReportLevel.INFO, "Forward soft limit: " + motor.getForwardSoftLimit().isReached()); + s.addReport(ReportLevel.INFO, "Reverse soft limit: " + motor.getReverseSoftLimit().isReached()); + + // faults + Faults faults = motor.getFaults(); + if(faults.can) { + s.addReport(ReportLevel.ERROR, "CAN Fault (Joe Johnson)"); + } + if(faults.escEeprom) { + s.addReport(ReportLevel.ERROR, "Escape Eeprom. Cannot write to internal memory (oh god I don't want to think about what this means)"); + } + if(faults.firmware) { + s.addReport(ReportLevel.ERROR, "Firmware Fault"); + } + if(faults.gateDriver) { + s.addReport(ReportLevel.ERROR, "Gate Driver Fault"); + } + if(faults.motorType) { + s.addReport(ReportLevel.ERROR, "Motor type Fault"); + } + if(faults.other) { + s.addReport(ReportLevel.ERROR, "Fault type is 'other'. Hope for the best!"); + } + if(faults.sensor) { + s.addReport(ReportLevel.ERROR, "Sensor fault"); + } + if(faults.temperature) { + s.addReport(ReportLevel.ERROR, "Tempreture fault"); + } + + Warnings warnings = motor.getWarnings(); + if(warnings.brownout) { + s.addReport(ReportLevel.WARNING, "Brownout detected"); + } + if (warnings.escEeprom) { + s.addReport(ReportLevel.WARNING, "Escape Eeprom. Cannot write to internal memory. (Why is only a warning)"); + } + if (warnings.extEeprom) { + s.addReport(ReportLevel.WARNING, "Exit Eeprom. Cannot write to internal memory. (Why is only a warning)"); + } + if (warnings.hasReset) { + s.addReport(ReportLevel.WARNING, "Has Reset"); + } + if (warnings.other) { + s.addReport(ReportLevel.WARNING, "Other. Warning message sold seperately"); + } + if (warnings.overcurrent) { + s.addReport(ReportLevel.WARNING, "Overcurrent"); + } + if (warnings.sensor) { + s.addReport(ReportLevel.WARNING, "Sensor problem"); + } + if (warnings.stall) { + s.addReport(ReportLevel.WARNING, "Motor stall detected"); + } + + return s; + } +} From d010829c21aa6f5bd1787cfcab1c02107fb353ff Mon Sep 17 00:00:00 2001 From: Shatcar Date: Sat, 28 Mar 2026 13:30:33 -0600 Subject: [PATCH 5/9] Add JankCoder --- src/main/java/frc4388/robot/RobotMap.java | 6 +- .../robot/subsystems/intake/Intake.java | 8 +- .../subsystems/intake/IntakeConstants.java | 12 +- .../robot/subsystems/intake/IntakeIO.java | 4 + .../robot/subsystems/intake/IntakeReal.java | 29 ++++- .../frc4388/utility/compute/JankCoder.java | 105 ++++++++++++++++++ .../frc4388/utility/status/FaultSparkMax.java | 2 +- 7 files changed, 150 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc4388/utility/compute/JankCoder.java diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index eba05db..fe771bf 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -34,6 +34,7 @@ import frc4388.robot.subsystems.swerve.SwerveIO; import frc4388.robot.subsystems.swerve.SwerveReal; import frc4388.robot.subsystems.vision.VisionIO; import frc4388.robot.subsystems.vision.VisionReal; +import frc4388.utility.compute.JankCoder; import frc4388.utility.status.FaultCANCoder; import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; @@ -106,11 +107,10 @@ public class RobotMap { // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - - // shooterIO = new ShooterIO() {}; shooterIO = new ShooterReal(shooter1, shooter2, indexer); + JankCoder armEncoder = new JankCoder(0, IntakeConstants.ARM_ENCODER_OFFSET); - intakeIO = new IntakeReal(arm, roller); + intakeIO = new IntakeReal(arm, roller, armEncoder); // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 0246ddd..3c28ce0 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -44,7 +44,7 @@ public class Intake extends SubsystemBase { switch (mode) { case Bouncing: // When bounce is enabled: set the bounce timer - this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD; + this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get(); break; default: break; @@ -118,15 +118,15 @@ public class Intake extends SubsystemBase { state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() && state.armMotorVelocity.in(RotationsPerSecond) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get() ) { - this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD; + this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get(); } // Get the time delta from the last bounce time update double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime; // Get the percentage through the bounce period (0 output means one half period has passed) - double percentOutput = (currentTime / IntakeConstants.BOUNCE_HALF_PERIOD) * IntakeConstants.INTAKE_BOUNCE_OUTPUT; + double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get(); // Clamp the output of the motor to some value - percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT); + percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()); io.armOutput(percentOutput); break; diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index e82ceea..fd689e7 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -13,11 +13,13 @@ 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.; - public static final double BOUNCE_HALF_PERIOD = 5.; - public static final double INTAKE_BOUNCE_OUTPUT = 0.2; - public static final double INTAKE_BOUNCE_MAX_OUTPUT = 0.5; + + public static final ConfigurableDouble INTAKE_BOUNCE_HALF_PERIOD = new ConfigurableDouble("Bounce Half Period", 5.); + public static final ConfigurableDouble INTAKE_BOUNCE_OUTPUT = new ConfigurableDouble("Bounce Output", 0.2); + public static final ConfigurableDouble INTAKE_BOUNCE_MAX_OUTPUT = new ConfigurableDouble("Bounce Max Output", 0.5); public static final ConfigurableDouble INTAKE_BOUNCE_CURRENT_LIMIT = new ConfigurableDouble("Intake Bounce Current Limit", 20); public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 20); @@ -27,7 +29,7 @@ public class IntakeConstants { public static final CanDevice ARM_ID = new CanDevice("ARM", 20); public static final CanDevice ROLLER_ID = new CanDevice("INTAKE_ROLLER", 21); - // public static final int ARM_LIMIT_SWITCH_CHANNEL = 9; + public static final int ARM_ENCODER_ID = 0; // Limits @@ -40,7 +42,7 @@ 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.1); - public static final ConfigurableDouble ARM_ssLIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33); public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.4); public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.4); public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .70); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 4308c26..6066fce 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -17,11 +17,15 @@ public interface IntakeIO { public class IntakeState { double currentBounceTime = 0; + boolean extendedLimit = false; boolean retractedLimit = false; + Angle intakeEncoder = Rotations.of(0); + Angle armAngle = Rotations.of(0); Angle armTargetAngle = Rotations.of(0); Current armMotorCurrent = Amps.of(0); + AngularVelocity armMotorVelocity = RotationsPerSecond.of(0); // AngularAcceleration armMotorAcceleration = RotationsPerSecondPerSecond.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 f170880..59fc5ba 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -19,22 +19,24 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc4388.utility.compute.JankCoder; public class IntakeReal implements IntakeIO { SparkMax m_armMotor; SparkMax m_rollerMotor; - DutyCycleEncoder encoder; + JankCoder m_encoder; public IntakeReal( - // DigitalInput armLimitSwitch, SparkMax armMotor, - SparkMax rollerMotor + SparkMax rollerMotor, + JankCoder jankCoder ) { // m_angleMotor = angleMotor; // m_pitchMotor = pitchMotor; m_armMotor = armMotor; m_rollerMotor = rollerMotor; + m_encoder = jankCoder; // m_armLimitSwitch = armLimitSwitch; } @@ -74,9 +76,24 @@ public class IntakeReal implements IntakeIO { // m_rollerMotor.set(0); } + private boolean retractedLimit() { + return m_encoder.get() >= IntakeConstants.ARM_LIMIT_EXTENDED.get(); + } + private boolean extendedLimit() { + return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get(); + } + @Override public void armOutput(double percentOutput){ + + if(retractedLimit()) { + percentOutput = Math.max(percentOutput, 0); + } else if (extendedLimit()) { + percentOutput = Math.min(percentOutput, 0); + } + m_armMotor.set(percentOutput); + } @Override @@ -89,6 +106,12 @@ public class IntakeReal implements IntakeIO { state.rollerOutput = m_rollerMotor.get(); state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent()); + state.retractedLimit = retractedLimit(); + state.extendedLimit = extendedLimit(); + state.armAngle = m_encoder.getRotations(); + + + // if(state.retractedLimit) { // // Set the arm motor to be zero if the limit switch is pressed diff --git a/src/main/java/frc4388/utility/compute/JankCoder.java b/src/main/java/frc4388/utility/compute/JankCoder.java new file mode 100644 index 0000000..801e69d --- /dev/null +++ b/src/main/java/frc4388/utility/compute/JankCoder.java @@ -0,0 +1,105 @@ +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; + int rotations = 0; + double lastRotation = 0; + final double offset; + + public JankCoder(int dio_channel) { + this.offset = 0; + m_encoder = new DutyCycleEncoder(dio_channel); + loadRotations(); + } + public JankCoder(int dio_channel, double offset) { + this.offset = 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) { + rotations += 1; + saveRotations(); + } else if (curRot - lastRotation > 0.5) { + rotations -= 1; + saveRotations(); + } + + lastRotation = curRot; + } + } + + public double get() { + return (double) rotations + m_encoder.get() + offset; + } + + public Angle getRotations() { + return Rotations.of(get()); + } + + public int getRotationCount() { + return rotations; + } + + public void resetRotations() { + setRotations(0); + } + + public void setRotations(int rotation) { + rotations = rotation; + saveRotations(); + } + + private void saveRotations() { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + stream.write(DataUtils.intToByteArray(rotations)); + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("ENCODER: Unable to write to trim file `" + m_encoder.getSourceChannel() + "`!?!"); + rotations = 0; + } + } + + private boolean loadRotations() { + lastRotation = m_encoder.get(); + this.loaded_rotations = true; + + try (FileInputStream stream = new FileInputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + int fileValue = DataUtils.byteArrayToInt(stream.readNBytes(4)); + + rotations = fileValue; + // 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/status/FaultSparkMax.java b/src/main/java/frc4388/utility/status/FaultSparkMax.java index f9035a9..0f52f21 100644 --- a/src/main/java/frc4388/utility/status/FaultSparkMax.java +++ b/src/main/java/frc4388/utility/status/FaultSparkMax.java @@ -63,7 +63,7 @@ public class FaultSparkMax implements Queryable { s.addReport(ReportLevel.ERROR, "Sensor fault"); } if(faults.temperature) { - s.addReport(ReportLevel.ERROR, "Tempreture fault"); + s.addReport(ReportLevel.ERROR, "Temperature fault"); } Warnings warnings = motor.getWarnings(); From 65c76aca95de81f342df7ff26a77e18856f22a83 Mon Sep 17 00:00:00 2001 From: Shatcar Date: Sat, 28 Mar 2026 15:56:54 -0600 Subject: [PATCH 6/9] working new intake --- .../java/frc4388/robot/RobotContainer.java | 2 +- .../robot/constants/BuildConstants.java | 10 ++--- .../robot/subsystems/intake/Intake.java | 5 ++- .../subsystems/intake/IntakeConstants.java | 25 ++++++++----- .../robot/subsystems/intake/IntakeIO.java | 5 ++- .../robot/subsystems/intake/IntakeReal.java | 37 +++++++++++-------- .../frc4388/utility/compute/JankCoder.java | 1 - 7 files changed, 50 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 77ee3f6..3928609 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -164,7 +164,7 @@ public class RobotContainer { public RobotContainer() { - configureSINGLEBindings(); + configureButtonBindings(); // Called on first robot enable DeferredBlock.addBlock(() -> { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 78ac3f6..e304983 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 = 185; - public static final String GIT_SHA = "7eba3d8faa208df9222599c53db1e11da82d7a96"; - public static final String GIT_DATE = "2026-03-25 11:39:55 MDT"; + public static final int GIT_REVISION = 188; + public static final String GIT_SHA = "d010829c21aa6f5bd1787cfcab1c02107fb353ff"; + public static final String GIT_DATE = "2026-03-28 13:30:33 MDT"; public static final String GIT_BRANCH = "New-Intake"; - public static final String BUILD_DATE = "2026-03-28 09:26:32 MDT"; - public static final long BUILD_UNIX_TIME = 1774711592752L; + public static final String BUILD_DATE = "2026-03-28 15:50:49 MDT"; + public static final long BUILD_UNIX_TIME = 1774734649429L; 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 3c28ce0..c16eb4a 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -109,7 +109,7 @@ public class Intake extends SubsystemBase { break; case Retracting: io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, 0); + io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); break; case Bouncing: io.setRollerOutput(state, 0); @@ -131,8 +131,9 @@ public class Intake extends SubsystemBase { io.armOutput(percentOutput); break; case Idle: + io.armOutput(0); // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); - io.setRollerOutput(state, 0); + // io.setRollerOutput(state, 0); break; } // if (state.retractedLimit){ diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index fd689e7..b71d075 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -13,7 +13,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.; + public static final double ARM_ENCODER_OFFSET = -0.466; @@ -41,11 +41,14 @@ public class IntakeConstants { // public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); - public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.1); - public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33); - public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.4); - public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.4); - public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .70); + 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 ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .80); + public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .40); + // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25); // public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); @@ -77,10 +80,14 @@ public class IntakeConstants { static { ARM_MOTOR_CONFIG.limitSwitch - .reverseLimitSwitchType(Type.kNormallyClosed) .limitSwitchPositionSensor(FeedbackSensor.kPrimaryEncoder) - .forwardLimitSwitchPosition(0) - .forwardLimitSwitchTriggerBehavior(Behavior.kStopMovingMotorAndSetPosition); + + .forwardLimitSwitchType(Type.kNormallyOpen) + .forwardLimitSwitchTriggerBehavior(Behavior.kKeepMovingMotor) + + .reverseLimitSwitchType(Type.kNormallyClosed) + .reverseLimitSwitchPosition(0) + .reverseLimitSwitchTriggerBehavior(Behavior.kStopMovingMotorAndSetPosition); } // 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 6066fce..8a40309 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -17,8 +17,9 @@ public interface IntakeIO { public class IntakeState { double currentBounceTime = 0; - boolean extendedLimit = false; - boolean retractedLimit = false; + boolean retractedLimitSwitch = false; + boolean extendedSoftLimit = false; + boolean retractedSoftLimit = 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 59fc5ba..288b25a 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -10,6 +10,8 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -37,7 +39,9 @@ public class IntakeReal implements IntakeIO { m_armMotor = armMotor; m_rollerMotor = rollerMotor; m_encoder = jankCoder; - // m_armLimitSwitch = armLimitSwitch; + + m_armMotor.configure(IntakeConstants.ARM_MOTOR_CONFIG, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + m_rollerMotor.configure(IntakeConstants.ROLELR_MOTOR_CONFIG, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } @@ -77,18 +81,20 @@ public class IntakeReal implements IntakeIO { } private boolean retractedLimit() { - return m_encoder.get() >= IntakeConstants.ARM_LIMIT_EXTENDED.get(); + return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get(); } private boolean extendedLimit() { - return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get(); + return m_encoder.get() >= IntakeConstants.ARM_LIMIT_EXTENDED.get(); } @Override public void armOutput(double percentOutput){ - if(retractedLimit()) { - percentOutput = Math.max(percentOutput, 0); - } else if (extendedLimit()) { + // if(retractedLimit()) { + // percentOutput = Math.max(percentOutput, 0); + // } + + if (extendedLimit()) { percentOutput = Math.min(percentOutput, 0); } @@ -98,6 +104,9 @@ public class IntakeReal implements IntakeIO { @Override public void updateInputs(IntakeState state) { + m_encoder.update(); + + state.armAngle = Rotations.of(m_armMotor.getEncoder().getPosition()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); state.armMotorVelocity = RotationsPerSecond.of(m_armMotor.getEncoder().getVelocity()).div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); // state.armMotorAcceleration = RotationsPerSecondPerSecond.of(m_armMotor.getEncoder().ge); @@ -106,17 +115,15 @@ public class IntakeReal implements IntakeIO { state.rollerOutput = m_rollerMotor.get(); state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent()); - state.retractedLimit = retractedLimit(); - state.extendedLimit = extendedLimit(); - state.armAngle = m_encoder.getRotations(); + state.retractedSoftLimit = retractedLimit(); + state.extendedSoftLimit = extendedLimit(); - + state.intakeEncoder = m_encoder.getRotations(); + state.retractedLimitSwitch = m_armMotor.getReverseLimitSwitch().isPressed(); - - // if(state.retractedLimit) { - // // Set the arm motor to be zero if the limit switch is pressed - // m_armMotor.setPosition(0., 0); - // } + if(state.retractedLimitSwitch) { + m_encoder.resetRotations(); + } } @Override diff --git a/src/main/java/frc4388/utility/compute/JankCoder.java b/src/main/java/frc4388/utility/compute/JankCoder.java index 801e69d..e04e3a8 100644 --- a/src/main/java/frc4388/utility/compute/JankCoder.java +++ b/src/main/java/frc4388/utility/compute/JankCoder.java @@ -75,7 +75,6 @@ public class JankCoder { } catch (Exception e) { // e.printStackTrace(); System.out.println("ENCODER: Unable to write to trim file `" + m_encoder.getSourceChannel() + "`!?!"); - rotations = 0; } } From 3fe5d0e5a1cfe24710b755b489fa61255e307095 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Mon, 30 Mar 2026 16:15:43 -0600 Subject: [PATCH 7/9] New controls --- .../java/frc4388/robot/RobotContainer.java | 70 ++++++++++++------- .../robot/constants/BuildConstants.java | 14 ++-- .../robot/subsystems/intake/Intake.java | 42 ++++++++--- .../subsystems/intake/IntakeConstants.java | 2 + 4 files changed, 85 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3928609..be215b1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -109,7 +109,7 @@ public class RobotContainer { private Command IntakeExtended = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendedREMOVEME), m_robotIntake) ); // private Command LidarIntake = new SequentialCommandGroup( @@ -146,7 +146,7 @@ public class RobotContainer { ); private Command IntakeRetracted = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RetractedREMOVEME), m_robotIntake) ); private Command RobotShoot = new SequentialCommandGroup( @@ -357,31 +357,49 @@ public class RobotContainer { m_robotShooter.spinUpIdle(); })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Retracted); - })); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(new InstantCommand(() -> { + // m_robotIntake.setMode(IntakeMode.RetractedREMOVEME); + // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Extended); - })); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> { + // m_robotIntake.setMode(IntakeMode.ExtendedREMOVEME); + // })); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Extending); + m_robotIntake.setMode(IntakeMode.ExtendingRolling); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.ExtendingIdle); + })); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracting); + })); + + + + new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.ExtendingIdle); })) .onFalse(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Retracting); })) .onFalse(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); })); + // .onFalse(new InstantCommand(() -> { + // m_robotIntake.setMode(IntakeMode.Idle); + // })); // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // .onTrue(new InstantCommand(() -> { @@ -493,21 +511,21 @@ public class RobotContainer { m_robotShooter.spinUpIdle(); })); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Extending); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); - })); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> { + // m_robotIntake.setMode(IntakeMode.ExtendedIdle); + // })) + // .onFalse(new InstantCommand(() -> { + // m_robotIntake.setMode(IntakeMode.Idle); + // })); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Retracting); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); - })); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> { + // m_robotIntake.setMode(IntakeMode.Retracting); + // })) + // .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 e304983..65c2b66 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"; + public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 188; - public static final String GIT_SHA = "d010829c21aa6f5bd1787cfcab1c02107fb353ff"; - public static final String GIT_DATE = "2026-03-28 13:30:33 MDT"; + 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 String GIT_BRANCH = "New-Intake"; - public static final String BUILD_DATE = "2026-03-28 15:50:49 MDT"; - public static final long BUILD_UNIX_TIME = 1774734649429L; - public static final int DIRTY = 1; + 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; 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 c16eb4a..2059e6d 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -28,10 +28,14 @@ public class Intake extends SubsystemBase { } public enum IntakeMode { - Extended, - Retracted, - Extending, + ExtendedREMOVEME, + RetractedREMOVEME, + + ExtendingIdle, + ExtendingRolling, + Retracting, + Idle, Bouncing } @@ -95,22 +99,35 @@ public class Intake extends SubsystemBase { // getCurrentTime switch (mode) { - case Extended: + case ExtendedREMOVEME: // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); // io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); break; - case Retracted: + case RetractedREMOVEME: // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); // io.setRollerOutput(state, 0); break; - case Extending: + + case ExtendingIdle: io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get()); + io.setRollerOutput(state, 0); break; + + case ExtendingRolling: + io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); + io.setRollerOutput(state, IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get()); + break; + case Retracting: io.armOutput(IntakeConstants.ARM_RETRACT_PERCENT_OUTPUT.get()); - io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); + + if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) { + io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); + } else { + io.setRollerOutput(state, 0); + } break; + case Bouncing: io.setRollerOutput(state, 0); @@ -129,11 +146,16 @@ public class Intake extends SubsystemBase { percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()); io.armOutput(percentOutput); + + if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) { + io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); + } else { + io.setRollerOutput(state, 0); + } break; case Idle: io.armOutput(0); - // io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); - // io.setRollerOutput(state, 0); + io.setRollerOutput(state, 0); break; } // if (state.retractedLimit){ diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index b71d075..59b6fba 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -45,6 +45,8 @@ public class IntakeConstants { 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_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 2.0); 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); From 9021f480beaf35912eb54a845ec87522fef5337a Mon Sep 17 00:00:00 2001 From: Shatcar Date: Mon, 30 Mar 2026 18:57:14 -0600 Subject: [PATCH 8/9] 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); + } } From 058866fc919b76aa06d5567f8a9cc33d7e65e252 Mon Sep 17 00:00:00 2001 From: Shatcar Date: Mon, 30 Mar 2026 19:52:57 -0600 Subject: [PATCH 9/9] Oh god I hate REV --- src/main/java/frc4388/robot/Robot.java | 5 +++++ .../java/frc4388/robot/constants/BuildConstants.java | 10 +++++----- .../java/frc4388/robot/subsystems/intake/Intake.java | 11 ++++++----- .../robot/subsystems/intake/IntakeConstants.java | 12 ++++++------ 4 files changed, 22 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 42c6f2c..31054f1 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,6 +15,8 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import com.ctre.phoenix6.SignalLogger; + import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -50,6 +52,9 @@ public class Robot extends LoggedRobot { // Start logging with AdvantageKit startLogging(); + com.revrobotics.util.StatusLogger.disableAutoLogging(); + SignalLogger.enableAutoLogging(false); + // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 7f3e6b2..93a48fb 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 = 190; - public static final String GIT_SHA = "3fe5d0e5a1cfe24710b755b489fa61255e307095"; - public static final String GIT_DATE = "2026-03-30 16:15:43 MDT"; + public static final int GIT_REVISION = 191; + public static final String GIT_SHA = "9021f480beaf35912eb54a845ec87522fef5337a"; + public static final String GIT_DATE = "2026-03-30 18:57:14 MDT"; public static final String GIT_BRANCH = "New-Intake"; - public static final String BUILD_DATE = "2026-03-30 18:15:12 MDT"; - public static final long BUILD_UNIX_TIME = 1774916112363L; + public static final String BUILD_DATE = "2026-03-30 19:50:05 MDT"; + public static final long BUILD_UNIX_TIME = 1774921805228L; 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 a837396..9cc56af 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -129,12 +129,13 @@ public class Intake extends SubsystemBase { break; case Bouncing: - io.setRollerOutput(state, 0); + // io.setRollerOutput(state, 0); if( - state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() && - state.armMotorVelocity.in(RotationsPerSecond) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get() + state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() + // Math.abs(state.armMotorVelocity.in(RotationsPerSecond)) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get() ) { + System.out.println("RESET BOUNCE"); this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get(); } @@ -143,11 +144,11 @@ public class Intake extends SubsystemBase { // Get the percentage through the bounce period (0 output means one half period has passed) double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get(); // Clamp the output of the motor to some value - percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()); + percentOutput = -Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()); io.armOutput(percentOutput); - if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) { + if(percentOutput < 0) { io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); } else { io.setRollerOutput(state, 0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 238786d..cc5216c 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -19,10 +19,10 @@ public class IntakeConstants { public static final ConfigurableDouble INTAKE_BOUNCE_HALF_PERIOD = new ConfigurableDouble("Bounce Half Period", 5.); - public static final ConfigurableDouble INTAKE_BOUNCE_OUTPUT = new ConfigurableDouble("Bounce Output", 0.2); - public static final ConfigurableDouble INTAKE_BOUNCE_MAX_OUTPUT = new ConfigurableDouble("Bounce Max Output", 0.5); - public static final ConfigurableDouble INTAKE_BOUNCE_CURRENT_LIMIT = new ConfigurableDouble("Intake Bounce Current Limit", 20); - public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 20); + public static final ConfigurableDouble INTAKE_BOUNCE_OUTPUT = new ConfigurableDouble("Bounce Output", 0.1); + public static final ConfigurableDouble INTAKE_BOUNCE_MAX_OUTPUT = new ConfigurableDouble("Bounce Max Output", 0.2); + public static final ConfigurableDouble INTAKE_BOUNCE_CURRENT_LIMIT = new ConfigurableDouble("Intake Bounce Current Limit", 16); + public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 4); @@ -44,8 +44,8 @@ public class IntakeConstants { 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.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_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.2); + public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.2); public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 1.17);