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

Finish X position and PID position

Browse Source
This commit is contained in:
mimigamin
2026-03-14 18:57:04 -06:00
parent ebab028818
commit 8c8ac26139
7 changed files with 76 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+12 -12
View File
@@ -233,21 +233,21 @@ public class RobotContainer {
//TEST - > Defense: X position on wheels and swerve drive pid on position
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.whileTrue(new RunCommand(() -> {
m_robotSwerveDrive.defenseXPosition();
}, m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> {
m_robotSwerveDrive.stopDefenseXPosition();
// .onTrue(new InstantCommand(() -> {
// currentPose = m_robotSwerveDrive.getCurrentPose();
// }))
// .whileTrue(new RunCommand(() -> {
// m_stayInPosition.goToTargetPose(currentPose);
// m_robotSwerveDrive.defenseXPosition();
// }, m_robotSwerveDrive))
// .onFalse(new InstantCommand(() -> {
// m_robotSwerveDrive.softStop();
// }));
// m_robotSwerveDrive.stopDefenseXPosition();
.onTrue(new InstantCommand(() -> {
currentPose = m_robotSwerveDrive.getCurrentPose();
}))
.whileTrue(new RunCommand(() -> {
m_stayInPosition.goToTargetPose(currentPose);
}, m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> {
m_robotSwerveDrive.softStop();
}));
src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java
+16 -22
View File
@@ -5,52 +5,46 @@
package frc4388.robot.commands.Swerve;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
import frc4388.robot.subsystems.swerve.SwerveDrive;
public class StayInPosition extends PID {
SwerveDrive drive;
/** Creates a new StayInPosition. */
public StayInPosition(SwerveDrive drive) {
super(0.3, 0.0, 0.0, 0.0, 1);
this.drive = drive;
addRequirements(drive);
}
public void goToTargetPose(Pose2d targetPose) {
Pose2d currentPose = drive.getCurrentPose();
double translationX = targetPose.getX() - currentPose.getX();
double translationY = targetPose.getY() - currentPose.getY();
Rotation2d deltaRotation = targetPose.getRotation().minus(currentPose.getRotation());
Translation2d driveTranslation = new Translation2d(translationX, translationY);
drive.driveFieldAngle(driveTranslation, deltaRotation);
double magnitude = Math.sqrt(translationX * translationX + translationY * translationY);
if (magnitude > 1.0) {
translationX /= magnitude;
translationY /= magnitude;
}
Translation2d driveTranslation;
if (magnitude < 0.05) {
driveTranslation = new Translation2d();
} else {
driveTranslation = new Translation2d(translationX, translationY);
}
drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation());
}
@Override
public double getError() {
return 0;
// return targetAngle - drive.getGyroAngle();
}
@Override
public void runWithOutput(double output) {
// drive.driveWithInput(new Pose2d(new Translation2d(0.0, 0.0), new Rotation2d(output / Math.abs(getError()))));
}
// @Override
// public boolean isFinished() {
// Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation();
// double ballAngleDeg = m_lidar.getLatestBallAngleDegrees();
// // TODO: Tune
// return Math.abs(curRot.getDegrees() +ballAngleDeg) < 5;
// }
public void runWithOutput(double output) {}
}
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 = 158;
public static final String GIT_SHA = "07ec609b019fd8db439e14beb74c50fdbf75a091";
public static final String GIT_DATE = "2026-03-14 15:41:27 MDT";
public static final int GIT_REVISION = 159;
public static final String GIT_SHA = "ebab028818fc0362daa4d7eb1010131819edd1cb";
public static final String GIT_DATE = "2026-03-14 18:29:15 MDT";
public static final String GIT_BRANCH = "MiraOrg";
public static final String BUILD_DATE = "2026-03-14 17:27:47 MDT";
public static final long BUILD_UNIX_TIME = 1773530867415L;
public static final String BUILD_DATE = "2026-03-14 18:46:53 MDT";
public static final long BUILD_UNIX_TIME = 1773535613285L;
public static final int DIRTY = 1;
private BuildConstants(){}
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+22 -5
View File
@@ -295,7 +295,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
// SmartDashboard.putBoolean("drift correction", true);
}
public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) {
leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
rotTarget = heading.getDegrees();
var ctrl = new SwerveRequest.FieldCentricFacingAngle()
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(heading);
ctrl.HeadingController.setPID(
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);
}
public void driveIntake(Translation2d leftStick, boolean invertRotation){
// if (invert){
@@ -371,14 +391,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
}
public void defenseXPosition(){
io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_FRONT_STEER, Rotation2d.fromDegrees(45.0));
io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER, Rotation2d.fromDegrees(-45.0));
io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_BACK_STEER, Rotation2d.fromDegrees(-45.0));
io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_BACK_STEER, Rotation2d.fromDegrees(45.0));
io.setControl(new SwerveRequest.SwerveDriveBrake());
}
public void stopDefenseXPosition(){
io.restoreSteerOffsets();
stopModules();
}
public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {
src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java
+4 -8
View File
@@ -77,8 +77,7 @@ public final class SwerveDriveConstants {
public static final class ModuleSpecificConstants { //2026
//Front Left
public static final Angle FRONT_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
public static Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
@@ -86,8 +85,7 @@ public final class SwerveDriveConstants {
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Front Right
public static final Angle FRONT_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.465332+0.3+0.003174-0.0103);
public static Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103);
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103);
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
@@ -95,8 +93,7 @@ public final class SwerveDriveConstants {
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
//Back Left
public static final Angle BACK_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.023438+0.5+0.0168-0.00562);
public static Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562);
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562);
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
@@ -104,8 +101,7 @@ public final class SwerveDriveConstants {
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Back Right
public static final Angle BACK_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.029541+0.05-0.002197-0.00366);
public static Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366);
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366);
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java
-4
View File
@@ -27,10 +27,6 @@ public interface SwerveIO {
public default void tareEverything() {}
public default void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {}
public default void restoreSteerOffsets(){}
public default void resetPose(Pose2d pose) {}
public default void addVisionMeasurement(List<PoseObservation> poses) {}
src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java
-9
View File
@@ -57,15 +57,6 @@ public class SwerveReal implements SwerveIO {
}
}
@Override
public void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {
}
@Override
public void restoreSteerOffsets() {
}
@Override
public void setLimits(double limitInAmps) {
for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {