Skip to content

Sotm acounting for rotational velocity #68

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

Open
vivi-o wants to merge 4 commits into
base: sotm-adjustment
Choose a base branch
from
+28 −13
Open

Sotm acounting for rotational velocity #68

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
049262e
account for rotational vellcity i think
vivi-o Mar 21, 2026
aec33d9
fix because turret pose was being adjusted for twice
vivi-o Mar 21, 2026
a4262a2
iterate for turret because why not
vivi-o Mar 21, 2026
e6be9e2
build
vivi-o Mar 21, 2026
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
41 changes: 28 additions & 13 deletions src/main/java/frc/robot/utils/autoaim/AutoAim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -225,19 +225,33 @@ public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPos

public static Rotation2d getVirtualTargetYaw(
Translation2d target, ChassisSpeeds fieldRelativeSpeeds, Pose2d robotPose, double tof) {
Translation2d vtarget = getVirtualSOTMTarget(target, fieldRelativeSpeeds, tof);
Translation2d vtarget = getVirtualSOTMTarget(robotPose, target, fieldRelativeSpeeds, tof);
return getTargetRotation(vtarget, robotPose);
}

// lock in
public static Translation2d getVirtualSOTMTarget(
Translation2d target, ChassisSpeeds fieldRelativeSpeeds, double timeOfFlightSecs) {
// velocity times shot time is how translated it is
Translation2d vtarget =
target.minus(
new Translation2d(
fieldRelativeSpeeds.vxMetersPerSecond * timeOfFlightSecs,
fieldRelativeSpeeds.vyMetersPerSecond * timeOfFlightSecs));
Pose2d robotPose,
Translation2d target,
ChassisSpeeds fieldRelativeSpeeds,
double timeOfFlightSecs) {

Translation2d vtarget = target;
Pose2d vrobot = robotPose;

for (int i = 0; i < 20; i++) {
double tof = COMP_HUB_SHOT_TREE.get(distanceToHub(vrobot)).timeOfFlightSecs();
vrobot =
robotPose.exp(
new Twist2d(
fieldRelativeSpeeds.vxMetersPerSecond * (tof),
fieldRelativeSpeeds.vyMetersPerSecond * (tof),
fieldRelativeSpeeds.omegaRadiansPerSecond * (tof)));

Translation2d delta = vrobot.getTranslation().minus(robotPose.getTranslation());

vtarget = target.minus(delta);
}
Logger.recordOutput("Autoaim/Virtual Target", vtarget);
return vtarget;
}
Expand Down Expand Up @@ -315,7 +329,7 @@ public static ShotData getSOTMShotData(
ShotData unadjustedShot = tree.calculateShot(robotPose, targetTranslation);
Translation2d virtualTarget =
getVirtualSOTMTarget(
targetTranslation, fieldRelativeSpeeds, unadjustedShot.timeOfFlightSecs());
robotPose, targetTranslation, fieldRelativeSpeeds, unadjustedShot.timeOfFlightSecs());
Pose2d turretPose = getTurretPose(robotPose);

return tree.get(turretPose.getTranslation().getDistance(virtualTarget));
Expand All @@ -329,15 +343,16 @@ public static ShotData getSOTMShotDataNewtonsMethod(

ShotData baseline = tree.calculateShot(robotPose, targetTranslation);

Pose2d turretPose = getTurretPose(robotPose);
Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation());
// Pose2d turretPose = getTurretPose(robotPose);
// Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation());

double distance = turretToTarget.getNorm();
Translation2d robotToTarget = targetTranslation.minus(robotPose.getTranslation());
double distance = robotToTarget.getNorm();

// get just direction of vector because its vector divded by length
// dont want to account for magnitude bc the speed we are going and shot tree do
// and we just want direction to find dot product
Translation2d shotDirection = turretToTarget.div(distance);
Translation2d shotDirection = robotToTarget.div(distance);

// dot product! <3
// get how fast we are going towards where we are shooting
Expand Down
Loading