From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Sat, 28 Feb 2026 22:36:26 +0000 (-0800) Subject: a X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=1c242c32a88bded21e0894e5d13fa98a07612ed9;p=FRC2026.git a --- diff --git a/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto b/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto index db0b7a0..82643b4 100644 --- a/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto +++ b/src/main/deploy/pathplanner/autos/Left(No SOTM) - Under Trench.auto @@ -13,7 +13,7 @@ { "type": "path", "data": { - "pathName": "#2 Left(No SOTM) under the trench" + "pathName": "Duplicated Not Working" } }, { diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java index eb884e1..05ae1b6 100644 --- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java +++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java @@ -30,7 +30,8 @@ public class ReverseMotors extends Command { @Override public void end(boolean interrupted){ - intake.spinStop(); + intake.extend(); + intake.spinStart(); //spindexer.maxSpindexer(); } diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 6112b51..e4aaf45 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -1,5 +1,7 @@ package frc.robot.commands.gpm; +import static edu.wpi.first.units.Units.Rotation; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; @@ -68,6 +70,8 @@ public class Superstructure extends Command { private double turretOffset = 0.0; + private double distanceFromTarget = 0.0; + public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) { this.turret = turret; this.drivetrain = drivetrain; @@ -126,7 +130,7 @@ public class Superstructure extends Command { target == FieldConstants.getHubTranslation().toTranslation2d() ? 2.0 : 2.0); - double TOFAdjustment = 0.75; + double TOFAdjustment = 0.85; timeOfFlight = goalState.timeOfFlight() * TOFAdjustment; double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; @@ -175,6 +179,7 @@ public class Superstructure extends Command { hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); lastHoodAngle = hoodAngle; + distanceFromTarget = target.getDistance(lookaheadPose.getTranslation()); } public void updateDrivePose(){ @@ -226,10 +231,13 @@ public class Superstructure extends Command { // if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){ // hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); // } else{ - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity); + //hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity); // } - shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); + shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget)); + Logger.recordOutput("Distance From Target", distanceFromTarget); + //shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); // if (phaseManager.shouldFeed()) { // spindexer.maxSpindexer(); diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java index 4f8ea0d..fc6c2b6 100644 --- a/src/main/java/frc/robot/constants/ShotInterpolation.java +++ b/src/main/java/frc/robot/constants/ShotInterpolation.java @@ -11,6 +11,10 @@ public class ShotInterpolation { public static final InterpolatingDoubleTreeMap exitVelocityMap = new InterpolatingDoubleTreeMap(); + public static final InterpolatingDoubleTreeMap shooterVelocityMap = new InterpolatingDoubleTreeMap(); + + public static final InterpolatingDoubleTreeMap newHoodMap = new InterpolatingDoubleTreeMap(); + static{ timeOfFlightMap.put(0.0, 0.67); timeOfFlightMap.put(1.0, 0.67); @@ -41,5 +45,26 @@ public class ShotInterpolation { exitVelocityMap.put(11.0, 26.0); exitVelocityMap.put(25.0, 25.0* 3.2); //exitVelocityMap.put(null, null); + + shooterVelocityMap.put(1.49, 11.5); + shooterVelocityMap.put(2.09, 12.5); + shooterVelocityMap.put(2.95, 13.5); + shooterVelocityMap.put(5.05, 16.0); + shooterVelocityMap.put(5.79, 17.0); + shooterVelocityMap.put(4.07, 15.5); + + shooterVelocityMap.put(0.0, 9.55); + shooterVelocityMap.put(25.0, 43.44); + + newHoodMap.put(1.49, 72.0); + newHoodMap.put(2.09, 70.0); + newHoodMap.put(2.95, 68.0); + newHoodMap.put(5.05, 60.0); + newHoodMap.put(5.79, 59.0); + newHoodMap.put(4.07, 65.0); + + newHoodMap.put(0.0, 75.9); + newHoodMap.put(27.99, 0.0); + } } diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index ade7e35..d19db1b 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -99,6 +99,9 @@ public class Hood extends SubsystemBase implements HoodIO{ updateInputs(); Logger.processInputs("Hood", inputs); + // goalAngle = Rotation2d.fromDegrees(SmartDashboard.getNumber("Hood Setpoint", goalAngle.getDegrees())); + // SmartDashboard.putNumber("Hood Setpoint", goalAngle.getDegrees()); + if (forceHoodDown){ goalAngle = Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE); goalVelocityRadPerSec = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9b1a45a..7b8ff5e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -30,7 +30,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - double powerModifier = 1.0; + double powerModifier = 1.3; public Shooter(){ updateInputs(); @@ -59,6 +59,9 @@ public class Shooter extends SubsystemBase implements ShooterIO { public void periodic(){ updateInputs(); + // shooterTargetSpeed = SmartDashboard.getNumber("Shooter Setpoint", shooterTargetSpeed); + // SmartDashboard.putNumber("Shooter Setpoint", shooterTargetSpeed); + powerModifier = SmartDashboard.getNumber("shooter power modifier", powerModifier); SmartDashboard.putNumber("shooter power modifier", powerModifier);