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.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID())));
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.x().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((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID()));
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){
if(distanceInMeters >= 3){
if(distanceInMeters >= 4){
return 200;
}
return 0;
}
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("ActualSwerveState", driveSystem.getActualModuleStates());
Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose3d());
}
}, 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.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;
public class HopperSubsystem extends TestableSubsystem {
@ -86,6 +89,17 @@ public class HopperSubsystem extends TestableSubsystem {
spindexerMotor.stopMotor();
}
public Command spinny() {
return this.runOnce(() -> {
if (spindexerMotor.get() == 0) {
spindexerOn();
} else {
spindexerOff();
}
});
}
@Override
public void periodic() {
Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));

36
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);
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
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.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
@ -203,18 +203,11 @@ public class TurretSubsystem extends TestableSubsystem {
@Override
public void periodic() {
if(turretState == TurretState.HOMING)
{
while(turretState != TurretState.READY)
{
long start = System.currentTimeMillis();
while(System.currentTimeMillis()-1000<start)
{
if(turretLimitSwitch.get())
{ // Turret has tripped the switch
if (turretState == TurretState.HOMING) {
if (turretLimitSwitch.get()) { // Turret has tripped the switch
turretMotor.stopMotor();
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
if(!Robot.isReal()){
if (!Robot.isReal()) {
turretOffsetInRotations = 0; // Flip turret around
}
turretState = TurretState.READY;
@ -227,33 +220,28 @@ public class TurretSubsystem extends TestableSubsystem {
.withForwardSoftLimitEnable(true)
.withForwardSoftLimitThreshold(forwardLimit)
.withReverseSoftLimitEnable(true)
.withReverseSoftLimitThreshold(reverseLimit)
);
.withReverseSoftLimitThreshold(reverseLimit));
// setTurretRobotRelative(0);
// 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-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-TurretState", turretState);
Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw",
turretMotorPosition.refresh().getValue().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw",
turretControlMode.getPositionMeasure().in(Units.Rotations));
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
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(){
return turretPositionError.refresh().getValueAsDouble() <= 0.1;
return Math.abs(turretPositionError.refresh().getValueAsDouble()) <= 0.1;
}
private double getRequestedPosTurretRelative() {