Skip to content

Cleaned Up Shoot On the Move (FIXED) #21

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Carbonic-Acidd merged 1 commit into from
Jan 27, 2026
Merged

Cleaned Up Shoot On the Move (FIXED) #21

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 0 additions & 10 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -313,16 +313,6 @@ public static class HoodConstants {
.withFeedback(feedbackConfigs)
.withMotorOutput(motorOutputConfigs)
.withSoftwareLimitSwitch(softwareLimitSwitchConfigs);

public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap();

static {
hoodAngleMap.put(1.5, 1.0);
hoodAngleMap.put(2.0, 2.0);
hoodAngleMap.put(2.5, 3.0);
hoodAngleMap.put(3.0, 4.0);
hoodAngleMap.put(3.53, 5.0); // random values (Distance, Rotations)
}
}

public static class SpindexerConstants {
Expand Down
5 changes: 2 additions & 3 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
import frc.robot.Constants.FieldConstants;
import frc.robot.Constants.OperatorConstants;
import frc.robot.Constants.SwerveConstants;
import frc.robot.commands.AimandShootStationary;
import frc.robot.commands.PhysicsStationaryShoot;
import frc.robot.commands.MoveToFuel;
import frc.robot.commands.ShootOnTheMove;
import frc.robot.commands.TeleopSwerve;
Expand Down Expand Up @@ -135,9 +135,8 @@ private void configureDriverBindings() {
turret.setDefaultCommand(turret.faceTarget(AllianceUtil::getHubPose, swerve::getRobotPose));

hood.setDefaultCommand(
new AimandShootStationary(
new PhysicsStationaryShoot(
shooter, hood, swerve::getRobotPose, () -> AllianceUtil.getHubPose()));

}

private void configureOperatorBindings() {
Expand Down
4 changes: 2 additions & 2 deletions ...robot/commands/AimandShootStationary.java → ...obot/commands/PhysicsStationaryShoot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,13 @@

/* You should consider using the more terse Command factories API instead
https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class AimandShootStationary extends Command {
public class PhysicsStationaryShoot extends Command {
private final Shooter shooter;
private final Hood hood;
private final Supplier<Pose2d> robotPose;
private final Supplier<Pose2d> targetPose;

public AimandShootStationary(
public PhysicsStationaryShoot(
Shooter shooter, Hood hood, Supplier<Pose2d> robotPose, Supplier<Pose2d> targetPose) {
this.shooter = shooter;
this.hood = hood;
Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/robot/commands/ShootOnTheMove.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,6 @@ public void initialize() {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {

ChassisSpeeds fieldSpeeds = swerve.getChassisSpeeds();

ChassisSpeeds fieldAcceleration = fieldSpeeds.minus(previousSpeeds).div(0.020);
Expand Down
139 changes: 6 additions & 133 deletions src/main/java/frc/robot/subsystems/Hood.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.HoodConstants;
import frc.robot.Constants.TurretConstants;
import frc.robot.util.SOTMCalculator;

import java.util.function.Supplier;

public class Hood extends SubsystemBase {
Expand All @@ -35,8 +37,6 @@ public class Hood extends SubsystemBase {

private SingleJointedArmSim hoodSim;

private final InterpolatingDoubleTreeMap hoodAngleMap = HoodConstants.hoodAngleMap;

private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0);

private Angle targetAngle = HoodConstants.minAngle;
Expand Down Expand Up @@ -74,95 +74,15 @@ public boolean atAngle(Angle target, Angle tolerance) {
return Math.abs(getHoodAngle().minus(target).in(Degrees)) < tolerance.in(Degrees);
}

public double getInterpolatedHoodAngle(double distanceMeters) {
return hoodAngleMap.get(distanceMeters);
public Angle getInterpolatedHoodAngle(double distanceMeters) {
return SOTMCalculator.hoodAngleMap.get(distanceMeters).getMeasure();
}

public double getInterpolatedHoodAngle(Pose2d poseA, Pose2d poseB) {
public Angle getInterpolatedHoodAngle(Pose2d poseA, Pose2d poseB) {
double distance = poseA.getTranslation().getDistance(poseB.getTranslation());
return hoodAngleMap.get(distance);
return SOTMCalculator.hoodAngleMap.get(distance).getMeasure();
}

// public Angle getPhysicsHoodAngle(
// Distance distance,
// Distance targetHeight,
// Distance shooterHeight,
// LinearVelocity exitVelocity) {
// double distanceMeters = distance.in(Meters);
// double targetHeightMeters = targetHeight.in(Meters);
// double shooterHeightMeters = shooterHeight.in(Meters);
// double exitVelocityMetersPerSec = exitVelocity.in(MetersPerSecond);

// double vSq = exitVelocityMetersPerSec * exitVelocityMetersPerSec;

// double dSq = distanceMeters * distanceMeters;

// double dH = targetHeightMeters - shooterHeightMeters;

// double g = 9.80665;
// // long physics equation i derived :D -> dont ask idk if it works
// double numerator =
// (vSq * distanceMeters)
// + Math.sqrt((vSq * vSq * dSq) - (g * dSq * ((2 * vSq * dH) + (g * dSq))));

// double denominator = g * dSq;

// return Radians.of(Math.atan2(numerator, denominator));
// }

// public Angle getPhysicsHoodAngle(
// Distance distance,
// Distance targetHeight,
// Distance shooterHeight,
// LinearVelocity exitVelocity) {
// double distanceMeters = distance.in(Meters);
// double targetHeightMeters = targetHeight.in(Meters);
// double shooterHeightMeters = shooterHeight.in(Meters);
// double exitVelocityMetersPerSec = exitVelocity.in(MetersPerSecond);

// double vSq = exitVelocityMetersPerSec * exitVelocityMetersPerSec;

// double dH = targetHeightMeters - shooterHeightMeters;

// double g = 9.80665;

// double discriminant = (vSq * vSq) - g * (g * distanceMeters * distanceMeters + 2 * dH * vSq);

// if (discriminant < 0) {
// return Radians.of(Double.NaN);
// }

// Angle lowShotAngle =
// Radians.of(Math.atan((vSq - Math.sqrt(discriminant)) / (g * distanceMeters)));
// Angle highShotAngle =
// Radians.of(Math.atan((vSq + Math.sqrt(discriminant)) / (g * distanceMeters)));

// Angle highHoodAngle = Degrees.of(90).minus(lowShotAngle);
// Angle lowHoodAngle = Degrees.of(90).minus(highShotAngle);

// boolean lowValid =
// lowHoodAngle.in(Degrees) > HoodConstants.minAngle.in(Degrees)
// && lowHoodAngle.in(Degrees) < HoodConstants.maxAngle.in(Degrees);

// boolean highValid =
// highHoodAngle.in(Degrees) > HoodConstants.minAngle.in(Degrees)
// && highHoodAngle.in(Degrees) < HoodConstants.maxAngle.in(Degrees);

// SmartDashboard.putNumber("Hood/Physics/AngleChoices/LowHoodAngle", lowHoodAngle.in(Degrees));
// SmartDashboard.putNumber("Hood/Physics/AngleChoices/HighHoodAngle",
// highHoodAngle.in(Degrees));

// if (lowValid && highValid) {
// return lowHoodAngle;
// } else if (lowValid) {
// return lowHoodAngle;
// } else if (highValid) {
// return highHoodAngle;
// } else {
// return Radians.of(Double.NaN);
// }
// }

public void stopHood() {
hoodMotor.stopMotor();
}
Expand Down Expand Up @@ -208,53 +128,6 @@ public Command aimForTarget(
.withName("Aim Hood At Target");
}

// public Command aimWithPhysics(
// Supplier<Pose2d> targetPoseSupplier,
// Supplier<Pose2d> robotPoseSupplier,
// Supplier<LinearVelocity> exitVelocitySupplier) {
// return run(() -> {
// Pose2d turretPose =
// robotPoseSupplier
// .get()
// .transformBy(
// new Transform2d(
// TurretConstants.robotToTurret.toTranslation2d(), Rotation2d.kZero));
// Distance distance =
// Meters.of(
// turretPose
// .getTranslation()
// .getDistance(targetPoseSupplier.get().getTranslation()));

// // Angle hoodAngle =
// // getPhysicsHoodAngle(
// // distance,
// // FieldConstants.hubHeight,
// // TurretConstants.robotToTurret.getMeasureZ(),
// // exitVelocitySupplier.get());
// ShotSolution solution =
// solveShot(
// distance,
// FieldConstants.hubHeight,
// TurretConstants.robotToTurret.getMeasureZ(),
// exitVelocitySupplier.get());

// Angle hoodAngle = solution.hoodAngle();

// SmartDashboard.putNumber("Hood/Physics/ Chosen Angle ", hoodAngle.in(Degrees));
// SmartDashboard.putNumber("Hood/Physics/distance ", distance.in(Meters));
// SmartDashboard.putNumber(
// "Hood/Physics/D-Height ",
// FieldConstants.hubHeight
// .minus(TurretConstants.robotToTurret.getMeasureZ())
// .in(Meters));
// SmartDashboard.putNumber(
// "Hood/Physics/Velocity ", exitVelocitySupplier.get().in(MetersPerSecond));

// hoodMotor.setControl(motionMagicRequest.withPosition(hoodAngle.in(Rotations)));
// })
// .withName("Aim Hood (Physics)");
// }

@Override
public void periodic() {
hoodPosition.refresh();
Expand Down
7 changes: 7 additions & 0 deletions src/main/java/frc/robot/subsystems/Swerve.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -874,9 +874,16 @@ public ChassisSpeeds getChassisSpeeds() {

@Logged(name = "Rotation")
public Rotation2d getRotation() {
return stateCache.Pose.getRotation();
}


@Logged(name = "Pigeon Rotation")
public Rotation2d getPigeonRotation() {
return getPigeon2().getRotation2d();
}


@Logged(name = "Module States")
public SwerveModuleState[] getModuleStates() {
return stateCache.ModuleStates;
Expand Down
73 changes: 54 additions & 19 deletions src/main/java/frc/robot/util/SOTMCalculator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.interpolation.InterpolatingTreeMap;
import edu.wpi.first.math.interpolation.InverseInterpolator;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.Time;
Expand All @@ -17,45 +19,78 @@
import frc.robot.subsystems.Turret;

public class SOTMCalculator {
private static InterpolatingDoubleTreeMap hoodAngleMap = HoodConstants.hoodAngleMap;
private static InterpolatingDoubleTreeMap shooterSpeedMap = new InterpolatingDoubleTreeMap();
private static InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap();
private static Transform2d robotToTurret =
new Transform2d(TurretConstants.robotToTurret.toTranslation2d(), Rotation2d.kZero);
private static Time feedTime = Seconds.of(0.1353);

public static InterpolatingTreeMap<Double, Rotation2d> hoodAngleMap =
new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate);
public static InterpolatingDoubleTreeMap shooterSpeedMap = new InterpolatingDoubleTreeMap();
public static InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap();

static {
hoodAngleMap.put(0.8, Rotation2d.fromDegrees(16.0));
hoodAngleMap.put(0.97, Rotation2d.fromDegrees(17.0));
hoodAngleMap.put(1.1, Rotation2d.fromDegrees(18.0));
hoodAngleMap.put(1.21, Rotation2d.fromDegrees(18.5));
hoodAngleMap.put(1.34, Rotation2d.fromDegrees(19.0));
hoodAngleMap.put(1.78, Rotation2d.fromDegrees(19.0));
hoodAngleMap.put(2.17, Rotation2d.fromDegrees(24.0));
hoodAngleMap.put(2.81, Rotation2d.fromDegrees(27.0));
hoodAngleMap.put(3.82, Rotation2d.fromDegrees(29.0));
hoodAngleMap.put(4.09, Rotation2d.fromDegrees(30.0));
hoodAngleMap.put(4.40, Rotation2d.fromDegrees(31.0));
hoodAngleMap.put(4.77, Rotation2d.fromDegrees(32.0));
hoodAngleMap.put(5.57, Rotation2d.fromDegrees(32.0));
hoodAngleMap.put(5.60, Rotation2d.fromDegrees(35.0)); // Random values

shooterSpeedMap.put(1.5, 1.0);
shooterSpeedMap.put(2.0, 2.0);
shooterSpeedMap.put(2.5, 3.0);
shooterSpeedMap.put(3.0, 4.0);
shooterSpeedMap.put(3.53, 5.0); // random values (Distance, RPM)

timeOfFlightMap.put(1.5, 1.0);
timeOfFlightMap.put(2.0, 1.0);
timeOfFlightMap.put(2.5, 1.0);
timeOfFlightMap.put(3.0, 1.0);
timeOfFlightMap.put(3.53, 1.0); // random values (Distance, seconds)
}

private static Translation2d robotToTurret2d = TurretConstants.robotToTurret.toTranslation2d();
public static Time feedTime = Seconds.of(0.1353);


public record ShootingParameters(double shooterSpeed, Angle turretAngle, Angle hoodAngle) {}

public static ShootingParameters getParameters(
Swerve swerve, Turret turret, Pose2d target, double fieldAccelX, double fieldAccelY) {
Pose2d targetPose = target;
Pose2d turretPose = swerve.getRobotPose().transformBy(robotToTurret);
Translation2d turretPose = swerve.getRobotPose().getTranslation().plus(robotToTurret2d);

ChassisSpeeds robotVelocity = swerve.getChassisSpeeds();

double robotAngle = swerve.getRobotPose().getRotation().getRadians();
double turretVelocityX =
robotVelocity.vxMetersPerSecond
+ robotVelocity.omegaRadiansPerSecond
* (robotToTurret.getY() * Math.cos(robotAngle)
- robotToTurret.getX() * Math.sin(robotAngle));
* (robotToTurret2d.getY() * Math.cos(robotAngle)
- robotToTurret2d.getX() * Math.sin(robotAngle));

double turretVelocityY =
robotVelocity.vyMetersPerSecond
+ robotVelocity.omegaRadiansPerSecond
* (robotToTurret.getX() * Math.cos(robotAngle)
- robotToTurret.getY() * Math.sin(robotAngle));
* (robotToTurret2d.getX() * Math.cos(robotAngle)
- robotToTurret2d.getY() * Math.sin(robotAngle));

Translation2d lookAheadPosition = targetPose.getTranslation();

double turretToTargetDistance = lookAheadPosition.getDistance(turretPose.getTranslation());
double turretToTargetDistance = lookAheadPosition.getDistance(turretPose);

Angle turretAngle = turret.angleToFaceTarget(lookAheadPosition, swerve.getRobotPose());
Angle hoodAngle = Rotations.of(hoodAngleMap.get(turretToTargetDistance));
Rotation2d hoodAngle = hoodAngleMap.get(turretToTargetDistance);
double shooterSpeed = shooterSpeedMap.get(turretToTargetDistance);

double timeOfFlight = timeOfFlightMap.get(turretToTargetDistance);

double distance = lookAheadPosition.getDistance(turretPose.getTranslation());
double distance = lookAheadPosition.getDistance(turretPose);

for (int i = 0; i < 5; i++) {
// + accelX * feedTime.in(Seconds) * 0.5
Expand All @@ -65,14 +100,14 @@ public static ShootingParameters getParameters(

lookAheadPosition = targetPose.getTranslation().minus(new Translation2d(offsetX, offsetY));

double newDistance = lookAheadPosition.getDistance(turretPose.getTranslation());
double newDistance = lookAheadPosition.getDistance(turretPose);

timeOfFlight = timeOfFlightMap.get(newDistance);
Angle newTurretAngle = turret.angleToFaceTarget(lookAheadPosition, swerve.getRobotPose());
Angle newHoodAngle = Rotations.of(hoodAngleMap.get(newDistance));
Rotation2d newHoodAngle = hoodAngleMap.get(newDistance);
double newShooterSpeed = shooterSpeedMap.get(newDistance);

boolean hasConverged = Math.abs(newDistance - distance) < 0.005;
boolean hasConverged = Math.abs(newDistance - distance) < 0.00005;

turretAngle = newTurretAngle;
hoodAngle = newHoodAngle;
Expand All @@ -84,6 +119,6 @@ public static ShootingParameters getParameters(
break;
}
}
return new ShootingParameters(shooterSpeed, turretAngle, hoodAngle);
}
return new ShootingParameters(shooterSpeed, turretAngle, hoodAngle.getMeasure());
}
}
Loading