From 73f17146e18431a8984b640bee8ab1c10cc86afd Mon Sep 17 00:00:00 2001 From: p-ifka Date: Thu, 20 Mar 2025 08:12:14 -0400 Subject: [PATCH 01/15] change camera drive, remove speed scale factor from drive command add tag_id to camera subsystem, increase elevator height (0.4->0.45) --- .../java/frc/robot/commands/DriveCommand.java | 6 ++--- .../commands/DriveWithHeadingCommand.java | 10 +++---- .../commands/auto/DriveTowardsThing.java | 26 +++++++++++++------ .../frc/robot/commands/auto/ScoreTest.java | 14 +++++----- .../robot/commands/instant/ClawCommand.java | 2 +- .../java/frc/robot/subsystems/Camera.java | 6 +++++ .../java/frc/robot/subsystems/Elevator.java | 2 +- 7 files changed, 42 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index cc981f3..2a2faa3 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -31,9 +31,9 @@ public void initialize() { @Override public void execute() { - double scaleFactor = triggerSupplier.getAsDouble() * 0.5 + 0.75; - drivetrain.drive(ySupplier.getAsDouble() * scaleFactor, xSupplier.getAsDouble() * scaleFactor, omegaSupplier.getAsDouble()); - System.out.println("x: " + ySupplier.getAsDouble() * scaleFactor + ", y: " + xSupplier.getAsDouble() * scaleFactor + ", o:" + omegaSupplier.getAsDouble()); + // double scaleFactor = triggerSupplier.getAsDouble() * 0.5 + 0.75; + drivetrain.drive(ySupplier.getAsDouble(), xSupplier.getAsDouble(), omegaSupplier.getAsDouble()); + // System.out.println("x: " + ySupplier.getAsDouble() * scaleFactor + ", y: " + xSupplier.getAsDouble() * scaleFactor + ", o:" + omegaSupplier.getAsDouble()); } @Override diff --git a/src/main/java/frc/robot/commands/DriveWithHeadingCommand.java b/src/main/java/frc/robot/commands/DriveWithHeadingCommand.java index c93dac0..958aa3a 100644 --- a/src/main/java/frc/robot/commands/DriveWithHeadingCommand.java +++ b/src/main/java/frc/robot/commands/DriveWithHeadingCommand.java @@ -13,6 +13,7 @@ public class DriveWithHeadingCommand extends Command { DoubleSupplier x; DoubleSupplier y; Rotation2d heading; + double angleDisplacement; public DriveWithHeadingCommand(Drivetrain drivetrain, Gyro gyro, DoubleSupplier x, DoubleSupplier y, Rotation2d heading) { this.drivetrain = drivetrain; @@ -34,10 +35,9 @@ public void initialize() {} */ @Override public void execute() { - double angleDifference = (gyro.getRotation2d().minus(heading).getRadians()) * 2.5; // @p-ifka (Jacob) where does this magic number come from? - - SmartDashboard.putNumber("drive heading difference", angleDifference); - drivetrain.drive(0.0, 0.25, -angleDifference); + angleDisplacement = (gyro.getRotation2d().minus(heading).getRadians()) * 2.5; // @p-ifka (Jacob) where does this magic number come from? + SmartDashboard.putNumber("drive heading difference", angleDisplacement); + drivetrain.drive(0.0, 0.25, -angleDisplacement); } @Override @@ -47,7 +47,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return false; + return angleDisplacement < 0.5; } } diff --git a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java index a8371f3..03fa280 100644 --- a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java +++ b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java @@ -36,12 +36,8 @@ public class DriveTowardsThing extends Command { Camera camera; private int straightTimeout = 0; - private static final int LEFT_BOUND = 53; - private static final int SLIGHT_LEFT_BOUND = 156; - private static final int STRAIGHT_BOUND = 164; - private static final int SLIGHT_RIGHT_BOUND = 265; - private static final double Y_APPROACH = 0.25; + private static final double Y_APPROACH = 0.10; private static boolean hasGottenLock = false; private static long[] lastLock = new long[4]; @@ -100,18 +96,21 @@ public DriveTowardsThing(Drivetrain drivetrain, Camera camera) { private boolean checkLock() { /* check if camera is locked, if it is update 'lastlock'*/ if(camera.isLocked()) { + System.out.println("lock found"); hasGottenLock = true; lastLock[0] = camera.getX(); lastLock[1] = camera.getY(); lastLock[2] = camera.getWidth(); lastLock[3] = camera.getHeight(); + System.out.println(camera.getId()); return true; } + System.out.println("no lock found"); return false; } private void updateTarget() { - double[] o = {(Y_APPROACH * Math.toDegrees(Math.atan((lastLock[0] - 160)/5.83))), Y_APPROACH}; + double[] o = {(0.05 * Math.atan((lastLock[0] - 220)/5.83)), 0.05}; targetSpeed = o; } @@ -120,17 +119,28 @@ public void initialize() { if(checkLock()) { updateTarget(); } } + @Override + public boolean isFinished() { + return camera.getWidth() > 125; + } + + @Override + public void end(boolean isInterrupted) { + drivetrain.driveRobotRelative(0.0, 0.0, 0.0); + } + @Override public void execute() { if(!hasGottenLock) { /* camera has not gotten lock yet, strafe left until lock is found*/ /* ideally the robot will be in a position where this is never run*/ if(checkLock()) { updateTarget(); } - drivetrain.driveRobotRelative(0.25, 0.0, 0.0); + // drivetrain.driveRobotRelative(0.25, 0.0, 0.0); } else { /* camera has gotten a lock at some point (lastLock is not nil)*/ - drivetrain.drive(targetSpeed[0], targetSpeed[1], 0.0); + if(checkLock()) { updateTarget();} + drivetrain.driveRobotRelative(targetSpeed[0], targetSpeed[1], 0.0); } diff --git a/src/main/java/frc/robot/commands/auto/ScoreTest.java b/src/main/java/frc/robot/commands/auto/ScoreTest.java index 2c02e28..1d6fcbf 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreTest.java +++ b/src/main/java/frc/robot/commands/auto/ScoreTest.java @@ -57,7 +57,7 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro new WaitCommand(0.1), new DriveDistance(0.5, drivetrain, gyro, 2.73, 0.0, 0.0), // drive to wall new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), - new WaitCommand(0.5), + new WaitCommand(0.7), new InstantCommand(() -> claw.setOutput(-0.4)), new WaitCommand(1.0), new InstantCommand(() -> claw.setOutput(0.0)), @@ -66,9 +66,11 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro } else if(side == 2) { addCommands( new InstantCommand(() -> drivetrain.resetPosition()), - new WaitCommand(0.1), - new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, .23599), - new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.5, 5.23599), + new WaitCommand(0.1), + new DriveDistance(0.25, drivetrain, gyro, 0.0, 0.0, 5.23599), + new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599), + new DriveTowardsThing(drivetrain, camera), + // new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.5, 5.23599), new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), new WaitCommand(0.5), new InstantCommand(() -> claw.setOutput(-0.4)), @@ -89,8 +91,8 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro new WaitCommand(0.1), new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472), new DriveDistance(0.25, drivetrain, gyro, 0.0, 2.5, 1.0472), - new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), - new WaitCommand(0.5), + new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.MED), + new WaitCommand(0.7), new InstantCommand(() -> claw.setOutput(-0.4)), new WaitCommand(1.0), new InstantCommand(() -> claw.setOutput(0.0)) diff --git a/src/main/java/frc/robot/commands/instant/ClawCommand.java b/src/main/java/frc/robot/commands/instant/ClawCommand.java index 3c5641a..4f20cf9 100644 --- a/src/main/java/frc/robot/commands/instant/ClawCommand.java +++ b/src/main/java/frc/robot/commands/instant/ClawCommand.java @@ -25,7 +25,7 @@ public ClawCommand(CoralClaw claw, DoubleSupplier triggerSupplier, int outin) { @Override public void execute() { if(outin == 0) { // OUT - claw.setOutput(-(triggerSupplier.getAsDouble() * 0.5 + 0.2)); + claw.setOutput(-(triggerSupplier.getAsDouble() * 0.4 + 0.2)); } else { // INTAKE claw.setOutput(triggerSupplier.getAsDouble() * 0.4 + 0.2); } diff --git a/src/main/java/frc/robot/subsystems/Camera.java b/src/main/java/frc/robot/subsystems/Camera.java index b49c58f..0b4b361 100644 --- a/src/main/java/frc/robot/subsystems/Camera.java +++ b/src/main/java/frc/robot/subsystems/Camera.java @@ -14,6 +14,7 @@ public class Camera extends SubsystemBase { private long y; private long width; private long height; + private long id; private boolean lock; public Camera() { @@ -32,6 +33,7 @@ public void periodic() { SmartDashboard.putNumber("camera height", height); lock = table.getEntry("lock").getBoolean(true); SmartDashboard.putBoolean("has lock", lock); + id = table.getEntry("tag_id").getInteger(0); } @@ -54,4 +56,8 @@ public long getHeight() { public boolean isLocked() { return lock; } + + public long getId() { + return id; + } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 4c1efdb..6845b68 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -48,7 +48,7 @@ public class Elevator extends SubsystemBase { private static final double FF_DOWN = 0.08; // 2% feedforward when moving down // Speed limitation for safety and control - private static final double MAX_SPEED = 0.4; // Reduced from 0.7 to operate at safer speed on real robot + private static final double MAX_SPEED = 0.45; // Reduced from 0.7 to operate at safer speed on real robot // PID tracking variables private double error = 0; From 7f7ebae345e880b8e75f7ac7a07d57fa279201a6 Mon Sep 17 00:00:00 2001 From: p-ifka Date: Thu, 20 Mar 2025 22:13:47 -0400 Subject: [PATCH 02/15] increase elevator speed, add more autos, add L4 autos --- src/main/java/frc/robot/RobotContainer.java | 22 ++-- .../java/frc/robot/commands/DriveCommand.java | 12 +- .../commands/auto/DriveTowardsThing.java | 38 ++++++- .../frc/robot/commands/auto/ScoreTest.java | 105 +++++++++++------- .../java/frc/robot/subsystems/Elevator.java | 19 +++- 5 files changed, 130 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6dd21a5..c9e1b35 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,15 +14,15 @@ import frc.robot.commands.instant.*; public class RobotContainer { - public final String[] AUTOS = {"none", "pass line", "side 1", "side 2", "side 6"}; - public final String AUTO_DEFAULT = AUTOS[1]; - public static String autoSelected; - public static SendableChooser autoSelector = new SendableChooser<>(); - + public final String[] AUTOS = {"none", "pass line", "side 1", "side 2", "side 3", "side 5", "side 6"}; + public final String AUTO_DEFAULT = AUTOS[1]; + public static String autoSelected; + public static SendableChooser autoSelector = new SendableChooser<>(); + - // Constants - private static final double BEVEL_IN_CORRECTION = 0.25; + // Constants + private static final double BEVEL_IN_CORRECTION = 0.25; /* init subsystems */ @@ -52,7 +52,7 @@ public class RobotContainer { public RobotContainer() { // Initialize OI with a reference to this RobotContainer instance configureBindings(); - drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier, OI.driveControllerRightTriggerSupplier)); + drivetrain.setDefaultCommand(new DriveCommand(drivetrain, elevator, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier, OI.driveControllerRightTriggerSupplier)); RobotMap.compressor.enableAnalog(70, 120); autoSelector.setDefaultOption("default side (2)", AUTO_DEFAULT); @@ -118,7 +118,11 @@ public Command getAutonomousCommand() { return new ScoreTest(1, false, drivetrain, gyro, camera, elevator, wrist, coralClaw); } else if(auto == AUTOS[3]) { // side 2 (right) return new ScoreTest(2, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); - } else if(auto == AUTOS[4]) { // sifre + } else if(auto == AUTOS[4]) { // side 3 (right 2) + return new ScoreTest(3, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); + } else if(auto == AUTOS[5]) { // side 5 + return new ScoreTest(5, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); + } else if(auto == AUTOS[6]) { // side 6 return new ScoreTest(6, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); } else { return null; diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 2a2faa3..5e7dd9c 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -6,19 +6,23 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Elevator; +import frc.robot.ElevatorLevel; public class DriveCommand extends Command { Drivetrain drivetrain; + Elevator elevator; DoubleSupplier xSupplier; DoubleSupplier ySupplier; DoubleSupplier omegaSupplier; DoubleSupplier triggerSupplier; /** drive robot according to the x and y of the drive controller, drive at half speed by default, increase by half the value of the drive trigger (0-1)**/ - public DriveCommand(Drivetrain drivetrain, DoubleSupplier xSupplier, DoubleSupplier ySupplier, + public DriveCommand(Drivetrain drivetrain, Elevator elevator, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier, DoubleSupplier triggerSupplier) { addRequirements(drivetrain); this.drivetrain = drivetrain; + this.elevator = elevator; this.xSupplier = xSupplier; this.ySupplier = ySupplier; this.omegaSupplier = omegaSupplier; @@ -31,8 +35,12 @@ public void initialize() { @Override public void execute() { + if(elevator.getTargetLevel() == ElevatorLevel.EXTRA_HIGH) { + drivetrain.drive(ySupplier.getAsDouble() * 0.5, xSupplier.getAsDouble() * 0.5, omegaSupplier.getAsDouble() * 0.5); + } else { + drivetrain.drive(ySupplier.getAsDouble(), xSupplier.getAsDouble(), omegaSupplier.getAsDouble()); + } // double scaleFactor = triggerSupplier.getAsDouble() * 0.5 + 0.75; - drivetrain.drive(ySupplier.getAsDouble(), xSupplier.getAsDouble(), omegaSupplier.getAsDouble()); // System.out.println("x: " + ySupplier.getAsDouble() * scaleFactor + ", y: " + xSupplier.getAsDouble() * scaleFactor + ", o:" + omegaSupplier.getAsDouble()); } diff --git a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java index 03fa280..0e533a5 100644 --- a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java +++ b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java @@ -3,6 +3,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Camera; +import frc.robot.subsystems.Elevator; +import frc.robot.ElevatorLevel; /** * The DriveTowardsThing command drives the robot towards a target based on the x-coordinate @@ -34,19 +36,32 @@ public class DriveTowardsThing extends Command { Drivetrain drivetrain; Camera camera; - private int straightTimeout = 0; + Elevator elevator; + ElevatorLevel level; + int xDirection; + int yDirection; + boolean reverseXY; + // private int straightTimeout = 0; - private static final double Y_APPROACH = 0.10; + + private static double yApproach = 0.10; private static boolean hasGottenLock = false; private static long[] lastLock = new long[4]; private static double[] targetSpeed = new double[2]; - public DriveTowardsThing(Drivetrain drivetrain, Camera camera) { + public DriveTowardsThing(int xDirection, int yDirection, boolean reverseXY, Drivetrain drivetrain, Camera camera, Elevator elevator, ElevatorLevel level) { addRequirements(drivetrain, camera); this.drivetrain = drivetrain; this.camera = camera; + this.elevator = elevator; + this.level = level; + + this.xDirection = xDirection; + this.yDirection = yDirection; + this.reverseXY = reverseXY; + } /** @@ -110,7 +125,10 @@ private boolean checkLock() { } private void updateTarget() { - double[] o = {(0.05 * Math.atan((lastLock[0] - 220)/5.83)), 0.05}; + yApproach = Math.min(3.0/lastLock[2], 0.25); + // yApproach = 0.1; + double[] o = {(yApproach * Math.atan((lastLock[0] - 220)/5.83)), yApproach}; + System.out.println("target set: "+ o[0] + ", " + o[1]); targetSpeed = o; } @@ -137,10 +155,18 @@ public void execute() { /* ideally the robot will be in a position where this is never run*/ if(checkLock()) { updateTarget(); } // drivetrain.driveRobotRelative(0.25, 0.0, 0.0); - } else { + } else { /* camera has gotten a lock at some point (lastLock is not nil)*/ if(checkLock()) { updateTarget();} - drivetrain.driveRobotRelative(targetSpeed[0], targetSpeed[1], 0.0); + if (reverseXY) { + drivetrain.driveRobotRelative(targetSpeed[1]*xDirection, targetSpeed[0]*yDirection, 0.0); + } else { + drivetrain.driveRobotRelative(targetSpeed[0]*xDirection, targetSpeed[1]*yDirection, 0.0); + } + + if(lastLock[2] > 60) { + elevator.setTarget(level); + } } diff --git a/src/main/java/frc/robot/commands/auto/ScoreTest.java b/src/main/java/frc/robot/commands/auto/ScoreTest.java index 1d6fcbf..33df819 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreTest.java +++ b/src/main/java/frc/robot/commands/auto/ScoreTest.java @@ -45,67 +45,86 @@ public class ScoreTest extends SequentialCommandGroup { * @param gyro The gyro sensor used for orientation and heading. * @param camera The camera used for vision processing (currently commented out). */ + private void scoreL4(Elevator elevator, Wrist wrist, CoralClaw claw) { + addCommands( + new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.EXTRA_HIGH), + new WaitCommand(0.8), + new InstantCommand(() -> claw.setOutput(-0.4)), + new WaitCommand(0.1), + new InstantCommand(() -> wrist.retract()), + new WaitCommand(1.0), + new InstantCommand(() -> claw.setOutput(0.0)), + new WaitCommand(0.2), + new InstantCommand(() -> elevator.setTarget(ElevatorLevel.STATION)) +); + } + + private void driveOffReef(int direction, Drivetrain drivetrain, Gyro gyro) { + addCommands( + new WaitCommand(1.0), + new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.5*direction, 3.14), + new InstantCommand(() -> drivetrain.resetPosition()) + + ); + + + } + public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, Wrist wrist, CoralClaw claw) { - if(side == 0) { // srive forward + if(side == 0) { // srive forward addCommands( new InstantCommand(() -> drivetrain.resetPosition()), - new DriveDistance(0.25, drivetrain, gyro, 2.0, 0.0, 0.0) + new DriveDistance(0.25, drivetrain, gyro, 2.2, -0.5, 0.0) ); } else if(side == 1) { - addCommands( + addCommands( new InstantCommand(() -> drivetrain.resetPosition()), new WaitCommand(0.1), - new DriveDistance(0.5, drivetrain, gyro, 2.73, 0.0, 0.0), // drive to wall - new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), - new WaitCommand(0.7), - new InstantCommand(() -> claw.setOutput(-0.4)), - new WaitCommand(1.0), - new InstantCommand(() -> claw.setOutput(0.0)), - new WaitCommand(1.5) - ); + new DriveDistance(0.25, drivetrain, gyro, 1.5, 0.0, 0.0), + new DriveTowardsThing(1, -1, true, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) + ); + scoreL4(elevator, wrist, claw); } else if(side == 2) { addCommands( new InstantCommand(() -> drivetrain.resetPosition()), new WaitCommand(0.1), new DriveDistance(0.25, drivetrain, gyro, 0.0, 0.0, 5.23599), new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599), - new DriveTowardsThing(drivetrain, camera), - // new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.5, 5.23599), - new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), - new WaitCommand(0.5), - new InstantCommand(() -> claw.setOutput(-0.4)), - new WaitCommand(1.0), - new InstantCommand(() -> claw.setOutput(0.0)) - ); - if(goToStation) { - addCommands( - new WaitCommand(1.0), - new DriveDistance(0.25, drivetrain, gyro, 0.0, 2.5, 3.14), - // new DriveDistance(0.25, drivetrain, gyro, 1.5, 0.0, 3.14), - new InstantCommand(() -> drivetrain.resetPosition()) + new DriveTowardsThing(1, 1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) + ); + scoreL4(elevator, wrist, claw); + if(goToStation) { driveOffReef(1, drivetrain, gyro); } + } else if(side == 3) { + addCommands( + new InstantCommand(() -> drivetrain.resetPosition()), + new WaitCommand(0.1), + new DriveDistance(0.25, drivetrain, gyro, 5.0, 0.0, 4.18879), + new DriveDistance(0.25, drivetrain, gyro, 0.0, -1.0, 4.18879), + new DriveTowardsThing(1, 1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) ); - } - } else if(side == 6) { - addCommands( + scoreL4(elevator, wrist, claw); + if(goToStation) { driveOffReef(1, drivetrain, gyro); } + } else if(side == 5) { + addCommands( new InstantCommand(() -> drivetrain.resetPosition()), new WaitCommand(0.1), - new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472), - new DriveDistance(0.25, drivetrain, gyro, 0.0, 2.5, 1.0472), - new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.MED), - new WaitCommand(0.7), - new InstantCommand(() -> claw.setOutput(-0.4)), - new WaitCommand(1.0), - new InstantCommand(() -> claw.setOutput(0.0)) - ); - if(goToStation) { - addCommands( - new WaitCommand(1.0), - new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.5, 3.14), - new InstantCommand(() -> drivetrain.resetPosition()) + new DriveDistance(0.25, drivetrain, gyro, 5.0, 0.0, 2.09439), + new DriveDistance(0.25, drivetrain, gyro, 0.0, 1.0, 2.09439), + new DriveTowardsThing(-1, -1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) + ); + scoreL4(elevator, wrist, claw); + if(goToStation) { driveOffReef(-1, drivetrain, gyro); } + } + else if(side == 6) { + addCommands( + new InstantCommand(() -> drivetrain.resetPosition()), + new WaitCommand(0.1), + new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472), + new DriveTowardsThing(-1, -1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) ); - } - + scoreL4(elevator, wrist, claw); + if(goToStation) { driveOffReef(-1, drivetrain, gyro); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 6845b68..7479ab5 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -34,8 +34,10 @@ public class Elevator extends SubsystemBase { private static final double POSITION_TOLERANCE = 0.01; // 5cm position tolerance private static final double EXIT_TOLERANCE = 0.08; // 8cm for exiting "at target" state (hysteresis) private static final int TARGET_STABLE_COUNT = 5; // Number of consecutive "at target" checks before declaring stable + private static ElevatorLevel targetLevel = ElevatorLevel.LOW; private int atTargetCounter = 0; // Counter for consecutive "at target" checks private boolean atTargetStatus = false; // Filtered status for UI + // PID Control constants - adjusted for better stability (will be adjusted again when we run on the robot for real) private static final double P_GAIN = 2.5; // Reduced from 1.5 to reduce oscillation @@ -48,7 +50,7 @@ public class Elevator extends SubsystemBase { private static final double FF_DOWN = 0.08; // 2% feedforward when moving down // Speed limitation for safety and control - private static final double MAX_SPEED = 0.45; // Reduced from 0.7 to operate at safer speed on real robot + private static final double MAX_SPEED = 1.0; // Reduced from 700 to operate at safer speed on real robot // PID tracking variables private double error = 0; @@ -137,6 +139,10 @@ public double getHeight() { } } + public ElevatorLevel getTargetLevel() { + return targetLevel; + } + /** * Resets the PID controller values */ @@ -149,23 +155,24 @@ public void resetPID() { public void setTarget(ElevatorLevel level) { System.out.println("elevator target set"); double groundHeight = 0.0; + targetLevel = level; // Tune these values to match the actual robot requirmenets to seed the coral switch (level) { case STATION: - groundHeight = Units.inchesToMeters(27.5); // all these measurements are from measuring the field components + groundHeight = Units.inchesToMeters(26.5); // all these measurements are from measuring the field components break; case LOW: groundHeight = Units.inchesToMeters(18.0); break; case MED: - groundHeight = Units.inchesToMeters(28.5); + groundHeight = Units.inchesToMeters(29); break; case HIGH: - groundHeight = Units.inchesToMeters(42.25); + groundHeight = Units.inchesToMeters(43.0); break; case EXTRA_HIGH: - groundHeight = Units.inchesToMeters(63.75); + groundHeight = Units.inchesToMeters(65.75); break; default: groundHeight = 0.0; @@ -281,7 +288,7 @@ public void periodic() { } // Calculate the combined output with feedforward - double output = pTerm + ffTerm + iTerm; // + iTerm + dTerm ; + double output = pTerm + ffTerm + iTerm + dTerm; // + iTerm + dTerm ; SmartDashboard.putNumber("elevator output term", output); // Limit the output for smoother operation From b527645c5fa25056c8e85054fcb271669d47b81d Mon Sep 17 00:00:00 2001 From: LER-Programming Date: Tue, 25 Mar 2025 19:17:30 -0400 Subject: [PATCH 03/15] north bay --- src/main/java/frc/robot/OI.java | 2 +- src/main/java/frc/robot/Robot.java | 66 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 4 +- .../commands/auto/DriveTowardsThing.java | 68 ++++++++++++++++--- .../frc/robot/commands/auto/ScoreTest.java | 6 ++ .../java/frc/robot/subsystems/CoralClaw.java | 4 ++ .../java/frc/robot/subsystems/Elevator.java | 40 +++++------ 7 files changed, 124 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 04a91cd..a38561a 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -63,7 +63,7 @@ private static double processRotationInput(double raw) { if (Math.abs(raw) < 0.1) { raw = 0; } - return raw*4; + return raw*3; } /** diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 422cbea..f61704f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,23 +45,23 @@ public String toString() { } // Create SendableChooser for test options - private final SendableChooser testChooser = new SendableChooser<>(); + // private final SendableChooser testChooser = new SendableChooser<>(); public Robot() { m_robotContainer = new RobotContainer(); // Initialize test chooser - testChooser.setDefaultOption("No Test", TestOption.NONE); - testChooser.addOption("Elevator to Low", TestOption.ELEVATOR_LOW); - testChooser.addOption("Elevator to Mid", TestOption.ELEVATOR_MID); - testChooser.addOption("Elevator to High", TestOption.ELEVATOR_HIGH); - testChooser.addOption("Elevator to Station", TestOption.ELEVATOR_STATION); - testChooser.addOption("Run Coral Claw Intake", TestOption.CORAL_CLAW_INTAKE); - testChooser.addOption("Fetch Coral From Station", TestOption.FETCH_CORAL_STATION); + // testChooser.setDefaultOption("No Test", TestOption.NONE); + // testChooser.addOption("Elevator to Low", TestOption.ELEVATOR_LOW); + // testChooser.addOption("Elevator to Mid", TestOption.ELEVATOR_MID); + // testChooser.addOption("Elevator to High", TestOption.ELEVATOR_HIGH); + // testChooser.addOption("Elevator to Station", TestOption.ELEVATOR_STATION); + // testChooser.addOption("Run Coral Claw Intake", TestOption.CORAL_CLAW_INTAKE); + // testChooser.addOption("Fetch Coral From Station", TestOption.FETCH_CORAL_STATION); // Add more options as needed // Add the chooser to the dashboard - SmartDashboard.putData("Test Selector", testChooser); + // SmartDashboard.putData("Test Selector", testChooser); } @Override @@ -114,33 +114,33 @@ public void teleopExit() {} public void testInit() { CommandScheduler.getInstance().cancelAll(); // Get the selected test option - TestOption selectedTest = testChooser.getSelected(); + // TestOption selectedTest = testChooser.getSelected(); // Run the selected test command - switch(selectedTest) { - case ELEVATOR_LOW: - new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.LOW).schedule(); - break; - case ELEVATOR_MID: - new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.MED).schedule(); - break; - case ELEVATOR_HIGH: - new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.HIGH).schedule(); - break; - case ELEVATOR_STATION: - new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.STATION).schedule(); - break; - case CORAL_CLAW_INTAKE: - new InstantCommand(() -> m_robotContainer.coralClaw.intake()).schedule(); - break; - case FETCH_CORAL_STATION: - new FetchCoralFromStationCommand(m_robotContainer.elevator, m_robotContainer.coralClaw).schedule(); - break; - case NONE: - default: + // switch(selectedTest) { + // case ELEVATOR_LOW: + // new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.LOW).schedule(); + // break; + // case ELEVATOR_MID: + // new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.MED).schedule(); + // break; + // case ELEVATOR_HIGH: + // new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.HIGH).schedule(); + // break; + // case ELEVATOR_STATION: + // new MoveElevatorToLevelCommand(m_robotContainer.elevator, ElevatorLevel.STATION).schedule(); + // break; + // case CORAL_CLAW_INTAKE: + // new InstantCommand(() -> m_robotContainer.coralClaw.intake()).schedule(); + // break; + // case FETCH_CORAL_STATION: + // new FetchCoralFromStationCommand(m_robotContainer.elevator, m_robotContainer.coralClaw).schedule(); + // break; + // case NONE: + // default: // Do nothing - break; - } + // break; + // } } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c9e1b35..e1c3f3f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -119,9 +119,9 @@ public Command getAutonomousCommand() { } else if(auto == AUTOS[3]) { // side 2 (right) return new ScoreTest(2, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); } else if(auto == AUTOS[4]) { // side 3 (right 2) - return new ScoreTest(3, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); + return new ScoreTest(3, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); } else if(auto == AUTOS[5]) { // side 5 - return new ScoreTest(5, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); + return new ScoreTest(5, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); } else if(auto == AUTOS[6]) { // side 6 return new ScoreTest(6, true, drivetrain, gyro, camera, elevator, wrist, coralClaw); } else { diff --git a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java index 0e533a5..fdb61df 100644 --- a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java +++ b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java @@ -1,5 +1,7 @@ package frc.robot.commands.auto; +import javax.net.ssl.TrustManagerFactorySpi; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Camera; @@ -41,11 +43,18 @@ public class DriveTowardsThing extends Command { int xDirection; int yDirection; boolean reverseXY; + double endTime; + + // private int straightTimeout = 0; - private static double yApproach = 0.10; + // private static double yApproach = 0.10; + private static double ySpeed = 0.1; + private static double xSpeed = ySpeed; + private static int targetCameraX = 160; + private static int noLockTime = 0; private static boolean hasGottenLock = false; private static long[] lastLock = new long[4]; @@ -121,25 +130,63 @@ private boolean checkLock() { return true; } System.out.println("no lock found"); + noLockTime++; return false; } - private void updateTarget() { - yApproach = Math.min(3.0/lastLock[2], 0.25); - // yApproach = 0.1; - double[] o = {(yApproach * Math.atan((lastLock[0] - 220)/5.83)), yApproach}; - System.out.println("target set: "+ o[0] + ", " + o[1]); - targetSpeed = o; + private void updateTarget() { + // xSpeed = Math.min(3.0/lastLock[2], 0.25); + // ySpeed = xSpeed; + // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + // targetSpeed[1] = ySpeed; + + + + if(lastLock[2] > 60) { + /** if robot is close enough to the tag, + * start going to the right**/ + targetCameraX = 220; + if(lastLock[0] > 115 && lastLock[0] < 270) { + /** robot is ~ aligned to the right + * start driving forward **/ + xSpeed = Math.min(3.0/lastLock[2], 0.25); + ySpeed = 0.1; + targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + targetSpeed[1] = ySpeed; + // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; + } else { + /** robot is close but not aligned to right, + * strafe right do not move forward**/ + xSpeed = Math.min(3.0/lastLock[2], 0.25); + ySpeed = 0.0; + targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + targetSpeed[1] = ySpeed; + // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; + } + } else { + /* robot far away from tag, + * drive towards middle */ + targetCameraX = 160; + xSpeed = Math.min(3.0/lastLock[2], 0.25); + ySpeed = 0.15; + targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + targetSpeed[1] = ySpeed; + // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; + + } } + + @Override public void initialize() { - if(checkLock()) { updateTarget(); } + if(checkLock()) { updateTarget(); } + endTime = System.currentTimeMillis() + 8000; } @Override public boolean isFinished() { - return camera.getWidth() > 125; + return (camera.getWidth() > 125 || System.currentTimeMillis() >= endTime || noLockTime >= 30); } @Override @@ -164,7 +211,8 @@ public void execute() { drivetrain.driveRobotRelative(targetSpeed[0]*xDirection, targetSpeed[1]*yDirection, 0.0); } - if(lastLock[2] > 60) { + if(lastLock[2] > 80) { + targetCameraX = 220; elevator.setTarget(level); } } diff --git a/src/main/java/frc/robot/commands/auto/ScoreTest.java b/src/main/java/frc/robot/commands/auto/ScoreTest.java index 33df819..1d9786d 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreTest.java +++ b/src/main/java/frc/robot/commands/auto/ScoreTest.java @@ -84,6 +84,12 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro new DriveTowardsThing(1, -1, true, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) ); scoreL4(elevator, wrist, claw); + addCommands( + new WaitCommand(0.5), + new DriveDistance(0.25, drivetrain, gyro, 0.0, 1.2, 0.0), + new InstantCommand(() -> wrist.extend()), + new InstantCommand(() -> elevator.setTarget(ElevatorLevel.HIGH)) + ); } else if(side == 2) { addCommands( new InstantCommand(() -> drivetrain.resetPosition()), diff --git a/src/main/java/frc/robot/subsystems/CoralClaw.java b/src/main/java/frc/robot/subsystems/CoralClaw.java index ba406ad..8b34765 100644 --- a/src/main/java/frc/robot/subsystems/CoralClaw.java +++ b/src/main/java/frc/robot/subsystems/CoralClaw.java @@ -140,6 +140,10 @@ public void outtake() { SmartDashboard.putString("Coral Claw State", "Outtaking"); } + public void t() { + primaryMotor.set(0.7); + } + public void setMotorsOn(boolean left, boolean right) { motorsOn[0] = left; motorsOn[1] = right; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 7479ab5..1050826 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -41,7 +41,7 @@ public class Elevator extends SubsystemBase { // PID Control constants - adjusted for better stability (will be adjusted again when we run on the robot for real) private static final double P_GAIN = 2.5; // Reduced from 1.5 to reduce oscillation - private static final double I_GAIN = 0.1; // Increased from 0.1 to help with steady-state error + private static final double I_GAIN = 0.15; // Increased from 0.1 to help with steady-state error private static final double D_GAIN = 0.3; // Increased from 0.2 for better damping private static final double MAX_INTEGRAL = 1.0; // Anti-windup limit @@ -50,7 +50,7 @@ public class Elevator extends SubsystemBase { private static final double FF_DOWN = 0.08; // 2% feedforward when moving down // Speed limitation for safety and control - private static final double MAX_SPEED = 1.0; // Reduced from 700 to operate at safer speed on real robot + private static final double MAX_SPEED = 0.8; // Reduced from 700 to operate at safer speed on real robot // PID tracking variables private double error = 0; @@ -160,13 +160,13 @@ public void setTarget(ElevatorLevel level) { // Tune these values to match the actual robot requirmenets to seed the coral switch (level) { case STATION: - groundHeight = Units.inchesToMeters(26.5); // all these measurements are from measuring the field components + groundHeight = Units.inchesToMeters(26.75); // all these measurements are from measuring the field components break; case LOW: groundHeight = Units.inchesToMeters(18.0); break; case MED: - groundHeight = Units.inchesToMeters(29); + groundHeight = Units.inchesToMeters(29.0); break; case HIGH: groundHeight = Units.inchesToMeters(43.0); @@ -257,7 +257,7 @@ public void periodic() { // } else { // Reset consecutive counter if we're not at target atTargetCounter = 0; - SmartDashboard.putString("Elevator Status", "Moving: error=" + String.format("%.3f", error) + "m"); + // SmartDashboard.putString("Elevator Status", "Moving: error=" + String.format("%.3f", error) + "m"); // Calculate PID terms @@ -288,13 +288,13 @@ public void periodic() { } // Calculate the combined output with feedforward - double output = pTerm + ffTerm + iTerm + dTerm; // + iTerm + dTerm ; + double output = pTerm + ffTerm + iTerm; // + iTerm + dTerm ; - SmartDashboard.putNumber("elevator output term", output); + // SmartDashboard.putNumber("elevator output term", output); // Limit the output for smoother operation double speed = Math.max(-MAX_SPEED, Math.min(MAX_SPEED, output)); - SmartDashboard.putNumber("elevator output speed", speed); + // SmartDashboard.putNumber("elevator output speed", speed); // Set the speed to move the elevator setSpeed(speed); @@ -302,7 +302,7 @@ public void periodic() { lastError = error; // Show feedforward contribution in SmartDashboard - SmartDashboard.putNumber("Elevator FF Term", ffTerm); + // SmartDashboard.putNumber("Elevator FF Term", ffTerm); // } lastTimestamp = currentTime; @@ -317,19 +317,19 @@ public void periodic() { // } // Update SmartDashboard with current height, target info, and safety limits - SmartDashboard.putNumber("Elevator Height (m)", getHeight()); + // SmartDashboard.putNumber("Elevator Height (m)", getHeight()); SmartDashboard.putNumber("Elevator Height (in)", Units.metersToInches(getHeight())); - SmartDashboard.putNumber("Elevator Target Height (m)", targetHeight); - SmartDashboard.putNumber("Height from Ground (m)", getHeight() + BASE_HEIGHT_OFFSET_METERS); + // SmartDashboard.putNumber("Elevator Target Height (m)", targetHeight); + // SmartDashboard.putNumber("Height from Ground (m)", getHeight() + BASE_HEIGHT_OFFSET_METERS); SmartDashboard.putNumber("Height from Ground (in)", Units.metersToInches(getHeight() + BASE_HEIGHT_OFFSET_METERS)); - SmartDashboard.putBoolean("Elevator At Target", isAtTarget()); - SmartDashboard.putNumber("Elevator Max Height (m)", MAX_HEIGHT_METERS); - SmartDashboard.putNumber("Elevator Min Height (m)", MIN_HEIGHT_METERS); - SmartDashboard.putNumber("Elevator Error", targetHeight - getHeight()); - SmartDashboard.putNumber("Elevator Error Sum", errorSum); - SmartDashboard.putNumber("At Target Counter", atTargetCounter); - SmartDashboard.putNumber("Position Tolerance", POSITION_TOLERANCE); - SmartDashboard.putBoolean("Within Tolerance", Math.abs(getHeight() - targetHeight) < POSITION_TOLERANCE); + // SmartDashboard.putBoolean("Elevator At Target", isAtTarget()); + // SmartDashboard.putNumber("Elevator Max Height (m)", MAX_HEIGHT_METERS); + // SmartDashboard.putNumber("Elevator Min Height (m)", MIN_HEIGHT_METERS); + // SmartDashboard.putNumber("Elevator Error", targetHeight - getHeight()); + // SmartDashboard.putNumber("Elevator Error Sum", errorSum); + // SmartDashboard.putNumber("At Target Counter", atTargetCounter); + // SmartDashboard.putNumber("Position Tolerance", POSITION_TOLERANCE); + // SmartDashboard.putBoolean("Within Tolerance", Math.abs(getHeight() - targetHeight) < POSITION_TOLERANCE); // Hysteresis-based at-target detection for UI boolean rawAtTarget = isAtTarget(); // Get raw status From 9563cfebc9fb8fb779f99a892800ae4e2cc5e076 Mon Sep 17 00:00:00 2001 From: p-ifka Date: Sat, 29 Mar 2025 00:51:16 -0400 Subject: [PATCH 04/15] make robot relative actually robot relative, change camera drive --- src/main/java/frc/robot/RobotContainer.java | 1 + .../commands/auto/DriveTowardsThing.java | 102 +++++++++++------- .../frc/robot/commands/auto/ScoreTest.java | 5 +- .../robot/commands/instant/ClawCommand.java | 2 +- .../java/frc/robot/subsystems/Camera.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 5 +- .../java/frc/robot/subsystems/Elevator.java | 19 ++-- 7 files changed, 81 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e1c3f3f..f7a17cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -77,6 +77,7 @@ private void configureBindings() { drivetrain.resetPosition(); })); + OI.driveControllerB.whileTrue(new DriveTowardsThing(1, 1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH)); // Operator controller bindings // Elevator level controls // OPERATOR B : INTAKE diff --git a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java index fdb61df..48b6210 100644 --- a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java +++ b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java @@ -7,6 +7,7 @@ import frc.robot.subsystems.Camera; import frc.robot.subsystems.Elevator; import frc.robot.ElevatorLevel; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * The DriveTowardsThing command drives the robot towards a target based on the x-coordinate @@ -123,7 +124,9 @@ private boolean checkLock() { System.out.println("lock found"); hasGottenLock = true; lastLock[0] = camera.getX(); + SmartDashboard.putNumber("lastlock x", lastLock[0]); lastLock[1] = camera.getY(); + SmartDashboard.putNumber("lastlock y", lastLock[1]); lastLock[2] = camera.getWidth(); lastLock[3] = camera.getHeight(); System.out.println(camera.getId()); @@ -139,41 +142,61 @@ private void updateTarget() { // ySpeed = xSpeed; // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); // targetSpeed[1] = ySpeed; + // double speed = Math.min(3.0/lastLock[2], 0.25); + System.out.println("Updating Target"); + double speed = 0.15; + if(Math.abs(lastLock[0] - 220) < 10) { + targetSpeed[0] = 0.0; + } else { + targetSpeed[0] = -((lastLock[0] - 220)/300.0); + } + + targetSpeed[1] = 0.0; - - - if(lastLock[2] > 60) { - /** if robot is close enough to the tag, - * start going to the right**/ - targetCameraX = 220; - if(lastLock[0] > 115 && lastLock[0] < 270) { - /** robot is ~ aligned to the right - * start driving forward **/ - xSpeed = Math.min(3.0/lastLock[2], 0.25); - ySpeed = 0.1; - targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); - targetSpeed[1] = ySpeed; - // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; - } else { - /** robot is close but not aligned to right, - * strafe right do not move forward**/ - xSpeed = Math.min(3.0/lastLock[2], 0.25); - ySpeed = 0.0; - targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); - targetSpeed[1] = ySpeed; - // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; - } - } else { - /* robot far away from tag, - * drive towards middle */ - targetCameraX = 160; - xSpeed = Math.min(3.0/lastLock[2], 0.25); - ySpeed = 0.15; - targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); - targetSpeed[1] = ySpeed; - // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; + SmartDashboard.putNumber("Target X", targetSpeed[0]); + SmartDashboard.putNumber("Target Y", targetSpeed[1]); + + // xSpeed = Math.min(3.0/lastLock[2], 0.25); + // ySpeed = 0.1; + // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - 220)/5.83); + // targetSpeed[1] = ySpeed; + + + // if(lastLock[2] > 60) { + // /** if robot is close enough to the tag, + // * start going to the right**/ + // targetCameraX = 220; + // if(lastLock[0] > 115 && lastLock[0] < 270) { + // /** robot is ~ aligned to the right + // * start driving forward **/ + // System.out.println("target 220 forward"); + // xSpeed = Math.min(3.0/lastLock[2], 0.25); + // ySpeed = 0.1; + // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + // targetSpeed[1] = ySpeed; + // // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; + // } else { + // /** robot is close but not aligned to right, + // * strafe right do not move forward**/ + // System.out.println("target 220 strafe"); + // xSpeed = Math.min(3.0/lastLock[2], 0.25); + // ySpeed = 0.0; + // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + // targetSpeed[1] = ySpeed; + // // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; + // } + // } else { + // /* robot far away from tag, + // * drive towards middle */ + // System.out.println("target 220"); + // targetCameraX = 220; + // xSpeed = Math.min(3.0/lastLock[2], 0.25); + // ySpeed = 0.15; + // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + // targetSpeed[1] = ySpeed; + // // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; - } + // } } @@ -186,7 +209,7 @@ public void initialize() { @Override public boolean isFinished() { - return (camera.getWidth() > 125 || System.currentTimeMillis() >= endTime || noLockTime >= 30); + return (camera.getWidth() > 225 || System.currentTimeMillis() >= endTime); } @Override @@ -204,13 +227,14 @@ public void execute() { // drivetrain.driveRobotRelative(0.25, 0.0, 0.0); } else { /* camera has gotten a lock at some point (lastLock is not nil)*/ - if(checkLock()) { updateTarget();} - if (reverseXY) { - drivetrain.driveRobotRelative(targetSpeed[1]*xDirection, targetSpeed[0]*yDirection, 0.0); - } else { - drivetrain.driveRobotRelative(targetSpeed[0]*xDirection, targetSpeed[1]*yDirection, 0.0); + if(checkLock()) { + updateTarget(); + drivetrain.driveRobotRelative(targetSpeed[1]*xDirection, targetSpeed[0]*yDirection, 0.0); + } else{ + drivetrain.driveRobotRelative(0.0, 0.0, 0.0); } + if(lastLock[2] > 80) { targetCameraX = 220; elevator.setTarget(level); diff --git a/src/main/java/frc/robot/commands/auto/ScoreTest.java b/src/main/java/frc/robot/commands/auto/ScoreTest.java index 1d9786d..67e6920 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreTest.java +++ b/src/main/java/frc/robot/commands/auto/ScoreTest.java @@ -48,10 +48,8 @@ public class ScoreTest extends SequentialCommandGroup { private void scoreL4(Elevator elevator, Wrist wrist, CoralClaw claw) { addCommands( new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.EXTRA_HIGH), - new WaitCommand(0.8), + new WaitCommand(1.0), new InstantCommand(() -> claw.setOutput(-0.4)), - new WaitCommand(0.1), - new InstantCommand(() -> wrist.retract()), new WaitCommand(1.0), new InstantCommand(() -> claw.setOutput(0.0)), new WaitCommand(0.2), @@ -96,6 +94,7 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro new WaitCommand(0.1), new DriveDistance(0.25, drivetrain, gyro, 0.0, 0.0, 5.23599), new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599), + new DriveDistance(0.25, drivetrain, gyro, 0.0, -1.5, 5.23599), new DriveTowardsThing(1, 1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) ); scoreL4(elevator, wrist, claw); diff --git a/src/main/java/frc/robot/commands/instant/ClawCommand.java b/src/main/java/frc/robot/commands/instant/ClawCommand.java index 4f20cf9..ec3b0f6 100644 --- a/src/main/java/frc/robot/commands/instant/ClawCommand.java +++ b/src/main/java/frc/robot/commands/instant/ClawCommand.java @@ -27,7 +27,7 @@ public void execute() { if(outin == 0) { // OUT claw.setOutput(-(triggerSupplier.getAsDouble() * 0.4 + 0.2)); } else { // INTAKE - claw.setOutput(triggerSupplier.getAsDouble() * 0.4 + 0.2); + claw.setOutput(triggerSupplier.getAsDouble() * 0.2 + 0.2); } diff --git a/src/main/java/frc/robot/subsystems/Camera.java b/src/main/java/frc/robot/subsystems/Camera.java index 0b4b361..872cb44 100644 --- a/src/main/java/frc/robot/subsystems/Camera.java +++ b/src/main/java/frc/robot/subsystems/Camera.java @@ -31,7 +31,7 @@ public void periodic() { SmartDashboard.putNumber("camera width", width); height = table.getEntry("height").getInteger(0); SmartDashboard.putNumber("camera height", height); - lock = table.getEntry("lock").getBoolean(true); + lock = table.getEntry("has_lock").getBoolean(true); SmartDashboard.putBoolean("has lock", lock); id = table.getEntry("tag_id").getInteger(0); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index f798bae..cc9876b 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -102,11 +102,10 @@ public void drive(double x, double y, double omega) { } public void driveRobotRelative(double x, double y, double omega) { - ChassisSpeeds chassisSpeed = ChassisSpeeds.fromRobotRelativeSpeeds( + ChassisSpeeds chassisSpeed = new ChassisSpeeds( FREE_SPEED * x, FREE_SPEED * y, - omega, - gyro.getRotation2d()); + omega); SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeed); leftFrontSwerve.setState(states[0]); rightFrontSwerve.setState(states[1]); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 1050826..9dfb180 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -37,11 +37,11 @@ public class Elevator extends SubsystemBase { private static ElevatorLevel targetLevel = ElevatorLevel.LOW; private int atTargetCounter = 0; // Counter for consecutive "at target" checks private boolean atTargetStatus = false; // Filtered status for UI - + // PID Control constants - adjusted for better stability (will be adjusted again when we run on the robot for real) private static final double P_GAIN = 2.5; // Reduced from 1.5 to reduce oscillation - private static final double I_GAIN = 0.15; // Increased from 0.1 to help with steady-state error + private static final double I_GAIN = 0.5; // Increased from 0.1 to help with steady-state error private static final double D_GAIN = 0.3; // Increased from 0.2 for better damping private static final double MAX_INTEGRAL = 1.0; // Anti-windup limit @@ -71,7 +71,7 @@ public class Elevator extends SubsystemBase { + 11 // Inner stage is around 11 lbs ); // Updated max height to match EXTRA_HIGH level - private static final double MAX_HEIGHT_METERS = Units.inchesToMeters(72.25); // Max height updated to match EXTRA_HIGH level + private static final double MAX_HEIGHT_METERS = Units.inchesToMeters(75.25); // Max height updated to match EXTRA_HIGH level private static final double MIN_HEIGHT_METERS = 0.0; private static final double GEAR_RATIO = 8.41; // 8.41:1 gear reduction for now, can change if current graphs are high @@ -160,19 +160,19 @@ public void setTarget(ElevatorLevel level) { // Tune these values to match the actual robot requirmenets to seed the coral switch (level) { case STATION: - groundHeight = Units.inchesToMeters(26.75); // all these measurements are from measuring the field components + groundHeight = Units.inchesToMeters(28.0); // all these measurements are from measuring the field components break; case LOW: - groundHeight = Units.inchesToMeters(18.0); + groundHeight = Units.inchesToMeters(18.5); break; case MED: - groundHeight = Units.inchesToMeters(29.0); + groundHeight = Units.inchesToMeters(29.5); break; case HIGH: - groundHeight = Units.inchesToMeters(43.0); + groundHeight = Units.inchesToMeters(44.25); break; case EXTRA_HIGH: - groundHeight = Units.inchesToMeters(65.75); + groundHeight = Units.inchesToMeters(68.0); break; default: groundHeight = 0.0; @@ -296,6 +296,9 @@ public void periodic() { // SmartDashboard.putNumber("elevator output speed", speed); // Set the speed to move the elevator + if(error < 0) { + speed = speed * 0.8; + } setSpeed(speed); // Store current values for next iteration From c06cbe5e8e094ede3cdc0832483bb1fb26686fae Mon Sep 17 00:00:00 2001 From: AdamG825 Date: Sat, 29 Mar 2025 10:02:49 -0400 Subject: [PATCH 05/15] Added functionality to orient the robot based on which april tag ID is registered. --- .../commands/auto/DriveTowardsThing.java | 56 ++++++++++++++++++- 1 file changed, 54 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java index 48b6210..aeb7c4b 100644 --- a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java +++ b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java @@ -43,6 +43,11 @@ public class DriveTowardsThing extends Command { ElevatorLevel level; int xDirection; int yDirection; + int aprilTagId; + double scoringAngle; + double currentAngle; + double correctionAngle; + double rotationConstant = 1.0; boolean reverseXY; double endTime; @@ -130,6 +135,51 @@ private boolean checkLock() { lastLock[2] = camera.getWidth(); lastLock[3] = camera.getHeight(); System.out.println(camera.getId()); + aprilTagId = camera.getId(); + + switch (aprilTagId){ + case 6: + scoringAngle = 2.0944; + break; + case 7: + scoringAngle = 3.14159; + break; + case 8: + scoringAngle = 4.18879; + break; + case 9: + scoringAngle = 5.23599; + break; + case 10: + scoringAngle = 0; + break; + case 11: + scoringAngle = 1.0472; + break; + case 17: + scoringAngle = 4.1888; + break; + case 18: + scoringAngle = 3.14159; + break; + case 19: + scoringAngle = 2.0944; + break; + case 20: + scoringAngle = 1.0472; + break; + case 21: + scoringAngle = 0; + break; + case 22: + scoringAngle = 5.23599; + break; + default: + scoringAngle = 0; + break; + } + + SmartDashboard.putNumber("Scoring Angle", scoringAngle); return true; } System.out.println("no lock found"); @@ -228,8 +278,10 @@ public void execute() { } else { /* camera has gotten a lock at some point (lastLock is not nil)*/ if(checkLock()) { - updateTarget(); - drivetrain.driveRobotRelative(targetSpeed[1]*xDirection, targetSpeed[0]*yDirection, 0.0); + updateTarget(); + currentAngle = gyro.getRotation2d().getRadians(); + correctionAngle = scoringAngle - currentAngle; + drivetrain.driveRobotRelative(targetSpeed[1]*xDirection, targetSpeed[0]*yDirection, rotationConstant * correctionAngle); } else{ drivetrain.driveRobotRelative(0.0, 0.0, 0.0); } From ac0a9a9ebe1cfc395fdb98be3e205921444dda9c Mon Sep 17 00:00:00 2001 From: p-ifka Date: Sat, 29 Mar 2025 12:33:48 -0400 Subject: [PATCH 06/15] add gyro to drivetowards thing for april tag angle alignment, fix conversion from long to int for tag id --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/auto/DriveTowardsThing.java | 7 +++++-- src/main/java/frc/robot/commands/auto/ScoreTest.java | 10 +++++----- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f7a17cf..d4ff70c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -77,7 +77,7 @@ private void configureBindings() { drivetrain.resetPosition(); })); - OI.driveControllerB.whileTrue(new DriveTowardsThing(1, 1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH)); + OI.driveControllerB.whileTrue(new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH)); // Operator controller bindings // Elevator level controls // OPERATOR B : INTAKE diff --git a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java index aeb7c4b..e4a8fe0 100644 --- a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java +++ b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Gyro; import frc.robot.subsystems.Camera; import frc.robot.subsystems.Elevator; import frc.robot.ElevatorLevel; @@ -38,6 +39,7 @@ */ public class DriveTowardsThing extends Command { Drivetrain drivetrain; + Gyro gyro; Camera camera; Elevator elevator; ElevatorLevel level; @@ -66,9 +68,10 @@ public class DriveTowardsThing extends Command { private static long[] lastLock = new long[4]; private static double[] targetSpeed = new double[2]; - public DriveTowardsThing(int xDirection, int yDirection, boolean reverseXY, Drivetrain drivetrain, Camera camera, Elevator elevator, ElevatorLevel level) { + public DriveTowardsThing(int xDirection, int yDirection, boolean reverseXY, Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, ElevatorLevel level) { addRequirements(drivetrain, camera); this.drivetrain = drivetrain; + this.gyro = gyro; this.camera = camera; this.elevator = elevator; this.level = level; @@ -135,7 +138,7 @@ private boolean checkLock() { lastLock[2] = camera.getWidth(); lastLock[3] = camera.getHeight(); System.out.println(camera.getId()); - aprilTagId = camera.getId(); + aprilTagId = (int) camera.getId(); switch (aprilTagId){ case 6: diff --git a/src/main/java/frc/robot/commands/auto/ScoreTest.java b/src/main/java/frc/robot/commands/auto/ScoreTest.java index 67e6920..66538a6 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreTest.java +++ b/src/main/java/frc/robot/commands/auto/ScoreTest.java @@ -79,7 +79,7 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro new InstantCommand(() -> drivetrain.resetPosition()), new WaitCommand(0.1), new DriveDistance(0.25, drivetrain, gyro, 1.5, 0.0, 0.0), - new DriveTowardsThing(1, -1, true, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) + new DriveTowardsThing(1, -1, true, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) ); scoreL4(elevator, wrist, claw); addCommands( @@ -95,7 +95,7 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro new DriveDistance(0.25, drivetrain, gyro, 0.0, 0.0, 5.23599), new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599), new DriveDistance(0.25, drivetrain, gyro, 0.0, -1.5, 5.23599), - new DriveTowardsThing(1, 1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) + new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) ); scoreL4(elevator, wrist, claw); if(goToStation) { driveOffReef(1, drivetrain, gyro); } @@ -105,7 +105,7 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro new WaitCommand(0.1), new DriveDistance(0.25, drivetrain, gyro, 5.0, 0.0, 4.18879), new DriveDistance(0.25, drivetrain, gyro, 0.0, -1.0, 4.18879), - new DriveTowardsThing(1, 1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) + new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) ); scoreL4(elevator, wrist, claw); if(goToStation) { driveOffReef(1, drivetrain, gyro); } @@ -115,7 +115,7 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro new WaitCommand(0.1), new DriveDistance(0.25, drivetrain, gyro, 5.0, 0.0, 2.09439), new DriveDistance(0.25, drivetrain, gyro, 0.0, 1.0, 2.09439), - new DriveTowardsThing(-1, -1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) + new DriveTowardsThing(-1, -1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) ); scoreL4(elevator, wrist, claw); if(goToStation) { driveOffReef(-1, drivetrain, gyro); } @@ -126,7 +126,7 @@ else if(side == 6) { new InstantCommand(() -> drivetrain.resetPosition()), new WaitCommand(0.1), new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472), - new DriveTowardsThing(-1, -1, false, drivetrain, camera, elevator, ElevatorLevel.EXTRA_HIGH) + new DriveTowardsThing(-1, -1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) ); scoreL4(elevator, wrist, claw); if(goToStation) { driveOffReef(-1, drivetrain, gyro); } From 1cdc92f592ca270e121dbe21bd4c3ffdfba4d482 Mon Sep 17 00:00:00 2001 From: p-ifka Date: Sat, 29 Mar 2025 17:38:58 -0400 Subject: [PATCH 07/15] optimize robotrelative drive, fix angles for drivetowardsthing --- .../commands/auto/DriveTowardsThing.java | 252 +++++++++--------- .../java/frc/robot/subsystems/Drivetrain.java | 6 + src/main/java/frc/robot/subsystems/Gyro.java | 2 +- 3 files changed, 137 insertions(+), 123 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java index e4a8fe0..a8e24d6 100644 --- a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java +++ b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java @@ -13,7 +13,7 @@ /** * The DriveTowardsThing command drives the robot towards a target based on the x-coordinate * provided by the camera. The robot adjusts its direction to align with the target. - * + * *

The command uses the following bounds to determine the direction: *

    *
  • LEFT_BOUND: x < 53
    • @@ -21,7 +21,7 @@ *
  • STRAIGHT_BOUND: 156 <= x < 164
    • *
  • SLIGHT_RIGHT_BOUND: 164 <= x < 265
    • *
- * + * *

When the x-coordinate is within these bounds, the robot will adjust its direction accordingly: *

    *
  • LEFT: Rotate left
    • @@ -30,10 +30,10 @@ *
  • SLIGHT RIGHT: Slightly rotate right
    • *
  • RIGHT: Rotate right
    • *
- * + * *

If the robot is moving straight, a timeout is set to continue moving straight for a certain * number of cycles before re-evaluating the direction. - * + * * @param drivetrain The drivetrain subsystem used to control the robot's movement. * @param camera The camera subsystem used to get the x-coordinate of the target. */ @@ -49,10 +49,10 @@ public class DriveTowardsThing extends Command { double scoringAngle; double currentAngle; double correctionAngle; - double rotationConstant = 1.0; + double rotationConstant = 0.4; boolean reverseXY; double endTime; - + // private int straightTimeout = 0; @@ -67,12 +67,12 @@ public class DriveTowardsThing extends Command { private static boolean hasGottenLock = false; private static long[] lastLock = new long[4]; private static double[] targetSpeed = new double[2]; - + public DriveTowardsThing(int xDirection, int yDirection, boolean reverseXY, Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, ElevatorLevel level) { - addRequirements(drivetrain, camera); - this.drivetrain = drivetrain; - this.gyro = gyro; - this.camera = camera; + addRequirements(drivetrain, camera); + this.drivetrain = drivetrain; + this.gyro = gyro; + this.camera = camera; this.elevator = elevator; this.level = level; @@ -83,18 +83,18 @@ public DriveTowardsThing(int xDirection, int yDirection, boolean reverseXY, Driv } /** - * Executes the DriveTowardsThing command. This method is called repeatedly - * while the command is scheduled to run. It uses the camera's X coordinate + * Executes the DriveTowardsThing command. This method is called repeatedly + * while the command is scheduled to run. It uses the camera's X coordinate * to determine the direction to drive the robot towards a target. - * + * * The robot will adjust its direction based on the following conditions: * - If the X coordinate is less than LEFT_BOUND, it will drive slightly to the left. * - If the X coordinate is between LEFT_BOUND and SLIGHT_LEFT_BOUND, it will drive slightly to the left. * - If the X coordinate is between SLIGHT_LEFT_BOUND and STRAIGHT_BOUND, it will drive straight for a set duration. * - If the X coordinate is between STRAIGHT_BOUND and SLIGHT_RIGHT_BOUND, it will drive slightly to the right. * - If the X coordinate is greater than or equal to SLIGHT_RIGHT_BOUND, it will drive to the right. - * - * If the robot is currently driving straight (indicated by straightTimeout > 0), + * + * If the robot is currently driving straight (indicated by straightTimeout > 0), * it will continue to drive straight until the timeout expires. */ // @Override @@ -139,125 +139,133 @@ private boolean checkLock() { lastLock[3] = camera.getHeight(); System.out.println(camera.getId()); aprilTagId = (int) camera.getId(); - - switch (aprilTagId){ - case 6: - scoringAngle = 2.0944; - break; - case 7: - scoringAngle = 3.14159; - break; - case 8: - scoringAngle = 4.18879; - break; - case 9: - scoringAngle = 5.23599; - break; - case 10: - scoringAngle = 0; - break; - case 11: - scoringAngle = 1.0472; - break; - case 17: - scoringAngle = 4.1888; - break; - case 18: - scoringAngle = 3.14159; - break; - case 19: - scoringAngle = 2.0944; - break; - case 20: - scoringAngle = 1.0472; - break; - case 21: - scoringAngle = 0; - break; - case 22: - scoringAngle = 5.23599; - break; - default: - scoringAngle = 0; - break; - } - - SmartDashboard.putNumber("Scoring Angle", scoringAngle); return true; } System.out.println("no lock found"); - noLockTime++; + noLockTime++; return false; } - private void updateTarget() { - // xSpeed = Math.min(3.0/lastLock[2], 0.25); - // ySpeed = xSpeed; - // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); - // targetSpeed[1] = ySpeed; + private void updateTarget() { + /* set target angle based on the current target id, + values are entered AS DEGREES and converted to radians later */ + switch (aprilTagId){ + case 6: + scoringAngle = 60.0; + break; + case 7: + scoringAngle = 0.0; + break; + case 8: + scoringAngle = 300.0; + break; + case 9: + scoringAngle = 240.0; + break; + case 10: + scoringAngle = 180.0; + break; + case 11: + scoringAngle = 120.0; + break; + case 17: + scoringAngle = 60.0; + break; + case 18: + scoringAngle = 0.0; + break; + case 19: + scoringAngle = 300.0; + break; + case 20: + scoringAngle = 240.0; + break; + case 21: + scoringAngle = 180.0; + break; + case 22: + scoringAngle = 120.0; + break; + default: + scoringAngle = 0.0; + break; + } + + SmartDashboard.putNumber("Scoring Angle", scoringAngle); + // xSpeed = Math.min(3.0/lastLock[2], 0.25); + // ySpeed = xSpeed; + // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + // targetSpeed[1] = ySpeed; // double speed = Math.min(3.0/lastLock[2], 0.25); System.out.println("Updating Target"); - double speed = 0.15; - if(Math.abs(lastLock[0] - 220) < 10) { - targetSpeed[0] = 0.0; + double error = lastLock[0] - 220; + + SmartDashboard.putNumber("cameradrive error", error); + + // drive forward, slow down as error decreases + // targetSpeed[0] = Math.abs(error) * 0.01; + targetSpeed[0] = 0.05; + + // strafe left-right, stop if error < 10 + if(Math.abs(error) < 10) { + targetSpeed[1] = 0.0; } else { - targetSpeed[0] = -((lastLock[0] - 220)/300.0); + targetSpeed[1] = -((error)/300.0); } - - targetSpeed[1] = 0.0; + SmartDashboard.putNumber("Target X", targetSpeed[0]); SmartDashboard.putNumber("Target Y", targetSpeed[1]); - + // xSpeed = Math.min(3.0/lastLock[2], 0.25); // ySpeed = 0.1; // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - 220)/5.83); // targetSpeed[1] = ySpeed; - // if(lastLock[2] > 60) { - // /** if robot is close enough to the tag, - // * start going to the right**/ - // targetCameraX = 220; - // if(lastLock[0] > 115 && lastLock[0] < 270) { - // /** robot is ~ aligned to the right - // * start driving forward **/ - // System.out.println("target 220 forward"); - // xSpeed = Math.min(3.0/lastLock[2], 0.25); - // ySpeed = 0.1; - // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); - // targetSpeed[1] = ySpeed; - // // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; - // } else { - // /** robot is close but not aligned to right, - // * strafe right do not move forward**/ - // System.out.println("target 220 strafe"); - // xSpeed = Math.min(3.0/lastLock[2], 0.25); - // ySpeed = 0.0; - // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); - // targetSpeed[1] = ySpeed; - // // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; - // } - // } else { - // /* robot far away from tag, - // * drive towards middle */ - // System.out.println("target 220"); - // targetCameraX = 220; - // xSpeed = Math.min(3.0/lastLock[2], 0.25); - // ySpeed = 0.15; - // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); - // targetSpeed[1] = ySpeed; - // // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; - - // } + // if(lastLock[2] > 60) { + // /** if robot is close enough to the tag, + // * start going to the right**/ + // targetCameraX = 220; + // if(lastLock[0] > 115 && lastLock[0] < 270) { + // /** robot is ~ aligned to the right + // * start driving forward **/ + // System.out.println("target 220 forward"); + // xSpeed = Math.min(3.0/lastLock[2], 0.25); + // ySpeed = 0.1; + // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + // targetSpeed[1] = ySpeed; + // // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; + // } else { + // /** robot is close but not aligned to right, + // * strafe right do not move forward**/ + // System.out.println("target 220 strafe"); + // xSpeed = Math.min(3.0/lastLock[2], 0.25); + // ySpeed = 0.0; + // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + // targetSpeed[1] = ySpeed; + // // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; + // } + // } else { + // /* robot far away from tag, + // * drive towards middle */ + // System.out.println("target 220"); + // targetCameraX = 220; + // xSpeed = Math.min(3.0/lastLock[2], 0.25); + // ySpeed = 0.15; + // targetSpeed[0] = xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83); + // targetSpeed[1] = ySpeed; + // // double[] o = {(xSpeed * Math.atan((lastLock[0] - targetCameraX)/5.83)), ySpeed}; + + // } } - + @Override public void initialize() { - if(checkLock()) { updateTarget(); } - endTime = System.currentTimeMillis() + 8000; + if(checkLock()) { updateTarget(); } + endTime = System.currentTimeMillis() + 8000; } @Override @@ -270,7 +278,7 @@ public void end(boolean isInterrupted) { drivetrain.driveRobotRelative(0.0, 0.0, 0.0); } - + @Override public void execute() { if(!hasGottenLock) { @@ -278,24 +286,24 @@ public void execute() { /* ideally the robot will be in a position where this is never run*/ if(checkLock()) { updateTarget(); } // drivetrain.driveRobotRelative(0.25, 0.0, 0.0); - } else { + } else { /* camera has gotten a lock at some point (lastLock is not nil)*/ if(checkLock()) { - updateTarget(); - currentAngle = gyro.getRotation2d().getRadians(); - correctionAngle = scoringAngle - currentAngle; - drivetrain.driveRobotRelative(targetSpeed[1]*xDirection, targetSpeed[0]*yDirection, rotationConstant * correctionAngle); + updateTarget(); + currentAngle = gyro.getRotation2d().getRadians() % (2*Math.PI); + correctionAngle = Math.toRadians(scoringAngle) - currentAngle; + drivetrain.driveRobotRelative(targetSpeed[0], targetSpeed[1], rotationConstant * correctionAngle); } else{ - drivetrain.driveRobotRelative(0.0, 0.0, 0.0); + drivetrain.driveRobotRelative(0.0, 0.0, 0.0); } if(lastLock[2] > 80) { - targetCameraX = 220; - elevator.setTarget(level); + targetCameraX = 220; + // elevator.setTarget(level); } } - - + + } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index cc9876b..e56ae41 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -107,6 +107,12 @@ public void driveRobotRelative(double x, double y, double omega) { FREE_SPEED * y, omega); SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeed); + states[0].optimize(new Rotation2d(leftFrontSwerve.getRotationEncoder().getPosition() * (Math.PI * 2))); + states[1].optimize(new Rotation2d(rightFrontSwerve.getRotationEncoder().getPosition() * (Math.PI * 2))); + states[2].optimize(new Rotation2d(leftBackSwerve.getRotationEncoder().getPosition() * (Math.PI * 2))); + states[3].optimize(new Rotation2d(rightBackSwerve.getRotationEncoder().getPosition() * (Math.PI * 2))); + + leftFrontSwerve.setState(states[0]); rightFrontSwerve.setState(states[1]); leftBackSwerve.setState(states[2]); diff --git a/src/main/java/frc/robot/subsystems/Gyro.java b/src/main/java/frc/robot/subsystems/Gyro.java index 66872ca..33ed24f 100644 --- a/src/main/java/frc/robot/subsystems/Gyro.java +++ b/src/main/java/frc/robot/subsystems/Gyro.java @@ -27,7 +27,7 @@ public void reset() { @Override public void periodic() { - SmartDashboard.putNumber("gyro angle", gyro.getRotation2d().getRadians()); + SmartDashboard.putNumber("gyro angle", Math.toDegrees(gyro.getRotation2d().getRadians())); } } From 3bfec98078148f04cbaae96a00b33590a9c6a0ea Mon Sep 17 00:00:00 2001 From: gregk27 Date: Mon, 31 Mar 2025 19:11:33 -0400 Subject: [PATCH 08/15] Add algae motor to robotmap --- src/main/java/frc/robot/RobotMap.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index fcbcbc3..4746240 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -40,6 +40,8 @@ private class CAN { /* MECHANISMS */ private static final int CORAL_CLAW_MOTOR_PRIMARY_CAN = 11; // First coral claw motor private static final int CORAL_CLAW_MOTOR_SECONDARY_CAN = 12; // Second coral claw motor + private static final int ALGAE_CLAW_MOTOR_CAN = 13; // Algae claw motor + /** * FRONT @@ -109,6 +111,7 @@ private class Pneumatics { // Create two NEO motors for the coral claw public static final SparkMax coralClawPrimaryMotor = new SparkMax(CAN.CORAL_CLAW_MOTOR_PRIMARY_CAN, MotorType.kBrushless); public static final SparkMax coralClawSecondaryMotor = new SparkMax(CAN.CORAL_CLAW_MOTOR_SECONDARY_CAN, MotorType.kBrushless); + public static final SparkMax algaeClawMotor = new SparkMax(CAN.ALGAE_CLAW_MOTOR_CAN, MotorType.kBrushless); // Coral claw limit switch public static final DigitalInput coralLimitSwitch = new DigitalInput(DIO.CORAL_LIMIT_SWITCH); From e95e2a3274ca2c73ee5378527c474c6774760ddb Mon Sep 17 00:00:00 2001 From: p-ifka Date: Mon, 31 Mar 2025 21:40:41 -0400 Subject: [PATCH 09/15] optimize turns in drivetowardsthing --- .../commands/auto/DriveTowardsThing.java | 34 ++++++++++++++----- .../java/frc/robot/subsystems/Drivetrain.java | 11 ++++++ src/main/java/frc/robot/subsystems/Gyro.java | 9 ++++- 3 files changed, 45 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java index a8e24d6..e9b4495 100644 --- a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java +++ b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java @@ -126,6 +126,15 @@ public DriveTowardsThing(int xDirection, int yDirection, boolean reverseXY, Driv // } + private double shortestAngleDistance(double from, double to) { + // TODO: find a better place to put this + double diff = (to - from + Math.PI) % (2*Math.PI); + if(diff < 0) { + diff += 2*Math.PI; + } + return diff - Math.PI; + } + private boolean checkLock() { /* check if camera is locked, if it is update 'lastlock'*/ if(camera.isLocked()) { @@ -151,25 +160,25 @@ private void updateTarget() { values are entered AS DEGREES and converted to radians later */ switch (aprilTagId){ case 6: - scoringAngle = 60.0; + scoringAngle = 300.0; break; case 7: scoringAngle = 0.0; break; case 8: - scoringAngle = 300.0; + scoringAngle = 60.0; break; case 9: - scoringAngle = 240.0; + scoringAngle = 120.0; break; case 10: scoringAngle = 180.0; break; case 11: - scoringAngle = 120.0; + scoringAngle = 240.0; break; case 17: - scoringAngle = 60.0; + scoringAngle = 300.0; break; case 18: scoringAngle = 0.0; @@ -190,6 +199,15 @@ private void updateTarget() { scoringAngle = 0.0; break; } + scoringAngle = Math.toRadians(scoringAngle); + currentAngle = gyro.getRotation2d().getRadians() % (2*Math.PI); + correctionAngle = shortestAngleDistance(currentAngle, scoringAngle); + System.out.println("scoring angle: " + scoringAngle + ", current angle: " + currentAngle + ", correction: " + correctionAngle); + + + // correctionAngle = Math.toRadians(scoringAngle) - currentAngle; + + SmartDashboard.putNumber("Scoring Angle", scoringAngle); // xSpeed = Math.min(3.0/lastLock[2], 0.25); @@ -205,6 +223,7 @@ private void updateTarget() { // drive forward, slow down as error decreases // targetSpeed[0] = Math.abs(error) * 0.01; targetSpeed[0] = 0.05; + // targetSpeed[0] = 0.00; // strafe left-right, stop if error < 10 if(Math.abs(error) < 10) { @@ -290,11 +309,10 @@ public void execute() { /* camera has gotten a lock at some point (lastLock is not nil)*/ if(checkLock()) { updateTarget(); - currentAngle = gyro.getRotation2d().getRadians() % (2*Math.PI); - correctionAngle = Math.toRadians(scoringAngle) - currentAngle; drivetrain.driveRobotRelative(targetSpeed[0], targetSpeed[1], rotationConstant * correctionAngle); } else{ - drivetrain.driveRobotRelative(0.0, 0.0, 0.0); + updateTarget(); + drivetrain.driveRobotRelative(0.0, 0.0, rotationConstant * correctionAngle); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index e56ae41..f0ad857 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -132,8 +132,19 @@ public void resetPosition() { leftBackSwerve.getPos(), rightBackSwerve.getPos() }, new Pose2d(0.0, 0.0, new Rotation2d(0.0))); + } + + public void setRotation(double desiredAngle) { + gyro.setGyro(desiredAngle); + odometry.resetPosition(new Rotation2d(desiredAngle), new SwerveModulePosition[] { + leftFrontSwerve.getPos(), + rightFrontSwerve.getPos(), + leftBackSwerve.getPos(), + rightBackSwerve.getPos() + }, new Pose2d(getPose2d().getX(), getPose2d().getY(), new Rotation2d(desiredAngle))); } + @Override public void periodic() { odometry.update(gyro.getRotation2d(), new SwerveModulePosition[] { diff --git a/src/main/java/frc/robot/subsystems/Gyro.java b/src/main/java/frc/robot/subsystems/Gyro.java index 33ed24f..60c2335 100644 --- a/src/main/java/frc/robot/subsystems/Gyro.java +++ b/src/main/java/frc/robot/subsystems/Gyro.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Gyro extends SubsystemBase { + + private static Rotation2d offset = new Rotation2d(0); AHRS gyro; public Gyro(AHRS gyro) { @@ -14,13 +16,18 @@ public Gyro(AHRS gyro) { } public Rotation2d getRotation2d() { - return gyro.getRotation2d(); + return gyro.getRotation2d().plus(offset); } public void reset() { gyro.reset(); } + public void setGyro(double desiredAngle) { + gyro.reset(); + offset = new Rotation2d(desiredAngle); + } + // public void setAngle(Rotation2d r) { // gyro.setAngleAdjustment(r.getDegrees()); // } From af231ba76e765a923c61679e9a6529257d13c513 Mon Sep 17 00:00:00 2001 From: gregk27 Date: Mon, 31 Mar 2025 21:56:05 -0400 Subject: [PATCH 10/15] Drive algae wheels on intake triggers --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/CoralClaw.java | 10 +++++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d4ff70c..ec4e596 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,7 +34,7 @@ public class RobotContainer { public Gyro gyro = new Gyro(RobotMap.gyro); public Elevator elevator = new Elevator(); public Wrist wrist = new Wrist(RobotMap.wristSolenoid); - public CoralClaw coralClaw = new CoralClaw(RobotMap.coralClawPrimaryMotor, RobotMap.coralClawSecondaryMotor, RobotMap.coralLimitSwitch); + public CoralClaw coralClaw = new CoralClaw(RobotMap.coralClawPrimaryMotor, RobotMap.coralClawSecondaryMotor, RobotMap.algaeClawMotor, RobotMap.coralLimitSwitch); public Camera camera = new Camera(); public Drivetrain drivetrain = new Drivetrain(leftBackSwerve, rightBackSwerve, leftFrontSwerve, rightFrontSwerve, gyro); diff --git a/src/main/java/frc/robot/subsystems/CoralClaw.java b/src/main/java/frc/robot/subsystems/CoralClaw.java index 8b34765..705c246 100644 --- a/src/main/java/frc/robot/subsystems/CoralClaw.java +++ b/src/main/java/frc/robot/subsystems/CoralClaw.java @@ -68,6 +68,7 @@ public class CoralClaw extends SubsystemBase { private final SparkMax primaryMotor; private final SparkMax secondaryMotor; + private final SparkMax algaeMotor; private final DigitalInput limitSwitch; // Simulation specific fields @@ -92,11 +93,13 @@ public class CoralClaw extends SubsystemBase { * * @param primaryMotor The first SparkMax controller for the intake/outtake wheels * @param secondaryMotor The second SparkMax controller for the intake/outtake wheels + * @param algaeMotor The SparkMax controller for the algae wheels * @param limitSwitch The limit switch that detects when a coral is captured */ - public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, DigitalInput limitSwitch) { + public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, SparkMax algaeMotor, DigitalInput limitSwitch) { this.primaryMotor = primaryMotor; this.secondaryMotor = secondaryMotor; + this.algaeMotor = algaeMotor; this.limitSwitch = limitSwitch; this.previousCoralDetected = isCoralDetected(); // Initialize previous state @@ -105,6 +108,7 @@ public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, DigitalInput li config.idleMode(IdleMode.kCoast); primaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); secondaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + algaeMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); // config.follow(primaryMotor, true); // true makes it follow with inversion // Initialize simulation-specific components if in simulation @@ -123,6 +127,7 @@ public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, DigitalInput li public void intake() { if(motorsOn[0]) { primaryMotor.set(INTAKE_SPEED); } if(motorsOn[1]) { secondaryMotor.set(INTAKE_SPEED); } + algaeMotor.set(-INTAKE_SPEED); // Secondary motor automatically follows with inversion // autoIntakeMode = true; SmartDashboard.putString("Coral Claw State", "Intaking (Auto)"); @@ -134,6 +139,7 @@ public void intake() { public void outtake() { if(motorsOn[0]) { primaryMotor.set(OUTTAKE_SPEED); } if(motorsOn[1]) { secondaryMotor.set(OUTTAKE_SPEED); } + algaeMotor.set(-OUTTAKE_SPEED); // Secondary motor automatically follows with inversion // autoIntakeMode = false; @@ -156,6 +162,7 @@ public void setMotorsOn(boolean left, boolean right) { public void stop() { primaryMotor.set(0); secondaryMotor.set(0); + algaeMotor.set(0); // Secondary motor automatically follows with inversion // autoIntakeMode = false; SmartDashboard.putString("Coral Claw State", "Stopped"); @@ -209,6 +216,7 @@ public void setSimulatedCoralDetected(boolean detected) { public void setOutput(double out) { if(motorsOn[0]) { primaryMotor.set(out); } if(motorsOn[1]) { secondaryMotor.set(-out); } + algaeMotor.set(-out); } @Override From 4bde143333e9454907070be705c80968d8797cd6 Mon Sep 17 00:00:00 2001 From: gregk27 Date: Mon, 31 Mar 2025 23:22:23 -0400 Subject: [PATCH 11/15] Re-tune the elevator to improve performance --- .../java/frc/robot/subsystems/Elevator.java | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 955d5a8..a05cd5e 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -40,14 +40,14 @@ public class Elevator extends SubsystemBase { // PID Control constants - adjusted for better stability (will be adjusted again when we run on the robot for real) - private static final double P_GAIN = 2.5; // Reduced from 1.5 to reduce oscillation - private static final double I_GAIN = 0.5; // Increased from 0.1 to help with steady-state error - private static final double D_GAIN = 0.3; // Increased from 0.2 for better damping + private static final double P_GAIN = 4; // Reduced from 1.5 to reduce oscillation + private static final double I_GAIN = 0.0; // Increased from 0.1 to help with steady-state error + private static final double D_GAIN = 0; // Increased from 0.2 for better damping private static final double MAX_INTEGRAL = 1.0; // Anti-windup limit // Feedforward constants - private static final double FF_UP = 0.10; // 12% feedforward when moving up - private static final double FF_DOWN = 0.08; // 2% feedforward when moving down + private static final double FF_UP = .08; // 12% feedforward when moving up + private static final double FF_DOWN = .08; // 2% feedforward when moving down // Speed limitation for safety and control private static final double MAX_SPEED = 0.8; // Reduced from 700 to operate at safer speed on real robot @@ -287,8 +287,14 @@ public void periodic() { ffTerm = FF_DOWN; } + + SmartDashboard.putNumber("P Out", pTerm); + SmartDashboard.putNumber("FF Out", ffTerm); + SmartDashboard.putNumber("I Out", iTerm); + SmartDashboard.putNumber("D Out", iTerm); + // Calculate the combined output with feedforward - double output = pTerm + ffTerm + iTerm; // + iTerm + dTerm ; + double output = pTerm + ffTerm + iTerm + dTerm; // + iTerm + dTerm ; // SmartDashboard.putNumber("elevator output term", output); // Limit the output for smoother operation @@ -320,15 +326,15 @@ public void periodic() { // } // Update SmartDashboard with current height, target info, and safety limits - // SmartDashboard.putNumber("Elevator Height (m)", getHeight()); - SmartDashboard.putNumber("Elevator Height (in)", Units.metersToInches(getHeight())); - // SmartDashboard.putNumber("Elevator Target Height (m)", targetHeight); + SmartDashboard.putNumber("Elevator Height (m)", getHeight()); + // SmartDashboard.putNumber("Elevator Height (in)", Units.metersToInches(getHeight())); + SmartDashboard.putNumber("Elevator Target Height (m)", targetHeight); // SmartDashboard.putNumber("Height from Ground (m)", getHeight() + BASE_HEIGHT_OFFSET_METERS); - SmartDashboard.putNumber("Height from Ground (in)", Units.metersToInches(getHeight() + BASE_HEIGHT_OFFSET_METERS)); + // SmartDashboard.putNumber("Height from Ground (in)", Units.metersToInches(getHeight() + BASE_HEIGHT_OFFSET_METERS)); // SmartDashboard.putBoolean("Elevator At Target", isAtTarget()); // SmartDashboard.putNumber("Elevator Max Height (m)", MAX_HEIGHT_METERS); // SmartDashboard.putNumber("Elevator Min Height (m)", MIN_HEIGHT_METERS); - // SmartDashboard.putNumber("Elevator Error", targetHeight - getHeight()); + SmartDashboard.putNumber("Elevator Error (m)", targetHeight - getHeight()); // SmartDashboard.putNumber("Elevator Error Sum", errorSum); // SmartDashboard.putNumber("At Target Counter", atTargetCounter); // SmartDashboard.putNumber("Position Tolerance", POSITION_TOLERANCE); From 75a13f3586e0fc2be7698417517405e1dc399edb Mon Sep 17 00:00:00 2001 From: Adam G Date: Tue, 1 Apr 2025 06:02:08 -0400 Subject: [PATCH 12/15] Lower P_GAIN and speed when lowering, higher when raising. --- .../java/frc/robot/subsystems/Elevator.java | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index a05cd5e..0fec978 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -40,7 +40,7 @@ public class Elevator extends SubsystemBase { // PID Control constants - adjusted for better stability (will be adjusted again when we run on the robot for real) - private static final double P_GAIN = 4; // Reduced from 1.5 to reduce oscillation + private static double P_GAIN = 4; // Initial value. Will be changed later based on raising vs loweing. private static final double I_GAIN = 0.0; // Increased from 0.1 to help with steady-state error private static final double D_GAIN = 0; // Increased from 0.2 for better damping private static final double MAX_INTEGRAL = 1.0; // Anti-windup limit @@ -50,7 +50,8 @@ public class Elevator extends SubsystemBase { private static final double FF_DOWN = .08; // 2% feedforward when moving down // Speed limitation for safety and control - private static final double MAX_SPEED = 0.8; // Reduced from 700 to operate at safer speed on real robot + private static final double MAX_RAISING_SPEED = 0.8; // Maximum motor power in up direction + private static final double MAX_LOWERING_SPEED = 0.5; // Maximum motor power in down direction // PID tracking variables private double error = 0; @@ -125,6 +126,14 @@ public void setSpeed(double speed) { speed = 0; } + // Making P_GAIN higher for raising, lower for lowering. To prevent equipment damage and overshoot when lowering. + if (currentHeight > targetHeight){ + P_GAIN = 1.5; + } + else { + P_GAIN = 4; + } + leader.set(speed); // No need to set follower speed - it follows the leader } @@ -298,13 +307,14 @@ public void periodic() { // SmartDashboard.putNumber("elevator output term", output); // Limit the output for smoother operation - double speed = Math.max(-MAX_SPEED, Math.min(MAX_SPEED, output)); + double speed = Math.max(-MAX_LOWERING_SPEED, Math.min(MAX_RAISING_SPEED, output)); // SmartDashboard.putNumber("elevator output speed", speed); // Set the speed to move the elevator - if(error < 0) { - speed = speed * 0.8; - } + //if(error < 0) { + //speed = speed * 0.8; + //} + setSpeed(speed); // Store current values for next iteration From 8d546a0ee0351dcd2a20205b1afd65e3c0102561 Mon Sep 17 00:00:00 2001 From: p-ifka Date: Wed, 2 Apr 2025 08:48:17 -0400 Subject: [PATCH 13/15] change elevator heights, add 180 offset to gyro at the beginning of auto, fix side 1 and 2 autos --- src/main/java/frc/robot/OI.java | 95 ++++---- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/commands/DriveCommand.java | 8 +- .../robot/commands/auto/DriveDistance.java | 141 +++++------ .../commands/auto/DriveTowardsThing.java | 26 ++- .../frc/robot/commands/auto/ScoreTest.java | 219 +++++++++--------- .../java/frc/robot/subsystems/CoralClaw.java | 6 +- .../java/frc/robot/subsystems/Elevator.java | 10 +- src/main/java/frc/robot/subsystems/Gyro.java | 3 +- 9 files changed, 258 insertions(+), 254 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index a38561a..8079ec3 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -1,6 +1,7 @@ package frc.robot; import java.util.function.DoubleSupplier; +import java.util.function.BooleanSupplier; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -9,9 +10,9 @@ import edu.wpi.first.wpilibj.XboxController; public class OI { - + // Reference to RobotContainer for accessing subsystems - + /** * Initialize OI with a reference to RobotContainer * @param container The RobotContainer instance @@ -21,8 +22,8 @@ public class OI { * Controller port mappings */ private static class PORT { - private static final int DRIVE_CONTROLLER_PORT = 0; - private static final int OPERATOR_CONTROLLER_PORT = 1; // Added operator controller port + private static final int DRIVE_CONTROLLER_PORT = 0; + private static final int OPERATOR_CONTROLLER_PORT = 1; // Added operator controller port } // Trigger thresholds public static final double TRIGGER_THRESHOLD = 0.1; @@ -33,7 +34,7 @@ private static class PORT { */ private static final XboxController driveController = new XboxController(PORT.DRIVE_CONTROLLER_PORT); public static final XboxController operatorController = new XboxController(PORT.OPERATOR_CONTROLLER_PORT); // Changed to public - + private static class DRIVER_MAP { private static final int ABUTTON = XboxController.Button.kA.value; private static final int BBUTTON = XboxController.Button.kB.value; @@ -46,60 +47,60 @@ private static class DRIVER_MAP { public static DoubleSupplier xboxRightStickYSupplier = () -> { return processRotationInput(driveController.getRightY()); }; // drive buttons - public static final JoystickButton driveControllerA = new JoystickButton(driveController, DRIVER_MAP.ABUTTON); + public static final JoystickButton driveControllerA = new JoystickButton(driveController, DRIVER_MAP.ABUTTON); public static final JoystickButton driveControllerB = new JoystickButton(driveController, DRIVER_MAP.BBUTTON); public static DoubleSupplier driveControllerRightTriggerSupplier = () -> { return driveController.getRightTriggerAxis();}; - - - + + + private static double processDriveInput(double raw) { - if (Math.abs(raw) < 0.1) { - raw = 0 ; - } - return raw; + if (Math.abs(raw) < 0.1) { + raw = 0 ; + } + return raw; } private static double processRotationInput(double raw) { - if (Math.abs(raw) < 0.1) { - raw = 0; - } - return raw*3; + if (Math.abs(raw) < 0.1) { + raw = 0; + } + return raw*3; } - + /** * Bindings on operator controls */ private static class operatorBindings { - private static final int A_BUTTON = XboxController.Button.kA.value; - private static final int B_BUTTON = XboxController.Button.kB.value; - private static final int X_BUTTON = XboxController.Button.kX.value; - private static final int Y_BUTTON = XboxController.Button.kY.value; - private static final int LEFT_BUMPER = XboxController.Button.kLeftBumper.value; - private static final int RIGHT_BUMPER = XboxController.Button.kRightBumper.value; - private static final int START_BUTTON = XboxController.Button.kStart.value; - private static final int BACK_BUTTON = XboxController.Button.kBack.value; - + private static final int A_BUTTON = XboxController.Button.kA.value; + private static final int B_BUTTON = XboxController.Button.kB.value; + private static final int X_BUTTON = XboxController.Button.kX.value; + private static final int Y_BUTTON = XboxController.Button.kY.value; + private static final int LEFT_BUMPER = XboxController.Button.kLeftBumper.value; + private static final int RIGHT_BUMPER = XboxController.Button.kRightBumper.value; + private static final int START_BUTTON = XboxController.Button.kStart.value; + private static final int BACK_BUTTON = XboxController.Button.kBack.value; + } - public static final JoystickButton operatorControllerA = new JoystickButton(operatorController, operatorBindings.A_BUTTON); - public static final JoystickButton operatorControllerB = new JoystickButton(operatorController, operatorBindings.B_BUTTON); - public static final JoystickButton operatorControllerX = new JoystickButton(operatorController, operatorBindings.X_BUTTON); - public static final JoystickButton operatorControllerY = new JoystickButton(operatorController, operatorBindings.Y_BUTTON); - public static final JoystickButton operatorControllerLeftBumper = new JoystickButton(operatorController, operatorBindings.LEFT_BUMPER); - public static final JoystickButton operatorControllerRightBumper = new JoystickButton(operatorController, operatorBindings.RIGHT_BUMPER); - public static final JoystickButton operatorControllerStart = new JoystickButton(operatorController, operatorBindings.START_BUTTON); - public static final JoystickButton operatorControllerBack = new JoystickButton(operatorController, operatorBindings.BACK_BUTTON); - - // Trigger controls for coral claw + public static final JoystickButton operatorControllerA = new JoystickButton(operatorController, operatorBindings.A_BUTTON); + public static final JoystickButton operatorControllerB = new JoystickButton(operatorController, operatorBindings.B_BUTTON); + public static final JoystickButton operatorControllerX = new JoystickButton(operatorController, operatorBindings.X_BUTTON); + public static final JoystickButton operatorControllerY = new JoystickButton(operatorController, operatorBindings.Y_BUTTON); + public static final JoystickButton operatorControllerLeftBumper = new JoystickButton(operatorController, operatorBindings.LEFT_BUMPER); + public static final JoystickButton operatorControllerRightBumper = new JoystickButton(operatorController, operatorBindings.RIGHT_BUMPER); + public static final JoystickButton operatorControllerStart = new JoystickButton(operatorController, operatorBindings.START_BUTTON); + public static final JoystickButton operatorControllerBack = new JoystickButton(operatorController, operatorBindings.BACK_BUTTON); + + // Trigger controls for coral claw public static DoubleSupplier operatorRightTriggerSupplier= () -> { return operatorController.getRightTriggerAxis();}; - public static final Trigger operatorRightTrigger = new Trigger(() -> - operatorController.getRightTriggerAxis() > TRIGGER_THRESHOLD); - - public static DoubleSupplier operatorLeftTriggerSupplier= () -> { return operatorController.getLeftTriggerAxis();}; + public static final Trigger operatorRightTrigger = new Trigger(() -> + operatorController.getRightTriggerAxis() > TRIGGER_THRESHOLD); - public static final Trigger operatorLeftTrigger = new Trigger(() -> - operatorController.getLeftTriggerAxis() > TRIGGER_THRESHOLD); - - public static double processElevatorInput(double input) { - return -Math.signum(input) * Math.pow(Math.abs(input), 2) * 0.7; - } + public static DoubleSupplier operatorLeftTriggerSupplier= () -> { return operatorController.getLeftTriggerAxis();}; + public static final Trigger operatorLeftTrigger = new Trigger(() -> + operatorController.getLeftTriggerAxis() > TRIGGER_THRESHOLD); + public static BooleanSupplier operatorLeftStickButtonSupplier = () -> { return operatorController.getLeftStickButton(); }; + + public static double processElevatorInput(double input) { + return -Math.signum(input) * Math.pow(Math.abs(input), 2) * 0.7; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ec4e596..9094f9e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,7 +52,7 @@ public class RobotContainer { public RobotContainer() { // Initialize OI with a reference to this RobotContainer instance configureBindings(); - drivetrain.setDefaultCommand(new DriveCommand(drivetrain, elevator, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier, OI.driveControllerRightTriggerSupplier)); + drivetrain.setDefaultCommand(new DriveCommand(drivetrain, elevator, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier, OI.driveControllerRightTriggerSupplier, OI.operatorLeftStickButtonSupplier)); RobotMap.compressor.enableAnalog(70, 120); autoSelector.setDefaultOption("default side (2)", AUTO_DEFAULT); @@ -77,7 +77,7 @@ private void configureBindings() { drivetrain.resetPosition(); })); - OI.driveControllerB.whileTrue(new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH)); + OI.driveControllerB.whileTrue(new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH)); // Operator controller bindings // Elevator level controls // OPERATOR B : INTAKE diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 5e7dd9c..add6eb6 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -2,6 +2,7 @@ package frc.robot.commands; import java.util.function.DoubleSupplier; +import java.util.function.BooleanSupplier; import edu.wpi.first.wpilibj2.command.Command; @@ -17,9 +18,10 @@ public class DriveCommand extends Command { DoubleSupplier ySupplier; DoubleSupplier omegaSupplier; DoubleSupplier triggerSupplier; + BooleanSupplier slowDownSupplier; /** drive robot according to the x and y of the drive controller, drive at half speed by default, increase by half the value of the drive trigger (0-1)**/ public DriveCommand(Drivetrain drivetrain, Elevator elevator, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier, DoubleSupplier triggerSupplier) { + DoubleSupplier omegaSupplier, DoubleSupplier triggerSupplier, BooleanSupplier slowDownSupplier) { addRequirements(drivetrain); this.drivetrain = drivetrain; this.elevator = elevator; @@ -27,6 +29,8 @@ public DriveCommand(Drivetrain drivetrain, Elevator elevator, DoubleSupplier xSu this.ySupplier = ySupplier; this.omegaSupplier = omegaSupplier; this.triggerSupplier = triggerSupplier; + this.slowDownSupplier = slowDownSupplier; + } @Override @@ -35,7 +39,7 @@ public void initialize() { @Override public void execute() { - if(elevator.getTargetLevel() == ElevatorLevel.EXTRA_HIGH) { + if(elevator.getTargetLevel() == ElevatorLevel.EXTRA_HIGH || slowDownSupplier.getAsBoolean()) { drivetrain.drive(ySupplier.getAsDouble() * 0.5, xSupplier.getAsDouble() * 0.5, omegaSupplier.getAsDouble() * 0.5); } else { drivetrain.drive(ySupplier.getAsDouble(), xSupplier.getAsDouble(), omegaSupplier.getAsDouble()); diff --git a/src/main/java/frc/robot/commands/auto/DriveDistance.java b/src/main/java/frc/robot/commands/auto/DriveDistance.java index c2f7f1a..b9f248a 100644 --- a/src/main/java/frc/robot/commands/auto/DriveDistance.java +++ b/src/main/java/frc/robot/commands/auto/DriveDistance.java @@ -9,88 +9,89 @@ public class DriveDistance extends Command { - Drivetrain drivetrain; - Gyro gyro; - Pose2d target; - double xDistance; - double yDistance; - double theta; + Drivetrain drivetrain; + Gyro gyro; + Pose2d target; + double xDistance; + double yDistance; + double theta; - double xDisplacement; - double yDisplacement; - double angleDisplacement; + double xDisplacement; + double yDisplacement; + double angleDisplacement; - double xyThreshold = 0.6; - double xyMaxSpeed; - double rotateMaxSpeed = 1.5; + double xyThreshold = 0.6; + double xyMaxSpeed; + double rotateMaxSpeed = 1.5; public DriveDistance(double xyMaxSpeed, Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) { - this.drivetrain = drivetrain; - this.gyro = gyro; - this.xDistance = xDistance; - this.yDistance = yDistance; - this.theta = theta; - this.xyMaxSpeed = xyMaxSpeed; - addRequirements(drivetrain); + this.drivetrain = drivetrain; + this.gyro = gyro; + this.xDistance = xDistance; + this.yDistance = yDistance; + this.theta = theta; + this.xyMaxSpeed = xyMaxSpeed; + addRequirements(drivetrain); - } - - @Override - public void initialize() { - System.out.println("drivedistance initialized"); - Pose2d start = drivetrain.getPose2d(); - this.target = new Pose2d((start.getX() + xDistance), (start.getY() + yDistance), new Rotation2d(theta)); - } - - @Override - public void execute() { - // TODO: add conversion to inches if needed - Pose2d currentPos = drivetrain.getPose2d(); - this.xDisplacement = target.getX() - currentPos.getX(); - double xSpeed = xDisplacement * 0.6; - if (xSpeed > 0) { - xSpeed = Math.min(xSpeed, xyMaxSpeed); - } else { - xSpeed = Math.max(xSpeed, -xyMaxSpeed); - } - // if (Math.abs(xDisplacement) <= xyThreshold) { - // xSpeed = 0.0; - // } + } - SmartDashboard.putNumber("xDisplacement", xDisplacement); - this.yDisplacement = target.getY() - currentPos.getY(); - double ySpeed = yDisplacement * 0.6; - if (ySpeed > 0) { - ySpeed = Math.min(ySpeed, xyMaxSpeed); - } else { - ySpeed = Math.max(ySpeed, -xyMaxSpeed); - } - // if (Math.abs(yDisplacement) <= xyThreshold) { - // ySpeed = 0.0; - // } - SmartDashboard.putNumber("yDisplacement", yDisplacement); - this.angleDisplacement = (gyro.getRotation2d().minus(target.getRotation())).getRadians(); - double rotateSpeed = angleDisplacement * 1.0; - if (rotateSpeed > 0) { - rotateSpeed = Math.min(rotateSpeed, rotateMaxSpeed); - } else { - rotateSpeed = Math.max(rotateSpeed, -rotateMaxSpeed); - } + @Override + public void initialize() { + System.out.println("drivedistance initialized"); + Pose2d start = drivetrain.getPose2d(); + this.target = new Pose2d((start.getX() + xDistance), (start.getY() + yDistance), new Rotation2d(theta)); + } - SmartDashboard.putNumber("angleDisplacement", angleDisplacement); - // System.out.println("x displacement: " + xDisplacement); - // System.out.println("y displacement: " + yDisplacement); - // System.out.println("angle displacement: ", angleDisplacement); - drivetrain.drive(xSpeed, ySpeed, -rotateSpeed * 1.0); + @Override + public void execute() { + // TODO: add conversion to inches if needed + Pose2d currentPos = drivetrain.getPose2d(); + this.xDisplacement = target.getX() - currentPos.getX(); + double xSpeed = xDisplacement * 0.6; + if (xSpeed > 0) { + xSpeed = Math.min(xSpeed, xyMaxSpeed); + } else { + xSpeed = Math.max(xSpeed, -xyMaxSpeed); } + // if (Math.abs(xDisplacement) <= xyThreshold) { + // xSpeed = 0.0; + // } - @Override - public void end(boolean interrupted) { - drivetrain.drive(0.0, 0.0, 0.0); + SmartDashboard.putNumber("xDisplacement", xDisplacement); + this.yDisplacement = target.getY() - currentPos.getY(); + double ySpeed = yDisplacement * 0.6; + if (ySpeed > 0) { + ySpeed = Math.min(ySpeed, xyMaxSpeed); + } else { + ySpeed = Math.max(ySpeed, -xyMaxSpeed); + } + // if (Math.abs(yDisplacement) <= xyThreshold) { + // ySpeed = 0.0; + // } + SmartDashboard.putNumber("yDisplacement", yDisplacement); + this.angleDisplacement = (gyro.getRotation2d().minus(target.getRotation())).getRadians(); + double rotateSpeed = angleDisplacement * 2.2; + if (rotateSpeed > 0) { + rotateSpeed = Math.min(rotateSpeed, rotateMaxSpeed); + } else { + rotateSpeed = Math.max(rotateSpeed, -rotateMaxSpeed); } + SmartDashboard.putNumber("angleDisplacement", angleDisplacement); + // System.out.println("x displacement: " + xDisplacement); + // System.out.println("y displacement: " + yDisplacement); + // System.out.println("angle displacement: ", angleDisplacement); + drivetrain.drive(xSpeed, ySpeed, -rotateSpeed * 1.0); + } + + @Override + public void end(boolean interrupted) { + System.out.println("drivedistance ended"); + drivetrain.drive(0.0, 0.0, 0.0); + } + @Override public boolean isFinished() { - return (Math.abs(xDisplacement) <= xyThreshold && Math.abs(yDisplacement) <= xyThreshold && angleDisplacement <= 0.4); + return (Math.abs(xDisplacement) <= xyThreshold && Math.abs(yDisplacement) <= xyThreshold && Math.abs(angleDisplacement) <= 0.05); } } diff --git a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java index e9b4495..2c810be 100644 --- a/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java +++ b/src/main/java/frc/robot/commands/auto/DriveTowardsThing.java @@ -7,6 +7,7 @@ import frc.robot.subsystems.Gyro; import frc.robot.subsystems.Camera; import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Wrist; import frc.robot.ElevatorLevel; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -42,6 +43,7 @@ public class DriveTowardsThing extends Command { Gyro gyro; Camera camera; Elevator elevator; + Wrist wrist; ElevatorLevel level; int xDirection; int yDirection; @@ -68,12 +70,14 @@ public class DriveTowardsThing extends Command { private static long[] lastLock = new long[4]; private static double[] targetSpeed = new double[2]; - public DriveTowardsThing(int xDirection, int yDirection, boolean reverseXY, Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, ElevatorLevel level) { + public DriveTowardsThing(int xDirection, int yDirection, boolean reverseXY, Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, Wrist wrist, ElevatorLevel level) { addRequirements(drivetrain, camera); this.drivetrain = drivetrain; this.gyro = gyro; this.camera = camera; this.elevator = elevator; + this.wrist = wrist; + this.level = level; this.xDirection = xDirection; @@ -226,11 +230,11 @@ private void updateTarget() { // targetSpeed[0] = 0.00; // strafe left-right, stop if error < 10 - if(Math.abs(error) < 10) { - targetSpeed[1] = 0.0; - } else { - targetSpeed[1] = -((error)/300.0); - } + // if(Math.abs(error) < 10) { + // targetSpeed[1] = 0.0; + // } else { + targetSpeed[1] = -((error)/300.0); + // } SmartDashboard.putNumber("Target X", targetSpeed[0]); @@ -289,7 +293,7 @@ public void initialize() { @Override public boolean isFinished() { - return (camera.getWidth() > 225 || System.currentTimeMillis() >= endTime); + return (camera.getWidth() > 118); } @Override @@ -315,10 +319,10 @@ public void execute() { drivetrain.driveRobotRelative(0.0, 0.0, rotationConstant * correctionAngle); } - - if(lastLock[2] > 80) { - targetCameraX = 220; - // elevator.setTarget(level); + ; + if(lastLock[2] > 60) { + wrist.extend(); + elevator.setTarget(level); } } diff --git a/src/main/java/frc/robot/commands/auto/ScoreTest.java b/src/main/java/frc/robot/commands/auto/ScoreTest.java index 66538a6..5c429d5 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreTest.java +++ b/src/main/java/frc/robot/commands/auto/ScoreTest.java @@ -5,6 +5,7 @@ import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Wrist; import frc.robot.subsystems.CoralClaw; +import frc.robot.IDLookUp; import frc.robot.commands.auto.DriveDistance; import frc.robot.commands.IntakeCommandGroup; import frc.robot.commands.ScoreCommandGroup; @@ -17,158 +18,148 @@ /** * The ScoreTest command is a SequentialCommandGroup that performs a series of driving maneuvers * using the provided drivetrain, gyro, and camera. The sequence of commands includes: - * + * * 1. Resetting the drivetrain's position. * 2. Driving forward a specified distance to reach a wall. * 3. Strafing left to a specific position (ID7). * 4. Correcting any drift from the previous command. * 5. Strafing left again to the same position (ID7). * 6. Driving backward to the starting wall. - * + * * @param drivetrain The drivetrain subsystem used to control the robot's movement. * @param gyro The gyro sensor used to maintain orientation. * @param camera The camera used for vision processing (currently commented out). */ public class ScoreTest extends SequentialCommandGroup { - /** - * Command sequence for scoring test in autonomous mode. - * - * This command sequence performs the following actions: - * 1. Resets the drivetrain position. - * 2. Drives forward to the wall. - * 3. Strafes left to a specific position (ID7). - * 4. Corrects any drift from the previous command. - * 5. Strafes left again to the same position (ID7). - * 6. Drives backward to the wall. - * - * @param drivetrain The drivetrain subsystem used for driving the robot. - * @param gyro The gyro sensor used for orientation and heading. - * @param camera The camera used for vision processing (currently commented out). - */ + /** + * Command sequence for scoring test in autonomous mode. + * + * This command sequence performs the following actions: + * 1. Resets the drivetrain position. + * 2. Drives forward to the wall. + * 3. Strafes left to a specific position (ID7). + * 4. Corrects any drift from the previous command. + * 5. Strafes left again to the same position (ID7). + * 6. Drives backward to the wall. + * + * @param drivetrain The drivetrain subsystem used for driving the robot. + * @param gyro The gyro sensor used for orientation and heading. + * @param camera The camera used for vision processing (currently commented out). + */ private void scoreL4(Elevator elevator, Wrist wrist, CoralClaw claw) { addCommands( new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.EXTRA_HIGH), new WaitCommand(1.0), new InstantCommand(() -> claw.setOutput(-0.4)), + new WaitCommand(0.1), + new InstantCommand(() -> wrist.retract()), new WaitCommand(1.0), new InstantCommand(() -> claw.setOutput(0.0)), new WaitCommand(0.2), new InstantCommand(() -> elevator.setTarget(ElevatorLevel.STATION)) -); + ); } private void driveOffReef(int direction, Drivetrain drivetrain, Gyro gyro) { addCommands( - new WaitCommand(1.0), - new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.5*direction, 3.14), - new InstantCommand(() -> drivetrain.resetPosition()) - - ); + new WaitCommand(1.0), + new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.5*direction, 3.14) + ); + - } - + public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, Wrist wrist, CoralClaw claw) { - if(side == 0) { // srive forward - addCommands( - new InstantCommand(() -> drivetrain.resetPosition()), - new DriveDistance(0.25, drivetrain, gyro, 2.2, -0.5, 0.0) + // zero the gyro to backwards (forwards) + addCommands( + new InstantCommand(() -> gyro.setGyro(Math.toRadians(180))), + new WaitCommand(0.1) + ); + + + if(side == 0) { // srive forward + addCommands( + new DriveDistance(0.25, drivetrain, gyro, -2.2, 0.0, 0.0) ); - } else if(side == 1) { - addCommands( - new InstantCommand(() -> drivetrain.resetPosition()), - new WaitCommand(0.1), - new DriveDistance(0.25, drivetrain, gyro, 1.5, 0.0, 0.0), - new DriveTowardsThing(1, -1, true, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) - ); - scoreL4(elevator, wrist, claw); - addCommands( - new WaitCommand(0.5), - new DriveDistance(0.25, drivetrain, gyro, 0.0, 1.2, 0.0), - new InstantCommand(() -> wrist.extend()), - new InstantCommand(() -> elevator.setTarget(ElevatorLevel.HIGH)) + } else if(side == 1) { + addCommands( + new DriveDistance(0.25, drivetrain, gyro, -1.0, 0.0, Math.toRadians(180)), + new DriveTowardsThing(1, -1, true, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH) ); - } else if(side == 2) { - addCommands( - new InstantCommand(() -> drivetrain.resetPosition()), - new WaitCommand(0.1), - new DriveDistance(0.25, drivetrain, gyro, 0.0, 0.0, 5.23599), - new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599), - new DriveDistance(0.25, drivetrain, gyro, 0.0, -1.5, 5.23599), - new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) - ); - scoreL4(elevator, wrist, claw); - if(goToStation) { driveOffReef(1, drivetrain, gyro); } - } else if(side == 3) { - addCommands( - new InstantCommand(() -> drivetrain.resetPosition()), - new WaitCommand(0.1), - new DriveDistance(0.25, drivetrain, gyro, 5.0, 0.0, 4.18879), - new DriveDistance(0.25, drivetrain, gyro, 0.0, -1.0, 4.18879), - new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) - ); - scoreL4(elevator, wrist, claw); - if(goToStation) { driveOffReef(1, drivetrain, gyro); } - } else if(side == 5) { - addCommands( - new InstantCommand(() -> drivetrain.resetPosition()), - new WaitCommand(0.1), - new DriveDistance(0.25, drivetrain, gyro, 5.0, 0.0, 2.09439), - new DriveDistance(0.25, drivetrain, gyro, 0.0, 1.0, 2.09439), - new DriveTowardsThing(-1, -1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) - ); - scoreL4(elevator, wrist, claw); - if(goToStation) { driveOffReef(-1, drivetrain, gyro); } - } - - else if(side == 6) { - addCommands( - new InstantCommand(() -> drivetrain.resetPosition()), - new WaitCommand(0.1), - new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472), - new DriveTowardsThing(-1, -1, false, drivetrain, gyro, camera, elevator, ElevatorLevel.EXTRA_HIGH) - ); - scoreL4(elevator, wrist, claw); - if(goToStation) { driveOffReef(-1, drivetrain, gyro); } - } - - - + scoreL4(elevator, wrist, claw); + } else if(side == 2) { + System.out.println("side 2 auto"); + addCommands( + new DriveDistance(0.25, drivetrain, gyro, 0.0, 0.0, IDLookUp.getIdAngle(22)), + new DriveDistance(0.25, drivetrain, gyro, -2.8, 0.0, IDLookUp.getIdAngle(22)), + new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH) + ); + // scoreL4(elevator, wrist, claw); + // if(goToStation) { driveOffReef(1, drivetrain, gyro); } + } else if(side == 3) { + addCommands( + new DriveDistance(0.25, drivetrain, gyro, 5.0, 0.0, 4.18879), + new DriveDistance(0.25, drivetrain, gyro, 0.0, -1.0, 4.18879), + new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH) + ); + scoreL4(elevator, wrist, claw); + if(goToStation) { driveOffReef(1, drivetrain, gyro); } + } else if(side == 5) { + addCommands( + new DriveDistance(0.25, drivetrain, gyro, 5.0, 0.0, 2.09439), + new DriveDistance(0.25, drivetrain, gyro, 0.0, 1.0, 2.09439), + new DriveTowardsThing(-1, -1, false, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH) + ); + scoreL4(elevator, wrist, claw); + if(goToStation) { driveOffReef(-1, drivetrain, gyro); } + } + + else if(side == 6) { + addCommands( + new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472), + new DriveTowardsThing(-1, -1, false, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH) + ); + scoreL4(elevator, wrist, claw); + if(goToStation) { driveOffReef(-1, drivetrain, gyro); } + } + + + // SIDE 2 // addCommands( - // new InstantCommand(() -> drivetrain.resetPosition()), - // new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599), - // new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.1, 5.23599), - // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), - // new WaitCommand(500), - // new InstantCommand(() -> claw.setOutput(-0.4)), - // new WaitCommand(1000.0), - // new InstantCommand(() -> claw.setOutput(0.0)) - // ); - - - // new InstantCommand(() -> drivetrain.driveRobotRelative(0.25, 0.0, 0.0)) + // new InstantCommand(() -> drivetrain.resetPosition()), + // new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599), + // new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.1, 5.23599), + // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), + // new WaitCommand(500), + // new InstantCommand(() -> claw.setOutput(-0.4)), + // new WaitCommand(1000.0), + // new InstantCommand(() -> claw.setOutput(0.0)) + // ); + + + // new InstantCommand(() -> drivetrain.driveRobotRelative(0.25, 0.0, 0.0)) // SIDE 1 L1 // addCommands( - // new InstantCommand(() -> drivetrain.resetPosition()), - // new DriveDistance(0.5, drivetrain, gyro, 2.73, 0.0, 0.0), // drive to wall - // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), - // new WaitCommand(500.0), - // new InstantCommand(() -> claw.setOutput(-0.4)), - // new WaitCommand(1000.0), - // new InstantCommand(() -> claw.setOutput(0.0)) - // ); + // new InstantCommand(() -> drivetrain.resetPosition()), + // new DriveDistance(0.5, drivetrain, gyro, 2.73, 0.0, 0.0), // drive to wall + // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), + // new WaitCommand(500.0), + // new InstantCommand(() -> claw.setOutput(-0.4)), + // new WaitCommand(1000.0), + // new InstantCommand(() -> claw.setOutput(0.0)) + // ); // ID7 from outer // addCommands( - // new InstantCommand(() -> drivetrain.resetPosition()), - // new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall - // new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7 - // new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command - // new DriveTowardsThing(drivetrain, camera) - // ); + // new InstantCommand(() -> drivetrain.resetPosition()), + // new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall + // new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7 + // new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command + // new DriveTowardsThing(drivetrain, camera) + // ); } } diff --git a/src/main/java/frc/robot/subsystems/CoralClaw.java b/src/main/java/frc/robot/subsystems/CoralClaw.java index 705c246..8b2a472 100644 --- a/src/main/java/frc/robot/subsystems/CoralClaw.java +++ b/src/main/java/frc/robot/subsystems/CoralClaw.java @@ -108,6 +108,8 @@ public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, SparkMax algaeM config.idleMode(IdleMode.kCoast); primaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); secondaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + config.idleMode(IdleMode.kBrake); algaeMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); // config.follow(primaryMotor, true); // true makes it follow with inversion @@ -162,7 +164,7 @@ public void setMotorsOn(boolean left, boolean right) { public void stop() { primaryMotor.set(0); secondaryMotor.set(0); - algaeMotor.set(0); + algaeMotor.set(0.05); // Secondary motor automatically follows with inversion // autoIntakeMode = false; SmartDashboard.putString("Coral Claw State", "Stopped"); @@ -216,7 +218,7 @@ public void setSimulatedCoralDetected(boolean detected) { public void setOutput(double out) { if(motorsOn[0]) { primaryMotor.set(out); } if(motorsOn[1]) { secondaryMotor.set(-out); } - algaeMotor.set(-out); + algaeMotor.set(-out); } @Override diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 0fec978..a22c358 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -169,19 +169,19 @@ public void setTarget(ElevatorLevel level) { // Tune these values to match the actual robot requirmenets to seed the coral switch (level) { case STATION: - groundHeight = Units.inchesToMeters(28.0); // all these measurements are from measuring the field components + groundHeight = Units.inchesToMeters(27.75); // all these measurements are from measuring the field components break; case LOW: - groundHeight = Units.inchesToMeters(18.5); + groundHeight = Units.inchesToMeters(19.0); break; case MED: - groundHeight = Units.inchesToMeters(29.5); + groundHeight = Units.inchesToMeters(30.0); break; case HIGH: - groundHeight = Units.inchesToMeters(41.25); + groundHeight = Units.inchesToMeters(46.25); break; case EXTRA_HIGH: - groundHeight = Units.inchesToMeters(64.75); + groundHeight = Units.inchesToMeters(71.75); break; default: groundHeight = 0.0; diff --git a/src/main/java/frc/robot/subsystems/Gyro.java b/src/main/java/frc/robot/subsystems/Gyro.java index 60c2335..a7d94f5 100644 --- a/src/main/java/frc/robot/subsystems/Gyro.java +++ b/src/main/java/frc/robot/subsystems/Gyro.java @@ -20,6 +20,7 @@ public Rotation2d getRotation2d() { } public void reset() { + offset = new Rotation2d(0.0); gyro.reset(); } @@ -34,7 +35,7 @@ public void setGyro(double desiredAngle) { @Override public void periodic() { - SmartDashboard.putNumber("gyro angle", Math.toDegrees(gyro.getRotation2d().getRadians())); + SmartDashboard.putNumber("gyro angle", Math.toDegrees(getRotation2d().getRadians())); } } From f763b76be881b243852c2e81bac6aac1d4a25fad Mon Sep 17 00:00:00 2001 From: p-ifka Date: Thu, 3 Apr 2025 17:42:07 -0400 Subject: [PATCH 14/15] district 1 --- src/main/java/frc/robot/OI.java | 4 +++ src/main/java/frc/robot/RobotContainer.java | 6 ++--- .../frc/robot/commands/auto/ScoreTest.java | 5 ++-- .../robot/commands/instant/ClawCommand.java | 25 ++++++++++++++----- .../java/frc/robot/subsystems/CoralClaw.java | 8 ++++-- .../java/frc/robot/subsystems/Elevator.java | 12 ++++----- 6 files changed, 41 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 8079ec3..71581d0 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -38,6 +38,7 @@ private static class PORT { private static class DRIVER_MAP { private static final int ABUTTON = XboxController.Button.kA.value; private static final int BBUTTON = XboxController.Button.kB.value; + private static final int LEFT_BUMPER = XboxController.Button.kLeftBumper.value; } // drive sticks @@ -49,7 +50,10 @@ private static class DRIVER_MAP { // drive buttons public static final JoystickButton driveControllerA = new JoystickButton(driveController, DRIVER_MAP.ABUTTON); public static final JoystickButton driveControllerB = new JoystickButton(driveController, DRIVER_MAP.BBUTTON); + public static final JoystickButton driveControllerLB = new JoystickButton(driveController, DRIVER_MAP.LEFT_BUMPER); + public static DoubleSupplier driveControllerRightTriggerSupplier = () -> { return driveController.getRightTriggerAxis();}; + public static DoubleSupplier driveControllerLeftTriggerSupplier = () -> { return driveController.getLeftTriggerAxis();}; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9094f9e..30ef1e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -76,8 +76,8 @@ private void configureBindings() { OI.driveControllerA.onTrue(Commands.runOnce(() -> { drivetrain.resetPosition(); })); - OI.driveControllerB.whileTrue(new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH)); + OI.driveControllerLB.whileTrue(new AlgaeRemoverCommand(coralClaw)); // Operator controller bindings // Elevator level controls // OPERATOR B : INTAKE @@ -100,9 +100,9 @@ private void configureBindings() { OI.operatorControllerBack.onTrue(new InstantCommand(() -> elevator.trimTarget(-0.0175))); // OPERATOR RT : INTAKE - OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorRightTriggerSupplier, 1)); + OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, elevator, OI.operatorRightTriggerSupplier, 1)); // OPERATOR LT : OUTTAKE - OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorLeftTriggerSupplier, 0)); + OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, elevator, OI.operatorLeftTriggerSupplier, 0)); // Manual control with right stick for testing in simulation // OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> diff --git a/src/main/java/frc/robot/commands/auto/ScoreTest.java b/src/main/java/frc/robot/commands/auto/ScoreTest.java index 5c429d5..5b2b180 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreTest.java +++ b/src/main/java/frc/robot/commands/auto/ScoreTest.java @@ -116,8 +116,9 @@ public ScoreTest(int side, boolean goToStation, Drivetrain drivetrain, Gyro gyro else if(side == 6) { addCommands( - new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472), - new DriveTowardsThing(-1, -1, false, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH) + new DriveDistance(0.25, drivetrain, gyro, 0.0, 0.0, IDLookUp.getIdAngle(20)), + new DriveDistance(0.25, drivetrain, gyro, -2.8, 0.0, IDLookUp.getIdAngle(20)), + new DriveTowardsThing(1, 1, false, drivetrain, gyro, camera, elevator, wrist, ElevatorLevel.EXTRA_HIGH) ); scoreL4(elevator, wrist, claw); if(goToStation) { driveOffReef(-1, drivetrain, gyro); } diff --git a/src/main/java/frc/robot/commands/instant/ClawCommand.java b/src/main/java/frc/robot/commands/instant/ClawCommand.java index ec3b0f6..f64acb3 100644 --- a/src/main/java/frc/robot/commands/instant/ClawCommand.java +++ b/src/main/java/frc/robot/commands/instant/ClawCommand.java @@ -1,19 +1,24 @@ package frc.robot.commands.instant; import frc.robot.subsystems.CoralClaw; +import frc.robot.subsystems.Elevator; +import frc.robot.ElevatorLevel; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.DoubleSupplier; public class ClawCommand extends Command { CoralClaw claw; + Elevator elevator; double speed; int outin; DoubleSupplier triggerSupplier; - - public ClawCommand(CoralClaw claw, DoubleSupplier triggerSupplier, int outin) { + + public ClawCommand(CoralClaw claw, Elevator elevator, DoubleSupplier triggerSupplier, int outin) { addRequirements(claw); this.claw = claw; + this.elevator = elevator; + this.speed = claw.INTAKE_SPEED; this.outin = outin; this.triggerSupplier = triggerSupplier; @@ -21,20 +26,28 @@ public ClawCommand(CoralClaw claw, DoubleSupplier triggerSupplier, int outin) { @Override public void initialize() { System.out.println("CLAWCOMMAND INIT"); } - + @Override public void execute() { if(outin == 0) { // OUT - claw.setOutput(-(triggerSupplier.getAsDouble() * 0.4 + 0.2)); + if(elevator.getTargetLevel() == ElevatorLevel.LOW) { + claw.setOutput(-(triggerSupplier.getAsDouble() * 0.5 + 0.4)); + } else { + claw.setOutput(-(triggerSupplier.getAsDouble() * 0.4 + 0.2)); + claw.setAlgaeRemoverOutput(triggerSupplier.getAsDouble() * 0.5 + 0.2); + } + } else { // INTAKE claw.setOutput(triggerSupplier.getAsDouble() * 0.2 + 0.2); + claw.setAlgaeRemoverOutput(-(triggerSupplier.getAsDouble() * 0.2 + 0.2)); } - + } - + @Override public void end(boolean isInterrupted) { claw.stop(); + claw.setAlgaeRemoverOutput(0.1); } } diff --git a/src/main/java/frc/robot/subsystems/CoralClaw.java b/src/main/java/frc/robot/subsystems/CoralClaw.java index 8b2a472..c6e0021 100644 --- a/src/main/java/frc/robot/subsystems/CoralClaw.java +++ b/src/main/java/frc/robot/subsystems/CoralClaw.java @@ -164,7 +164,7 @@ public void setMotorsOn(boolean left, boolean right) { public void stop() { primaryMotor.set(0); secondaryMotor.set(0); - algaeMotor.set(0.05); + // algaeMotor.set(0.05); // Secondary motor automatically follows with inversion // autoIntakeMode = false; SmartDashboard.putString("Coral Claw State", "Stopped"); @@ -218,8 +218,12 @@ public void setSimulatedCoralDetected(boolean detected) { public void setOutput(double out) { if(motorsOn[0]) { primaryMotor.set(out); } if(motorsOn[1]) { secondaryMotor.set(-out); } - algaeMotor.set(-out); } + + public void setAlgaeRemoverOutput(double out) { + algaeMotor.set(out); + } + @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index a22c358..f46f1d2 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -128,7 +128,7 @@ public void setSpeed(double speed) { // Making P_GAIN higher for raising, lower for lowering. To prevent equipment damage and overshoot when lowering. if (currentHeight > targetHeight){ - P_GAIN = 1.5; + P_GAIN = 2.0; } else { P_GAIN = 4; @@ -169,19 +169,19 @@ public void setTarget(ElevatorLevel level) { // Tune these values to match the actual robot requirmenets to seed the coral switch (level) { case STATION: - groundHeight = Units.inchesToMeters(27.75); // all these measurements are from measuring the field components + groundHeight = Units.inchesToMeters(27.25); // all these measurements are from measuring the field components break; case LOW: - groundHeight = Units.inchesToMeters(19.0); + groundHeight = Units.inchesToMeters(19.5); break; case MED: - groundHeight = Units.inchesToMeters(30.0); + groundHeight = Units.inchesToMeters(30.5); break; case HIGH: - groundHeight = Units.inchesToMeters(46.25); + groundHeight = Units.inchesToMeters(47.25); break; case EXTRA_HIGH: - groundHeight = Units.inchesToMeters(71.75); + groundHeight = Units.inchesToMeters(72.0); break; default: groundHeight = 0.0; From 55b0351005d5ca9bfeb2742c3a6509a42b7c65c2 Mon Sep 17 00:00:00 2001 From: p-ifka Date: Thu, 3 Apr 2025 17:47:05 -0400 Subject: [PATCH 15/15] district 1 untracked files --- src/main/java/frc/robot/IDLookUp.java | 50 +++++++++++++++++++ .../commands/instant/AlgaeRemoverCommand.java | 28 +++++++++++ 2 files changed, 78 insertions(+) create mode 100644 src/main/java/frc/robot/IDLookUp.java create mode 100644 src/main/java/frc/robot/commands/instant/AlgaeRemoverCommand.java diff --git a/src/main/java/frc/robot/IDLookUp.java b/src/main/java/frc/robot/IDLookUp.java new file mode 100644 index 0000000..ff4e2aa --- /dev/null +++ b/src/main/java/frc/robot/IDLookUp.java @@ -0,0 +1,50 @@ +package frc.robot; + +public class IDLookUp { + public static double getIdAngle(int id) { + double output = 0.0; + switch (id) { + case 6: + output = Math.toRadians(300.0); + break; + case 7: + output = Math.toRadians(0.0); + break; + case 8: + output = Math.toRadians(60.0); + break; + case 9: + output = Math.toRadians(120.0); + break; + case 10: + output = Math.toRadians(180.0); + break; + case 11: + output = Math.toRadians(240.0); + break; + case 17: + output = Math.toRadians(60.0); + break; + case 18: + output = Math.toRadians(0.0); + break; + case 19: + output = Math.toRadians(300.0); + break; + case 20: + output = Math.toRadians(240.0); + break; + case 21: + output = Math.toRadians(180.0); + break; + case 22: + output = Math.toRadians(120.0); + break; + default: + output = Math.toRadians(0.0); + break; + } + return output; + + } +} diff --git a/src/main/java/frc/robot/commands/instant/AlgaeRemoverCommand.java b/src/main/java/frc/robot/commands/instant/AlgaeRemoverCommand.java new file mode 100644 index 0000000..a044c41 --- /dev/null +++ b/src/main/java/frc/robot/commands/instant/AlgaeRemoverCommand.java @@ -0,0 +1,28 @@ +package frc.robot.commands.instant; + +import frc.robot.subsystems.CoralClaw; + +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.DoubleSupplier; + +public class AlgaeRemoverCommand extends Command { + CoralClaw claw; + + public AlgaeRemoverCommand(CoralClaw claw) { + addRequirements(claw); + this.claw = claw; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + claw.setAlgaeRemoverOutput(0.76); + } + + @Override + public void end(boolean isInterrupted) { + claw.setAlgaeRemoverOutput(0.0); + } +}