Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Robot should be ready

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-09 17:18:54 -08:00
parent d90bddac0f
commit 51d2b80ea0
14 changed files with 183 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+12 -9
View File
@@ -1,6 +1,9 @@
package frc4388.robot.subsystems.intake;
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 java.util.function.Supplier;
@@ -12,7 +15,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.utility.status.FaultReporter;
public class Intake extends SubsystemBase {
IntakeIO io;
public IntakeIO io;
IntakeStateAutoLogged state = new IntakeStateAutoLogged();
Supplier<Pose2d> m_swervePoseSupplier;
@@ -26,19 +29,19 @@ public class Intake extends SubsystemBase {
}
public enum IntakeMode {
Up,
Down,
Extended,
Retracted,
}
public void setMode(IntakeMode mode) {
switch (mode) {
case Up:
io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP);
case Extended:
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get()));
io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get()));
break;
case Down:
io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
case Retracted:
io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get()));
io.setRollerVelocity(state, RotationsPerSecond.of(0));
break;
}
}
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+16 -8
View File
@@ -12,6 +12,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import frc4388.utility.configurable.ConfigurableDouble;
import frc4388.utility.status.CanDevice;
public class IntakeConstants {
@@ -33,21 +34,28 @@ public class IntakeConstants {
//when testing the negative output of 10% made the robot put the intake up
public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0);
// 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 ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10);
// public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
// public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
// public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0);
public static final Slot0Configs ARM_PID = new Slot0Configs()
.withKP(2.0)
.withKP(0.2)
.withKI(0.0)
.withKD(10.0);
.withKD(0.0);
public static final Slot0Configs ROLLER_PID = new Slot0Configs()
.withKP(2.0)
.withKP(0.2)
.withKI(0.0)
.withKD(10.0);
.withKD(0.0);
// 0 is paralell to the ground, 90 is directly up
// public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java
+1
View File
@@ -39,4 +39,5 @@ public interface IntakeIO {
public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {}
public default void updateInputs(IntakeState state) {}
public default void updateGains() {}
}
src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java
+4
View File
@@ -95,6 +95,10 @@ public class IntakeReal implements IntakeIO {
state.rollerVelocity = m_rollerMotor.getVelocity().getValue();
state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue();
}
@Override
public void updateGains() {
IntakeConstants.ARM_PID.kP = arm_kP.get();
IntakeConstants.ARM_PID.kI = arm_kI.get();
src/main/java/frc4388/robot/subsystems/shooter/Shooter.java
+14 -10
View File
@@ -1,5 +1,9 @@
package frc4388.robot.subsystems.shooter;
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 java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
@@ -11,7 +15,7 @@ import frc4388.robot.subsystems.intake.IntakeConstants;
import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState;
public class Shooter extends SubsystemBase {
ShooterIO io;
public ShooterIO io;
ShooterStateAutoLogged state = new ShooterStateAutoLogged();
// Supplier<Pose2d> m_swervePoseSupplier;
@@ -46,19 +50,19 @@ public class Shooter extends SubsystemBase {
public void setMode(ShooterMode mode) {
switch (mode) {
case Active:
io.setMotor1Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
io.setIndexerVelocity(state, ShooterConstants.INDEXER_ACTIVE_VELOCITY);
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.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY);
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
// io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get()));
break;
case Inactive:
io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY);
io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
// io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get()));
break;
}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java
+26 -14
View File
@@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import frc4388.utility.configurable.ConfigurableDouble;
import frc4388.utility.status.CanDevice;
public class ShooterConstants {
@@ -20,16 +21,20 @@ public class ShooterConstants {
public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.;
public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.;
public static final double INDEXER_GEAR_RATIO = 1.;
public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0);
public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(-15);
public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0);
public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
// public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30);
// public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(15);
// public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
// public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10);
// public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30);
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_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
.withKV(0.0)
@@ -37,6 +42,12 @@ public class ShooterConstants {
.withKI(0.0)
.withKD(0.0);
public static Slot0Configs INDEXER_PID = new Slot0Configs()
.withKV(0.0)
.withKP(0.0)
.withKI(0.0)
.withKD(0.0);
// Limits
// 0 is the forward angle on the robot.
@@ -45,8 +56,8 @@ public class ShooterConstants {
// public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180);
// 0 is paralell to the ground, 90 is directly up
public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
// public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90);
// public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0);
// Motor configs
// public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration()
@@ -61,10 +72,10 @@ public class ShooterConstants {
// );
public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22);
public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23);
public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24);
public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22);
public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23);
public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24);
public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
@@ -73,7 +84,7 @@ public class ShooterConstants {
.withStatorCurrentLimitEnable(true)
).withMotorOutput(
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration()
@@ -86,6 +97,7 @@ public class ShooterConstants {
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
);
public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java
+6 -3
View File
@@ -36,14 +36,17 @@ public interface ShooterIO {
Current motor2Current = Amps.of(0);
Current indexerCurrent = Amps.of(0);
LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
// LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
}
// public default void setShooterAngle(ShooterState state, Angle angle) {}
// public default void setShooterPitch(ShooterState state, Angle angle) {}
public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {}
public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {}
// public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {}
public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {}
public default void updateInputs(ShooterState state) {}
public default void updateGains() {}
}
src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java
+44 -11
View File
@@ -8,12 +8,15 @@ import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import frc4388.robot.subsystems.intake.IntakeConstants;
import frc4388.utility.configurable.ConfigurableDouble;
public class ShooterReal implements ShooterIO {
TalonFX m_shooter1Motor;
TalonFX m_shooter2Motor;
TalonFX m_indexerMotor;
VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0);
VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
@@ -27,14 +30,18 @@ public class ShooterReal implements ShooterIO {
m_shooter1Motor = shooter1Motor;
m_shooter2Motor = shooter2Motor;
m_indexerMotor = indexerMotor;
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG);
shooter1Velocity.Slot = 0;
shooter2Velocity.Slot = 0;
m_indexerVelocity.Slot = 0;
}
private Angle clampAng(Angle x, Angle min, Angle max){
@@ -83,34 +90,60 @@ public class ShooterReal implements ShooterIO {
@Override
public void setMotor1Velocity(ShooterState state, AngularVelocity target) {
public void setShooterVelocity(ShooterState state, AngularVelocity target) {
state.motor1TargetVelocity = target;
state.motor2TargetVelocity = target;
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
m_shooter1Motor.setControl(new VelocityDutyCycle(motorRps));
m_shooter2Motor.setControl(new VelocityDutyCycle(motorRps));
AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO);
m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps));
m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps));
}
@Override
public void setIndexerVelocity(ShooterState state, AngularVelocity target) {
state.indexerTargetVelocity = target;
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
m_indexerMotor.setControl(new VelocityDutyCycle(motorRps));
AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO);
m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps));
}
ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2);
ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0);
ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0);
ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2);
ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0);
ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
@Override
public void updateInputs(ShooterState state) {
state.motor1Velocity = m_shooter1Motor.getVelocity().getValue();
state.motor2Velocity = m_shooter2Motor.getVelocity().getValue();
state.indexerVelocity = m_indexerMotor.getVelocity().getValue();
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.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
// state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue();
state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue();
state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue();
}
@Override
public void updateGains() {
// TEMPORARY PIDs
ShooterConstants.SHOOTER_PID.kP = shooter_kP.get();
ShooterConstants.SHOOTER_PID.kI = shooter_kI.get();
ShooterConstants.SHOOTER_PID.kD = shooter_kD.get();
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
ShooterConstants.INDEXER_PID.kP = indexer_kP.get();
ShooterConstants.INDEXER_PID.kI = indexer_kI.get();
ShooterConstants.INDEXER_PID.kD = indexer_kD.get();
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID);
}
}
src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java
+2 -2
View File
@@ -22,6 +22,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.units.measure.Angle;
import frc4388.robot.constants.Constants;
import frc4388.utility.configurable.ConfigurableDouble;
//import edu.wpi.first.units.measure.measure.Distance;
import frc4388.utility.status.CanDevice;
@@ -31,7 +32,6 @@ import frc4388.utility.structs.Gains;
// No mans land
// Beware, there be dragons.
public final class SwerveDriveConstants {
public static final String CANBUS_NAME = "drivetrain";
public static final double MAX_ROT_SPEED = Math.PI * 2;
public static final double AUTO_MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 1.0;
@@ -209,7 +209,7 @@ public final class SwerveDriveConstants {
}
public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
.withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(CANBUS_NAME);
.withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(Constants.CANIVORE_CANBUS.getName());
private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // holy verbosity batman.