Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Fix offset model

Browse Source
This commit is contained in:
mimigamin
2026-03-15 11:17:55 -06:00
parent f4a42f02ee
commit d5f316b0c1
3 changed files with 31 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+12 -10
View File
@@ -409,13 +409,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
chassisSpeeds.vyMetersPerSecond
);
if (ballVelocity > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){
if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){
double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity));
fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
Logger.recordOutput("Aim Offset", aimOffset);
// double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub)));
// Logger.recordOutput("Aim Offset", aimOffset);
fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
Logger.recordOutput("Offset", aimOffset);
}
@@ -424,6 +424,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
Logger.recordOutput("Aim at Offset", fieldPos);
driveFieldAngle(leftStick, ang);
}
@@ -531,6 +532,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
vision.setLastOdomPose(state.currentPose);
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
// if (state.speeds != null) {
// this.chassisSpeeds = state.speeds;
// } else {
// this.chassisSpeeds = new ChassisSpeeds();
// }
if (vision.isTag()) {
Pose2d pose = vision.getPose2d();
if (!robotKnowsWhereItIs) {
@@ -545,11 +552,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
vision.setLastOdomPose(state.currentPose);
setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
if (state.speeds != null) {
this.chassisSpeeds = state.speeds;
} else {
this.chassisSpeeds = new ChassisSpeeds();
}
}
// if(e.isPresent())