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

Add aim lead

Browse Source
This commit is contained in:
Michael Mikovsky
2026-01-18 12:53:07 -07:00
parent a27cde3f84
commit c68329c9ca
6 changed files with 55 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+25 -8
View File
@@ -251,14 +251,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
io.setControl(ctrl);
}
// Drive with a specific velocity and heading
public void driveFieldAngle(Translation2d leftStick, Rotation2d heading) {
if (leftStick.getNorm() < 0.05) // if no imput and the swerve drive is still going:
stopModules(); // stop the swerve
// if (leftStick.getNorm() < 0.05) // if no imput
// return; // don't bother doing swerve drive math and return early.
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
@@ -268,13 +265,33 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(heading);
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
SwerveDriveConstants.PIDConstants.AIM_kP.get(),
SwerveDriveConstants.PIDConstants.AIM_kI.get(),
SwerveDriveConstants.PIDConstants.AIM_kD.get()
// SwerveDriveConstants.PIDConstants.AIM_GAINS.kP,
// SwerveDriveConstants.PIDConstants.AIM_GAINS.kI,
// SwerveDriveConstants.PIDConstants.AIM_GAINS.kD
);
io.setControl(ctrl);
// SmartDashboard.putBoolean("drift correction", true);
}
// Drive with the robot facing towards a specific position
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {
// Get the current speed of the robot
Translation2d robotSpeed = new Translation2d(
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond
);
// Calculate a point to aim ahead of the actual position.
Translation2d fieldPosLead = robotSpeed.times(SwerveDriveConstants.AIM_LEAD_TIME.get()).plus(fieldPos);
// Calculate the angle between the current position and the lead position
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
driveFieldAngle(leftStick, ang);
}
public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java
+12
View File
@@ -22,6 +22,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.units.measure.Angle;
import frc4388.utility.configurable.ConfigurableDouble;
//import edu.wpi.first.units.measure.measure.Distance;
import frc4388.utility.status.CanDevice;
import frc4388.utility.structs.Gains;
@@ -37,6 +38,9 @@ public final class SwerveDriveConstants {
public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
// TODO: Replace with a constant
public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50;
@@ -156,6 +160,14 @@ public final class SwerveDriveConstants {
public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
// TODO: Replace this with a static constant
public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 2.5);
public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0);
public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1);
// public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1);
}
public static final class Configurations {