From 539c1bd8eb96c91eb22dfc882b8a16591d4d0de2 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Tue, 10 Feb 2026 17:33:39 -0800 Subject: [PATCH] Working robot in testing --- src/main/java/frc4388/robot/RobotMap.java | 10 ++--- .../robot/constants/BuildConstants.java | 10 ++--- .../robot/subsystems/intake/Intake.java | 24 +++++++----- .../subsystems/intake/IntakeConstants.java | 12 +++--- .../robot/subsystems/intake/IntakeReal.java | 14 +++---- .../robot/subsystems/shooter/Shooter.java | 37 ++++++++++--------- .../subsystems/shooter/ShooterConstants.java | 12 +++--- .../robot/subsystems/shooter/ShooterReal.java | 16 +++++--- 8 files changed, 74 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3cc013a..5067e69 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -84,13 +84,13 @@ public class RobotMap { swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); // Configure Shooter 22,23,24 - TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.RIO_CANBUS); - TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.RIO_CANBUS); - TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.RIO_CANBUS); + TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS); + TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.CANIVORE_CANBUS); + TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.CANIVORE_CANBUS); //Configure Intake 20,21 - TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.RIO_CANBUS); - TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.RIO_CANBUS); + TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.CANIVORE_CANBUS); + TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.CANIVORE_CANBUS); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index c3642e0..764bf0b 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 = 36; - public static final String GIT_SHA = "d90bddac0fff1c8f1bb6dd9a6b4a862c822a005a"; - public static final String GIT_DATE = "2026-02-09 17:03:48 MST"; + public static final int GIT_REVISION = 38; + public static final String GIT_SHA = "6ce6d0eb0b8faec5af448ec23ffca156303cbed5"; + public static final String GIT_DATE = "2026-02-09 19:38:55 MST"; public static final String GIT_BRANCH = "arm-test"; - public static final String BUILD_DATE = "2026-02-09 18:14:36 MST"; - public static final long BUILD_UNIX_TIME = 1770686076395L; + public static final String BUILD_DATE = "2026-02-10 17:12:07 MST"; + public static final long BUILD_UNIX_TIME = 1770768727439L; 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 0b212d7..d83292a 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -33,17 +33,10 @@ public class Intake extends SubsystemBase { Retracted, } + private IntakeMode mode = IntakeMode.Extended; + public void setMode(IntakeMode mode) { - switch (mode) { - case Extended: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); - io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); - break; - case Retracted: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); - io.setRollerVelocity(state, RotationsPerSecond.of(0)); - break; - } + this.mode = mode; } @@ -70,6 +63,17 @@ public class Intake extends SubsystemBase { io.updateInputs(state); + switch (mode) { + case Extended: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); + break; + case Retracted: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(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 f3c8aad..f2c3490 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -18,8 +18,8 @@ import frc4388.utility.status.CanDevice; public class IntakeConstants { // Motor conversions - public static final double ARM_MOTOR_GEAR_RATIO = 1/100; - public static final double ROLLER_MOTOR_GEAR_RATIO = 1/3; + public static final double ARM_MOTOR_GEAR_RATIO = 100; + public static final double ROLLER_MOTOR_GEAR_RATIO = 3; //IDs @@ -37,8 +37,8 @@ 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); - public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.25); + public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", -0.3); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0); public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10); // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25); @@ -58,8 +58,8 @@ public class IntakeConstants { .withKD(0.0); public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); - public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0); - public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0); + public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); + public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0); public static ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2); public static ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index ea9c61a..598fa91 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -65,7 +65,7 @@ public class IntakeReal implements IntakeIO { } // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) - AngularVelocity motorSpeed = angularVelocity.div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); + AngularVelocity motorSpeed = angularVelocity.times(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); // m_rollerMotor.set(motorSpeed); // VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); @@ -80,7 +80,7 @@ public class IntakeReal implements IntakeIO { // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - Angle motorAngle = angle.div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); m_armMotor.setControl(armPosition.withPosition(motorAngle)); @@ -88,10 +88,10 @@ public class IntakeReal implements IntakeIO { @Override public void updateInputs(IntakeState state) { - state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); - state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue(); + state.armAngle = m_armMotor.getPosition().getValue().div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); - state.rollerVelocity = m_rollerMotor.getVelocity().getValue(); + state.rollerVelocity = m_rollerMotor.getVelocity().getValue().div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); } @@ -101,11 +101,11 @@ public class IntakeReal implements IntakeIO { 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_MOTOR_CONFIG); + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); IntakeConstants.ROLLER_PID.kP = IntakeConstants.roller_kP.get(); IntakeConstants.ROLLER_PID.kI = IntakeConstants.roller_kI.get(); IntakeConstants.ROLLER_PID.kD = IntakeConstants.roller_kD.get(); - m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 869ed14..49ab73e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -40,25 +40,10 @@ public class Shooter extends SubsystemBase { Inactive, } + private ShooterMode mode = ShooterMode.Inactive; public void setMode(ShooterMode mode) { - switch (mode) { - case Active: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get())); - break; - case Resting: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(0)); - break; - case Inactive: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(0)); - break; - } + this.mode = mode; } // Calculate what should be done based off of the position of the robot @@ -83,5 +68,23 @@ public class Shooter extends SubsystemBase { io.updateInputs(state); + switch (mode) { + case Active: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get())); + break; + case Resting: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(0)); + break; + case Inactive: + io.setShooterVelocity(state, RotationsPerSecond.of(0)); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(0)); + break; + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index f398491..81deb0e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -14,7 +14,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; -public class ShooterConstants { +public class ShooterConstants { // Motor conversions public static final double FEEDER_INCHES_PER_ROT = 1.; @@ -33,25 +33,25 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); - public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 10); + public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Indexer Active Velocity", 10); // public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.0) .withKI(0.0) - .withKD(0.0); + .withKD(0.2); public static Slot0Configs INDEXER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.0) .withKI(0.0) - .withKD(0.0); + .withKD(0.2); public static ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2); - public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0); - public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0); + public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KI", 0); + public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KD", 0); public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2); public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 8aee915..928b09d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -100,7 +100,7 @@ public class ShooterReal implements ShooterIO { return; } - AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); + AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO); m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps)); m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps)); @@ -109,7 +109,13 @@ public class ShooterReal implements ShooterIO { @Override public void setIndexerVelocity(ShooterState state, AngularVelocity target) { state.indexerTargetVelocity = target; - AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); + + if(target.baseUnitMagnitude() == 0) { + m_indexerMotor.set(0); + return; + } + + AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO); m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps)); } @@ -117,9 +123,9 @@ public class ShooterReal implements ShooterIO { @Override public void updateInputs(ShooterState state) { - state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO); - state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO); - state.indexerVelocity = m_indexerMotor.getVelocity().getValue().times(ShooterConstants.INDEXER_GEAR_RATIO); + state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO); + state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO); + state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); // state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);