From e379794da1ca894fa05699f78472287fef35eadd Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 30 Jan 2025 19:28:46 -0700 Subject: [PATCH] lidar align works --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 13 ++++++++++++- .../frc4388/robot/commands/GotoPositionCommand.java | 4 ++-- .../java/frc4388/robot/commands/LidarAlign.java | 8 ++++---- .../java/frc4388/robot/subsystems/SwerveDrive.java | 2 ++ 5 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 27a0469..3ff474b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -105,8 +105,8 @@ public final class Constants { public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 public static final boolean DRIFT_CORRECTION_ENABLED = true; - public static final boolean INVERT_X = true; - public static final boolean INVERT_Y = false; + public static final boolean INVERT_X = false; + public static final boolean INVERT_Y = true; public static final boolean INVERT_ROTATION = false; // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8d9505f..b73b7e5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -37,6 +37,7 @@ import edu.wpi.first.wpilibj2.command.Commands; // Autos import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.GotoPositionCommand; +import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -106,6 +107,15 @@ public class RobotContainer { new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision); + private Command AprilLidarLeft = new SequentialCommandGroup( + AutoGotoPosition.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_lidar, false) + ); + + private Command AprilLidarRight = new SequentialCommandGroup( + AutoGotoPosition.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_lidar, true) + ); private Command placeCoral = new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.stopModules()), @@ -217,7 +227,8 @@ public class RobotContainer { // ? /* Operator Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(AutoGotoPosition); + // .onTrue(AutoGotoPosition); + .onTrue(AprilLidarRight); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index 4b4b115..593e812 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -65,8 +65,8 @@ public class GotoPositionCommand extends Command { double rotoutput = rotPID.update(roterr); Translation2d leftStick = new Translation2d( - Math.max(Math.min(youtput, 1), -1), - Math.max(Math.min(xoutput, 1), -1) + Math.max(Math.min(-youtput, 1), -1), + Math.max(Math.min(-xoutput, 1), -1) // 0,0 ); diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index 5ce0a7e..8b96ad9 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -37,7 +37,7 @@ public class LidarAlign extends Command { @Override public void initialize() { this.currentFinderTick = 0; - this.speed = 0.1; // TODO: find good speed for this + this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; this.headedRight = constructedHeadedRight; } @@ -57,11 +57,11 @@ public class LidarAlign extends Command { currentFinderTick *= -1; } - double relAngle = Math.round(swerveDrive.getGyroAngle() / 60.d) * 60; // Relative driving to the side of the reef + double relAngle = (Math.round(swerveDrive.getGyroAngle() / 60.d) * 60) + 90; // Relative driving to the side of the reef if (!headedRight) { - swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle)); - } else { swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle)); + } else { + swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle)); } currentFinderTick++; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 9e403e7..a16246f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -12,6 +12,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -149,6 +150,7 @@ public class SwerveDrive extends Subsystem { .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); SmartDashboard.putBoolean("drift correction", false); } else { var ctrl = new SwerveRequest.FieldCentricFacingAngle()