From 0c1ba8c2940e9060b243cf851cb4e9a4b314ffaa Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Fri, 28 Feb 2025 17:31:04 -0700 Subject: [PATCH] fix elevator manual driving, and slow fine driving --- .../java/frc4388/robot/RobotContainer.java | 6 +-- .../frc4388/robot/subsystems/Elevator.java | 4 ++ .../frc4388/robot/subsystems/SwerveDrive.java | 41 +++++++++++-------- 3 files changed, 30 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index baccf11..15c9954 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -438,12 +438,12 @@ public class RobotContainer { // While Left Trigger NOT Pressed: Fine Alignment new Trigger(() -> getDeadbandedDriverController().getPOV() != -1 && !(getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)) .whileTrue(new RunCommand( - () -> m_robotSwerveDrive.driveWithInput( + () -> m_robotSwerveDrive.driveFine( new Translation2d( 1, - Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()-90) + Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) ), - getDeadbandedDriverController().getRight(), false + getDeadbandedDriverController().getRight(), 0.15 ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index ceffca0..7b1bb5d 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -256,6 +256,8 @@ public class Elevator extends SubsystemBase { public void manualElevatorVel(double velocity) { if (Math.abs(velocity) > 0.1) { elevatorMotor.set(velocity); + elevatorManualStop = false; + return; } if (!elevatorManualStop) { elevatorManualStop = true; @@ -266,6 +268,8 @@ public class Elevator extends SubsystemBase { public void manualEndeffectorVel(double velocity) { if (Math.abs(velocity) > 0.1) { endeffectorMotor.set(velocity); + endefectorManualStop = false; + return; } if (!endefectorManualStop) { endefectorManualStop = true; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index eff4e3c..bce7c32 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -51,6 +51,7 @@ public class SwerveDrive extends Subsystem { private int gear_index = SwerveDriveConstants.STARTING_GEAR; private boolean stopped = false; + private boolean robotKnowsWhereItIs = false; public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; @@ -59,6 +60,7 @@ public class SwerveDrive extends Subsystem { public Pose2d initalPose2d = null; + public double rotTarget = 0.0; public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); @@ -170,24 +172,7 @@ public class SwerveDrive extends Subsystem { SmartDashboard.putBoolean("drift correction", true); } - // // SmartDashboard.putBoolean("drift correction", true); - - // // rot = ((rotTarget - gyro.getAngle()) / 360) * - // SwerveDriveConstants.ROT_CORRECTION_SPEED; - - // } - - // // Use the left joystick to set speed. Apply a cubic curve and the set max - // speed. - // Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), - // Math.pow(speed.getY(), 3.00)); - - // // Convert field-relative speeds to robot-relative speeds. - // // chassisSpeeds = chassisSpeeds. - // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * - // speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, - // gyro.getRotation2d().times(-1)); + } else { // Create robot-relative speeds. swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * speedAdjust) @@ -196,6 +181,16 @@ public class SwerveDrive extends Subsystem { } } + public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { + stopped = false; + // Create robot-relative speeds. + swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + } + + public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical // reason to have a robot // relitive version of @@ -291,6 +286,8 @@ public class SwerveDrive extends Subsystem { public void resetGyro() { swerveDriveTrain.tareEverything(); + robotKnowsWhereItIs = false; + rotTarget = 0; } @@ -321,6 +318,14 @@ public class SwerveDrive extends Subsystem { vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); if (vision.isTag()) { + Pose2d pose = vision.getPose2d(); + if (!robotKnowsWhereItIs) { + robotKnowsWhereItIs = true; + Pose2d currPose = getPose2d(); + double deltaAngle = pose.getRotation().getDegrees() - currPose.getRotation().getDegrees(); + rotTarget += deltaAngle; + } + swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); }