Team_2648/2026-Robot-Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Work from build 2/28

Browse Source
This commit is contained in:
Bradley Bickford
2026-02-28 17:42:37 -05:00
parent 7621cfd009
commit 3791333f56
12 changed files with 101 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
src/main/java/frc/robot/RobotContainer.java
View File

@@ -42,14 +42,14 @@ import frc.robot.utilities.Elastic;
import frc.robot.utilities.Utilities;
public class RobotContainer {
private PhotonVision vision;
//private PhotonVision vision;
private Drivetrain drivetrain;
private Hood hood;
private Shooter shooter;
private IntakePivot intakePivot;
private IntakeRoller intakeRoller;
private Spindexer spindexer;
private Climber climber;
//private Climber climber;
private CommandXboxController driver;
private CommandXboxController secondary;
@@ -59,22 +59,23 @@ public class RobotContainer {
private Timer shiftTimer;
public RobotContainer() {
vision = new PhotonVision();
//vision = new PhotonVision();
drivetrain = new Drivetrain(null);
hood = new Hood();
shooter = new Shooter();
intakePivot = new IntakePivot();
intakeRoller = new IntakeRoller();
spindexer = new Spindexer();
climber = new Climber();
//climber = new Climber();
/*
vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
vision.addPoseEstimateConsumer((vp) -> {
Logger.recordOutput(
"Vision/" + vp.cameraName() + "/Pose",
vp.visualPose()
);
});
});*/
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
@@ -82,7 +83,8 @@ public class RobotContainer {
shiftTimer = new Timer();
shiftTimer.reset();
configureBindings();
//configureBindings();
testConfigureBindings();
configureShiftDisplay();
if(AutoConstants.kAutoConfigOk) {
@@ -106,7 +108,8 @@ public class RobotContainer {
// This should just work, if it doesn't it's likely modules aren't assigned the right IDs
// after the electronics rebuild. For testing normal operation nothing about the Drivetrain
// class should need to change
drivetrain.setDefaultCommand(
drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true));
/*drivetrain.setDefaultCommand(
drivetrain.drive(
driver::getLeftY,
driver::getLeftX,
@@ -138,7 +141,7 @@ public class RobotContainer {
driver::getLeftY,
driver::getLeftX
)
);
);*/
// Stop everything by default other than the drivetrain
shooter.setDefaultCommand(shooter.stop());
@@ -146,7 +149,7 @@ public class RobotContainer {
intakeRoller.setDefaultCommand(intakeRoller.stop());
hood.setDefaultCommand(hood.stop());
spindexer.setDefaultCommand(spindexer.stop());
climber.setDefaultCommand(climber.stop());
//climber.setDefaultCommand(climber.stop());
// While holding POV up of the driver controller, the climber
// should move such that its motor moves the climber down with the left
@@ -154,9 +157,9 @@ public class RobotContainer {
// trigger axis.
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted
// from the constants file for the subsystem having the problem.
driver.povUp().whileTrue(climber.manualSpeed(() -> {
return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis();
}));
//driver.povUp().whileTrue(climber.manualSpeed(() -> {
// return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis();
//}));
// While holding the right bumper of the driver controller, the intake rollers
// and the spindexer and feeder should move such that all motors are moving in such a way
@@ -165,7 +168,8 @@ public class RobotContainer {
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
// constants file for the subsystem having the problem
driver.rightBumper().whileTrue(
intakeRoller.runIn().alongWith(spindexer.spinToShooter())
//intakeRoller.runIn().alongWith(spindexer.spinToShooter())
spindexer.spinToShooter()
);
// While holding the left bumper of the driver controller, the intake rollers
@@ -244,6 +248,7 @@ public class RobotContainer {
)
);
/*
driver.b().whileTrue(
drivetrain.lockToYaw(
() -> {
@@ -254,7 +259,7 @@ public class RobotContainer {
driver::getLeftY,
driver::getLeftX
)
);
);*/
shooter.setDefaultCommand(
shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)

10
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@@ -20,15 +20,15 @@ public class DrivetrainConstants {
public static final double kRearLeftMagEncoderOffset = 3.761;
public static final double kRearRightMagEncoderOffset = 2.573;
public static final int kFrontLeftDrivingCANID = 0;
public static final int kFrontLeftDrivingCANID = 4;
public static final int kFrontRightDrivingCANID = 3;
public static final int kRearLeftDrivingCANID = 1;
public static final int kRearRightDrivingCANID = 2;
public static final int kFrontLeftTurningCANID = 8;
public static final int kFrontRightTurningCANID = 9;
public static final int kRearLeftTurningCANID = 7;
public static final int kRearRightTurningCANID = 6;
public static final int kFrontLeftTurningCANID = 7; // 8
public static final int kFrontRightTurningCANID = 21; //9
public static final int kRearLeftTurningCANID = 6; //7
public static final int kRearRightTurningCANID = 8; //6
public static final int kFrontLeftAnalogInPort = 3;
public static final int kFrontRightAnalogInPort = 2;

4
src/main/java/frc/robot/constants/HoodConstants.java
View File

@@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.Filesystem;
public class HoodConstants {
// TODO Real Values
public static final int kMotorCANID = 0;
public static final int kMotorCANID = 12;
public static final double kConversionFactor = 3.0*147.0/8.0;
@@ -26,7 +26,7 @@ public class HoodConstants {
public static final double kV = 0;
public static final double kA = 0;
public static final double kStartupAngle = 0;
public static final double kMaxManualSpeedMultiplier = .5;
public static final double kMaxManualSpeedMultiplier = 1;
public static final double kAmpsToTriggerPositionReset = 10;

6
src/main/java/frc/robot/constants/IntakePivotConstants.java
View File

@@ -20,8 +20,8 @@ public class IntakePivotConstants {
}
}
public static final int kLeftMotorCANID = 0;
public static final int kRightMotorCANID = 0;
public static final int kLeftMotorCANID = 16;
public static final int kRightMotorCANID = 9;
public static final double kConversionFactor = 60.0/11.0*60.0/18.0*38.0/16.0;
@@ -40,7 +40,7 @@ public class IntakePivotConstants {
public static final int kCurrentLimit = 30;
public static final IdleMode kIdleMode = IdleMode.kBrake;
public static final IdleMode kIdleMode = IdleMode.kCoast;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM

14
src/main/java/frc/robot/constants/IntakeRollerConstants.java
View File

@@ -5,30 +5,24 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
public class IntakeRollerConstants {
// TODO Real values
public static final int kLeftMotorCANID = 0;
public static final int kRightMotorCANID = 0;
public static final int kMotorCANID = 20;
public static final int kCurrentLimit = 40;
public static final boolean kInvertMotors = false;
public static final boolean kInvertMotors = true;
public static final double kSpeed = .6;
public static final IdleMode kIdleMode = IdleMode.kCoast;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig();
public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig();
static {
leftMotorConfig
.idleMode(kIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kInvertMotors);
rightMotorConfig
.idleMode(kIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kInvertMotors)
.follow(kLeftMotorCANID);
}
}

54
src/main/java/frc/robot/constants/ShooterConstants.java
View File

@@ -9,7 +9,7 @@ import edu.wpi.first.math.util.Units;
public class ShooterConstants {
public enum ShooterSpeeds {
kHubSpeed(0),
kFeedSpeed(0);
kFeedSpeed(0.35);
private double speedMPS;
@@ -27,30 +27,28 @@ public class ShooterConstants {
public static final double kWheelDiameter = Units.inchesToMeters(4);
// TODO Real values
public static final int kLeftShooterMotor1CANID = 0;
public static final int kLeftShooterMotor2CANID = 0;
public static final int kRightShooterMotor1CANID = 0;
public static final int kRightShooterMotor2CANID = 0;
public static final int kLeftShooterMotorCANID = 2;
public static final int kRightShooterMotorCANID = 5;
public static final boolean kLeftShooterMotor1Inverted = false;
public static final boolean kLeftShooterMotor2Inverted = false;
public static final boolean kRightShooterMotor1Inverted = false;
public static final boolean kRightShooterMotor2Inverted = false;
public static final boolean kLeftShooterMotorInverted = true;
public static final boolean kRightShooterMotorInverted = false;
public static final double kLeftP = 0;
public static final double kLeftP = 0.1;
public static final double kLeftI = 0;
public static final double kLeftD = 0;
public static final double kLeftS = 0;
public static final double kLeftV = 0.21;
public static final double kLeftA = 0;
public static final double kRightP = 0;
public static final double kRightP = 0.1;
public static final double kRightI = 0;
public static final double kRightD = 0;
public static final double kRightS = 0;
public static final double kRightV = 0.21;
public static final double kRightA = 0;
public static final double kMaxManualSpeedMultiplier = 1;
public static final double kShooterHeightMeters = 0;
// TODO Is this value sane?
@@ -60,51 +58,37 @@ public class ShooterConstants {
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
public static final SparkMaxConfig kLeftMotor1Config = new SparkMaxConfig();
public static final SparkMaxConfig kLeftMotor2Config = new SparkMaxConfig();
public static final SparkMaxConfig kRightMotor1Config = new SparkMaxConfig();
public static final SparkMaxConfig kRightMotor2Config = new SparkMaxConfig();
public static final SparkMaxConfig kLeftMotorConfig = new SparkMaxConfig();
public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
static {
kLeftMotor1Config
kLeftMotorConfig
.idleMode(kShooterIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kLeftShooterMotor1Inverted);
kLeftMotor1Config.absoluteEncoder
.inverted(kLeftShooterMotorInverted);
kLeftMotorConfig.absoluteEncoder
.positionConversionFactor(kWheelDiameter * Math.PI)
.velocityConversionFactor(kWheelDiameter * Math.PI / 60);
kLeftMotor1Config.closedLoop
kLeftMotorConfig.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
.pid(kLeftP, kLeftI, kLeftD)
.outputRange(-1, 1)
.feedForward
.sva(kLeftS, kLeftV, kLeftA);
kLeftMotor2Config
kRightMotorConfig
.idleMode(kShooterIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kLeftShooterMotor2Inverted)
.follow(kLeftShooterMotor1CANID);
kRightMotor1Config
.idleMode(kShooterIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kRightShooterMotor1Inverted);
kRightMotor1Config.absoluteEncoder
.inverted(kRightShooterMotorInverted);
kRightMotorConfig.absoluteEncoder
.positionConversionFactor(kWheelDiameter * Math.PI)
.velocityConversionFactor(kWheelDiameter * Math.PI / 60);
kRightMotor1Config.closedLoop
kRightMotorConfig.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
.pid(kRightP, kRightI, kRightD)
.outputRange(-1, 1)
.feedForward
.sva(kRightS, kRightV, kRightA);
kRightMotor2Config
.idleMode(kShooterIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kRightShooterMotor2Inverted)
.follow(kRightShooterMotor1CANID);
}
}

13
src/main/java/frc/robot/constants/SpindexerConstants.java
View File

@@ -10,16 +10,19 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
public class SpindexerConstants {
// TODO Real values
public static final int kSpindexerMotorCANID = 0;
public static final int kFeederMotorCANID = 0;
public static final int kFeederMotorCANID = 4;
public static final int kSpindexerStatorCurrentLimit = 90;
public static final int kSpindexerSupplyCurrentLimit = 50;
public static final int kSpindexerStatorCurrentLimit = 110;
public static final int kSpindexerSupplyCurrentLimit = 60;
public static final int kFeederCurrentLimit = 40;
public static final boolean kFeederMotorInverted = false;
public static final double kSpindexerSpeed = 1;
public static final double kFeederSpeed = 1;
public static final boolean kFeederMotorInverted = true;
public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive;
public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Brake;
public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast;
public static final IdleMode kFeederIdleMode = IdleMode.kBrake;

1
src/main/java/frc/robot/subsystems/Hood.java
View File

@@ -4,7 +4,6 @@ import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.PersistMode;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.ResetMode;

20
src/main/java/frc/robot/subsystems/IntakeRoller.java
View File

@@ -10,41 +10,33 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.IntakeRollerConstants;
public class IntakeRoller extends SubsystemBase {
private SparkMax leftMotor;
private SparkMax rightMotor;
private SparkMax motor;
public IntakeRoller() {
leftMotor = new SparkMax(IntakeRollerConstants.kLeftMotorCANID, MotorType.kBrushless);
rightMotor = new SparkMax(IntakeRollerConstants.kRightMotorCANID, MotorType.kBrushless);
motor = new SparkMax(IntakeRollerConstants.kMotorCANID, MotorType.kBrushless);
leftMotor.configure(
motor.configure(
IntakeRollerConstants.leftMotorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
rightMotor.configure(
IntakeRollerConstants.rightMotorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
}
public Command runIn() {
return run(() -> {
leftMotor.set(1);
motor.set(IntakeRollerConstants.kSpeed);
});
}
public Command runOut() {
return run(() -> {
leftMotor.set(-1);
motor.set(-IntakeRollerConstants.kSpeed);
});
}
public Command stop() {
return run(() -> {
leftMotor.set(0);
motor.set(0);
});
}

2
src/main/java/frc/robot/subsystems/PhotonVision.java
View File

@@ -75,7 +75,6 @@ public class PhotonVision extends SubsystemBase {
if(!results.isEmpty()) {
latestResults.set(i, results.get(results.size() - 1));
}
Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults.get(i));
@@ -92,6 +91,7 @@ public class PhotonVision extends SubsystemBase {
}
}
}
}
/**
* Returns the best 3D pose for a given AprilTag ID as seen by the cameras on the robot.

50
src/main/java/frc/robot/subsystems/Shooter.java
View File

@@ -19,10 +19,8 @@ import frc.robot.constants.ShooterConstants;
import frc.robot.constants.ShooterConstants.ShooterSpeeds;
public class Shooter extends SubsystemBase {
private SparkMax leftMotor1;
private SparkMax leftMotor2;
private SparkMax rightMotor1;
private SparkMax rightMotor2;
private SparkMax leftMotor;
private SparkMax rightMotor;
private AbsoluteEncoder leftEncoder;
private AbsoluteEncoder rightEncoder;
@@ -33,40 +31,26 @@ public class Shooter extends SubsystemBase {
private ShooterSpeeds targetSpeeds;
public Shooter() {
leftMotor1 = new SparkMax(ShooterConstants.kLeftShooterMotor1CANID, MotorType.kBrushless);
leftMotor2 = new SparkMax(ShooterConstants.kLeftShooterMotor2CANID, MotorType.kBrushless);
rightMotor1 = new SparkMax(ShooterConstants.kRightShooterMotor1CANID, MotorType.kBrushless);
rightMotor2 = new SparkMax(ShooterConstants.kRightShooterMotor2CANID, MotorType.kBrushless);
leftMotor = new SparkMax(ShooterConstants.kLeftShooterMotorCANID, MotorType.kBrushless);
rightMotor = new SparkMax(ShooterConstants.kRightShooterMotorCANID, MotorType.kBrushless);
leftMotor1.configure(
ShooterConstants.kLeftMotor1Config,
leftMotor.configure(
ShooterConstants.kLeftMotorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
rightMotor1.configure(
ShooterConstants.kRightMotor1Config,
rightMotor.configure(
ShooterConstants.kRightMotorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
leftMotor2.configure(
ShooterConstants.kLeftMotor2Config,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
leftEncoder = leftMotor.getAbsoluteEncoder();
rightEncoder = rightMotor.getAbsoluteEncoder();
rightMotor2.configure(
ShooterConstants.kRightMotor2Config,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
leftEncoder = leftMotor1.getAbsoluteEncoder();
rightEncoder = rightMotor1.getAbsoluteEncoder();
leftClosedLoopController = leftMotor1.getClosedLoopController();
rightClosedLoopController = rightMotor1.getClosedLoopController();
leftClosedLoopController = leftMotor.getClosedLoopController();
rightClosedLoopController = rightMotor.getClosedLoopController();
// TODO Set this to the initial startup speed
targetSpeeds = null;
@@ -92,8 +76,8 @@ public class Shooter extends SubsystemBase {
return run(() -> {
if(targetSpeeds == null) {
leftMotor1.disable();
rightMotor1.disable();
leftMotor.disable();
rightMotor.disable();
} else {
leftClosedLoopController.setSetpoint(
targetSpeeds.getSpeedMPS(),
@@ -101,7 +85,7 @@ public class Shooter extends SubsystemBase {
);
rightClosedLoopController.setSetpoint(
targetSpeeds.getSpeedMPS(),
-targetSpeeds.getSpeedMPS(),
ControlType.kVelocity
);
}
@@ -112,8 +96,8 @@ public class Shooter extends SubsystemBase {
targetSpeeds = null;
return run(() -> {
leftMotor1.set(speed.getAsDouble());
rightMotor1.set(speed.getAsDouble());
leftMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
rightMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
});
}

12
src/main/java/frc/robot/subsystems/Spindexer.java
View File

@@ -37,15 +37,19 @@ public class Spindexer extends SubsystemBase {
public Command spinToShooter() {
return run(() -> {
spindexerMotor.setControl(spindexerMotorOutput.withOutput(1));
feederMotor.set(1);
spindexerMotor.setControl(
spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed)
);
feederMotor.set(SpindexerConstants.kFeederSpeed);
});
}
public Command spinToIntake() {
return run(() -> {
spindexerMotor.setControl(spindexerMotorOutput.withOutput(-1));
feederMotor.set(-1);
spindexerMotor.setControl(
spindexerMotorOutput.withOutput(-SpindexerConstants.kSpindexerSpeed)
);
feederMotor.set(-SpindexerConstants.kFeederSpeed);
});
}