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/subsystems/swerve/SwerveDrive.java
+23 -6
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
@@ -56,15 +56,6 @@ public class SwerveReal implements SwerveIO {
swerveDriveTrain.addVisionMeasurement(pose, Utils.fpgaToCurrentTime(Vision.getTime()));
}
}
@Override
public void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {
}
@Override
public void restoreSteerOffsets() {
}
@Override
public void setLimits(double limitInAmps) {