FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Guys were def the 1st place aliance fr fr

This commit is contained in:
Team578 Driver Station 2026-04-18 12:35:31 -04:00
parent df53b75d08
commit 5986bc8f92
5 changed files with 43 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -83,8 +83,8 @@ public class RobotContainer {
m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID()))); m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID())));
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
m_driverController.a().whileTrue(Commands.parallel(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)/*, intakeSubsystem.stopIntake(m_driverController.getHID())*/)); m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
m_driverController.x().whileTrue(Commands.parallel(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)/*, intakeSubsystem.stopIntake(m_driverController.getHID())*/)); m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID())); m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID()));
m_driverController.y().onTrue(intakeSubsystem.deploy()); m_driverController.y().onTrue(intakeSubsystem.deploy());
} }

4
src/main/java/frc/robot/Utils.java
View File

@ -101,14 +101,14 @@ public class Utils {
} }
public static double getHoodAngleForDistance(double distanceInMeters){ public static double getHoodAngleForDistance(double distanceInMeters){
if(distanceInMeters >= 3){ if(distanceInMeters >= 4){
return 200; return 200;
} }
return 0; return 0;
} }
public static double getLauncherRPMForDistance(double distanceInMeters){ public static double getLauncherRPMForDistance(double distanceInMeters){
return Math.min((667.557*distanceInMeters) + 1024.699, 6000); return Math.min((667.557*distanceInMeters) + 1500.699, 5000);
} }
/** /**

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

@ -149,7 +149,6 @@ public class DriveSubsystem extends TestableSubsystem {
Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates()); Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates()); Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose3d());
} }
}, this)); }, this));

14
src/main/java/frc/robot/subsystems/HopperSubsystem.java
View File

@ -17,6 +17,9 @@ import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.units.Units; import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants; import frc.robot.Constants;
public class HopperSubsystem extends TestableSubsystem { public class HopperSubsystem extends TestableSubsystem {
@ -86,6 +89,17 @@ public class HopperSubsystem extends TestableSubsystem {
spindexerMotor.stopMotor(); spindexerMotor.stopMotor();
} }
public Command spinny() {
return this.runOnce(() -> {
if (spindexerMotor.get() == 0) {
spindexerOn();
} else {
spindexerOff();
}
});
}
@Override @Override
public void periodic() { public void periodic() {
Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond)); Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));

62
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -115,7 +115,7 @@ public class TurretSubsystem extends TestableSubsystem {
SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
config.voltageCompensation(10); config.voltageCompensation(10);
config.closedLoop.p(0.0001).i(0.000004).d(0.000007); // I = 0.0000001 config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001
config.closedLoop.feedForward.kS(0.2).kV(0.000).kA(0.000); config.closedLoop.feedForward.kS(0.2).kV(0.000).kA(0.000);
// config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
@ -203,57 +203,45 @@ public class TurretSubsystem extends TestableSubsystem {
@Override @Override
public void periodic() { public void periodic() {
if(turretState == TurretState.HOMING) if (turretState == TurretState.HOMING) {
{ if (turretLimitSwitch.get()) { // Turret has tripped the switch
while(turretState != TurretState.READY) turretMotor.stopMotor();
{ turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
long start = System.currentTimeMillis(); if (!Robot.isReal()) {
while(System.currentTimeMillis()-1000<start) turretOffsetInRotations = 0; // Flip turret around
{ }
if(turretLimitSwitch.get()) turretState = TurretState.READY;
{ // Turret has tripped the switch forwardLimit = (1.5 + turretOffsetInRotations);
turretMotor.stopMotor(); reverseLimit = (-1.5 + turretOffsetInRotations);
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323); Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
if(!Robot.isReal()){ Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretOffsetInRotations = 0; // Flip turret around turretMotor.getConfigurator().apply(
} new SoftwareLimitSwitchConfigs()
turretState = TurretState.READY;
forwardLimit = (1.5 + turretOffsetInRotations);
reverseLimit = (-1.5 + turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretMotor.getConfigurator().apply(
new SoftwareLimitSwitchConfigs()
.withForwardSoftLimitEnable(true) .withForwardSoftLimitEnable(true)
.withForwardSoftLimitThreshold(forwardLimit) .withForwardSoftLimitThreshold(forwardLimit)
.withReverseSoftLimitEnable(true) .withReverseSoftLimitEnable(true)
.withReverseSoftLimitThreshold(reverseLimit) .withReverseSoftLimitThreshold(reverseLimit));
); // setTurretRobotRelative(0);
// setTurretRobotRelative(0); // setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
// setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}
if(turretState != TurretState.READY)
{
turretMotor.set((turretMotor.get()*-1));
}
} }
} }
if(LOGGING) if (LOGGING) {
{
Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get()); Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-TurretState", turretState); Logger.recordOutput("TurretSubsystem-TurretState", turretState);
Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative()); Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition()); Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Rotations)); Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw",
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Rotations)); turretMotorPosition.refresh().getValue().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw",
turretControlMode.getPositionMeasure().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative()); Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations); Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretPositionError", turretPositionError.refresh().getValueAsDouble()); Logger.recordOutput("TurretSubsystem-TurretPositionError", turretPositionError.refresh().getValueAsDouble());
Logger.recordOutput("TurretSubsystem-LauncherSetpoint(RPM)", launcherControler.getSetpoint());
} }
} }
@ -314,7 +302,7 @@ public class TurretSubsystem extends TestableSubsystem {
} }
public boolean isTurretAtTarget(){ public boolean isTurretAtTarget(){
return turretPositionError.refresh().getValueAsDouble() <= 0.1; return Math.abs(turretPositionError.refresh().getValueAsDouble()) <= 0.1;
} }
private double getRequestedPosTurretRelative() { private double getRequestedPosTurretRelative() {