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/RobotContainer.java
+14 -13
View File
@@ -254,20 +254,21 @@ public class RobotContainer {
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
.whileTrue(new RunCommand(() -> m_robotSwerveDrive.driveFacingPosition(
getDeadbandedDriverController().getLeft(),
FieldPositions.HUB_POSITION,
ShooterConstants.AIM_LEAD_TIME.get()
), m_robotSwerveDrive)
// () -> {
// m_robotSwerveDrive.driveFacingVelocity(
// getDeadbandedDriverController().getLeft(),
.whileTrue(new RunCommand(
// () -> m_robotSwerveDrive.driveFacingPosition(
// getDeadbandedDriverController().getLeft(),
// FieldPositions.HUB_POSITION,
// ShooterConstants.AIM_LEAD_TIME.get(),
// m_robotShooter.getBallVelocity(),
// m_robotShooter.getDistanceToHub()
// );
// }, m_robotSwerveDrive)
// ShooterConstants.AIM_LEAD_TIME.get()
// ), m_robotSwerveDrive)
() -> {
m_robotSwerveDrive.driveFacingVelocity(
getDeadbandedDriverController().getLeft(),
FieldPositions.HUB_POSITION,
ShooterConstants.AIM_LEAD_TIME.get(),
m_robotShooter.getBallVelocity(),
m_robotShooter.getDistanceToHub()
);
}, m_robotSwerveDrive)
);
// D-PAD fine alignment
src/main/java/frc4388/robot/constants/BuildConstants.java
+5 -5
View File
@@ -7,12 +7,12 @@ public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2026KPopRobotHunters";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 162;
public static final String GIT_SHA = "aac68dad5eabfaa1e098ad8cf6e42cd3bddd1ef3";
public static final String GIT_DATE = "2026-03-14 20:20:55 MDT";
public static final int GIT_REVISION = 164;
public static final String GIT_SHA = "f4a42f02eed0fa3b0950a05da55174b9f4b89ce7";
public static final String GIT_DATE = "2026-03-14 20:39:45 MDT";
public static final String GIT_BRANCH = "MiraOrg";
public static final String BUILD_DATE = "2026-03-14 20:33:21 MDT";
public static final long BUILD_UNIX_TIME = 1773542001176L;
public static final String BUILD_DATE = "2026-03-15 11:14:11 MDT";
public static final long BUILD_UNIX_TIME = 1773594851399L;
public static final int DIRTY = 1;
private BuildConstants(){}
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())