diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8ee2599c..20fa887e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,6 +48,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.ExtensionKinematics; import frc.robot.subsystems.ExtensionKinematics.ExtensionState; +import frc.robot.subsystems.ExtensionPathing; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.ManipulatorSubsystem; import frc.robot.subsystems.Superstructure; @@ -123,6 +124,9 @@ private RobotHardware(SwerveConstants swerveConstants) { public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; // For replay to work properly this should match the hardware used in the log public static final RobotHardware ROBOT_HARDWARE = RobotHardware.KELPIE; + // for testing class loading + public static final ExtensionState test = + ExtensionPathing.getNearest(new ExtensionState(0.0, Rotation2d.kZero, Rotation2d.kZero)); public static enum ReefTarget { L1( @@ -338,7 +342,7 @@ public static enum AlgaeScoreTarget { .withMotionMagic(WristSubsystem.DEFAULT_MOTION_MAGIC) .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(30.0) + .withStatorCurrentLimit(50.0) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimit(20.0) .withSupplyCurrentLimitEnable(true)) @@ -618,6 +622,60 @@ public Robot() { }) .ignoringDisable(true)); + System.out.println("Node Count " + ExtensionPathing.graph.nodes().size()); + + SmartDashboard.putData( + "Traverse Extension Graph", + superstructure + .extendWithClearance( + () -> + new ExtensionState( + ElevatorSubsystem.HP_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_HP_POS, + WristSubsystem.WRIST_HP_POS)) + .until( + () -> + elevator.isNearExtension(ElevatorSubsystem.HP_EXTENSION_METERS) + && shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_HP_POS) + && wrist.isNearAngle(WristSubsystem.WRIST_HP_POS)) + .andThen( + Commands.sequence( + ExtensionPathing.graph.nodes().stream() + .map( + (node) -> + superstructure + .extendWithClearance(() -> node) + .alongWith( + Commands.print("Traversing to " + node), + Commands.runOnce( + () -> Logger.recordOutput("Traversal Target", node))) + .until( + () -> + elevator.isNearExtension(node.elevatorHeightMeters()) + && shoulder.isNearAngle(node.shoulderAngle()) + && wrist.isNearAngle(node.wristAngle())) + .finallyDo(() -> System.out.println("done")) + .andThen( + Commands.waitSeconds(0.5), + superstructure + .extendWithClearance( + () -> + new ExtensionState( + ElevatorSubsystem.HP_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_HP_POS, + WristSubsystem.WRIST_HP_POS)) + .alongWith(Commands.print("Retracting")) + .until( + () -> + elevator.isNearExtension( + ElevatorSubsystem.HP_EXTENSION_METERS) + && shoulder.isNearAngle( + ShoulderSubsystem.SHOULDER_HP_POS) + && wrist.isNearAngle( + WristSubsystem.WRIST_HP_POS)), + Commands.waitSeconds(0.5))) + .toArray(Command[]::new)))); + // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); @@ -701,9 +759,9 @@ public Robot() { manipulator.setDefaultCommand(manipulator.hold()); - shoulder.setDefaultCommand(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_RETRACTED_POS)); + shoulder.setDefaultCommand(shoulder.hold()); - wrist.setDefaultCommand(wrist.setTargetAngle(WristSubsystem.WRIST_RETRACTED_POS)); + wrist.setDefaultCommand(wrist.hold()); funnel.setDefaultCommand(funnel.setVoltage(0.0)); diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java new file mode 100644 index 00000000..50c47e74 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -0,0 +1,199 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.google.common.collect.Sets; +import com.google.common.graph.GraphBuilder; +import com.google.common.graph.MutableGraph; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.ExtensionKinematics.ExtensionState; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.shoulder.ShoulderSubsystem; +import frc.robot.subsystems.wrist.WristSubsystem; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Set; + +public class ExtensionPathing { + public static final MutableGraph graph = + GraphBuilder.undirected().allowsSelfLoops(true).build(); + // TODO make this cache distances so we can do partial caches + private static final Map, List> cache = + new HashMap<>(); + + static { + final var hp = + new ExtensionState( + ElevatorSubsystem.HP_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_HP_POS, + WristSubsystem.WRIST_HP_POS); + graph.addNode(hp); + final var tucked = + new ExtensionState( + 0.0, + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, + WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); + graph.addNode(tucked); + graph.putEdge(hp, tucked); + final var l2Tucked = + new ExtensionState( + ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(), + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, + WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); + graph.addNode(l2Tucked); + graph.putEdge(tucked, l2Tucked); + final var l3Tucked = + new ExtensionState( + ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(), + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, + WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); + graph.addNode(l3Tucked); + graph.putEdge(tucked, l3Tucked); + graph.putEdge(l3Tucked, l2Tucked); + final var l4Tucked = + new ExtensionState( + ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(), + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, + WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); + graph.addNode(l4Tucked); + graph.putEdge(tucked, l4Tucked); + graph.putEdge(l4Tucked, l3Tucked); + graph.putEdge(l4Tucked, l2Tucked); + final var l4TuckedOut = + new ExtensionState( + ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(), + Rotation2d.fromDegrees(25.0), + Rotation2d.fromDegrees(120.0)); + graph.addNode(l4TuckedOut); + graph.putEdge(l4Tucked, l4TuckedOut); + + final var betweenTucked = + new ExtensionState(0.0, Rotation2d.fromDegrees(35.0), WristSubsystem.WRIST_CLEARANCE_POS); + graph.addNode(betweenTucked); + graph.putEdge(tucked, betweenTucked); + + final var untucked = + new ExtensionState( + 0.0, ShoulderSubsystem.SHOULDER_CLEARANCE_POS, WristSubsystem.WRIST_CLEARANCE_POS); + graph.addNode(untucked); + graph.putEdge(betweenTucked, untucked); + + final var preCoralGround = + new ExtensionState( + ElevatorSubsystem.GROUND_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, + Rotation2d.kCCW_90deg); + graph.addNode(preCoralGround); + graph.putEdge(tucked, preCoralGround); + + final var coralGround = + new ExtensionState( + ElevatorSubsystem.GROUND_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, + WristSubsystem.WRIST_CORAL_GROUND); + graph.addNode(coralGround); + graph.putEdge(preCoralGround, coralGround); + + final var retracted = + new ExtensionState( + 0.0, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_RETRACTED_POS); + graph.addNode(retracted); + graph.putEdge(untucked, retracted); + graph.putEdge(betweenTucked, retracted); + + final var l1 = ExtensionKinematics.L1_EXTENSION; + graph.addNode(l1); + graph.putEdge(l1, betweenTucked); + } + + private ExtensionPathing() {} + + public static ExtensionState getNearest(ExtensionState state) { + return graph.nodes().stream() + .min( + (a, b) -> { + var aFK = ExtensionKinematics.solveFK(a); + var bFK = ExtensionKinematics.solveFK(b); + var stateFK = ExtensionKinematics.solveFK(state); + return (int) + (1000 + * (aFK.getTranslation().getDistance(stateFK.getTranslation()) + + Math.abs( + aFK.getRotation().getRotations() + - stateFK.getRotation().getRotations())) + - 1000 + * (bFK.getTranslation().getDistance(stateFK.getTranslation()) + + Math.abs( + bFK.getRotation().getRotations() + - stateFK.getRotation().getRotations()))); + }) + .get(); + } + + private record TotalMotion(double elevator, double shoulderRotations, double wristRotations) { + public TotalMotion plus(double elevator, double shoulderRotations, double wristRotations) { + return new TotalMotion( + this.elevator + elevator, + this.shoulderRotations + shoulderRotations, + this.wristRotations + wristRotations); + } + } + + private static Pair, TotalMotion> search( + ExtensionState current, ExtensionState target, Set visited) { + if (current == target) { + List result = new ArrayList<>(); + result.add(target); + return Pair.of(result, new TotalMotion(0, 0, 0)); + } + + final var edges = Sets.difference(graph.successors(current), visited); + final List, TotalMotion>> result = new ArrayList<>(); + for (var edge : edges) { + final var path = search(edge, target, Sets.union(visited, Set.of(current))); + if (path != null) result.add(path); + } + if (result.size() == 0) return null; + final var best = + result.stream() + .min( + Comparator.comparing( + (Pair, TotalMotion> s) -> s.getSecond().elevator) + .thenComparing((s) -> s.getSecond().wristRotations) + .thenComparing((s) -> s.getSecond().shoulderRotations)) + .get(); + final List newList = new ArrayList<>(); + newList.add(current); + newList.addAll(best.getFirst()); + return Pair.of( + newList, + best.getSecond() + .plus( + Math.abs( + current.elevatorHeightMeters() - best.getFirst().get(0).elevatorHeightMeters()), + Math.abs( + current.shoulderAngle().getRotations() + - best.getFirst().get(0).shoulderAngle().getRotations()), + Math.abs( + current.wristAngle().getRotations() + - best.getFirst().get(0).wristAngle().getRotations()))); + } + + public static List getPath(ExtensionState current, ExtensionState target) { + final var nearestCurrent = getNearest(current); + final var nearestTarget = getNearest(target); + final var path = + cache.containsKey(Pair.of(nearestCurrent, nearestTarget)) + ? cache.get(Pair.of(nearestCurrent, nearestTarget)) + : search(nearestCurrent, nearestTarget, Set.of()).getFirst(); + path.add(target); + cache.putIfAbsent(Pair.of(nearestCurrent, nearestTarget), path); + return path; + } +} diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 664b90d3..4dae0079 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -78,12 +78,12 @@ public void periodic() { if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput(NAME + "/Filtered Current", currentFilterValue); - if (firstBBInputs.get && !secondBBInputs.get) { + if (getFirstBeambreak() && !getSecondBeambreak()) { Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(0.0)); zeroTimer.reset(); } - if (!firstBBInputs.get && secondBBInputs.get) { + if (!getFirstBeambreak() && getSecondBeambreak()) { // Number calculated from coral length, may need tuning Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(1.0)); zeroTimer.reset(); @@ -95,16 +95,16 @@ public void periodic() { public Command index() { return Commands.sequence( setVelocity(9.0) - .until(() -> firstBBInputs.get || secondBBInputs.get) - .unless(() -> firstBBInputs.get), - setVelocity(3.0).until(() -> secondBBInputs.get).unless(() -> secondBBInputs.get), + .until(() -> getFirstBeambreak() || getSecondBeambreak()) + .unless(() -> getFirstBeambreak()), + setVelocity(3.0).until(() -> getSecondBeambreak()).unless(() -> getSecondBeambreak()), setVelocity(-3.0) - .until(() -> firstBBInputs.get && !secondBBInputs.get) + .until(() -> getFirstBeambreak() && !getSecondBeambreak()) .unless(() -> zeroTimer.get() < 0.25), // TODO tune timeout // Commands.runOnce(() -> io.resetEncoder(0.0)), Commands.run(() -> io.setPosition(Rotation2d.fromRotations(1.1))) - .until(() -> !firstBBInputs.get && !secondBBInputs.get)); + .until(() -> !getFirstBeambreak() && !getSecondBeambreak())); } // TODO check if anything got lost in merge? public Command jog(double rotations) { @@ -146,26 +146,26 @@ public Command intakeCoral() { public Command intakeCoralAir(double vel) { return Commands.sequence( setVelocity(vel) - .until(() -> secondBBInputs.get) + .until(() -> getSecondBeambreak()) .finallyDo( () -> { io.setPosition(Rotation2d.fromRotations(0.63)); positionSetpoint = 0.63; }), - setVoltage(2.0).until(() -> !firstBBInputs.get), - jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); + setVoltage(2.0).until(() -> !getFirstBeambreak()), + jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak())); } public Command intakeCoral(double vel) { return Commands.sequence( - setVelocity(vel).until(new Trigger(() -> secondBBInputs.get).debounce(0.5)), + setVelocity(vel).until(new Trigger(() -> getSecondBeambreak()).debounce(0.5)), Commands.runOnce( () -> { io.setPosition(Rotation2d.fromRotations(0.5)); positionSetpoint = 0.5; }), - setVelocity(1.0).until(() -> !firstBBInputs.get), - jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); + setVelocity(1.0).until(() -> !getFirstBeambreak()), + jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak())); } public Command intakeAlgae() { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 5551b756..9cec1a6f 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -23,7 +23,10 @@ import frc.robot.utils.autoaim.CoralTargets; import frc.robot.utils.autoaim.HumanPlayerTargets; import java.util.HashMap; +import java.util.List; import java.util.Map; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.atomic.AtomicReference; import java.util.function.DoubleSupplier; import java.util.function.Supplier; import java.util.stream.Stream; @@ -300,7 +303,10 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.GROUND_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, WristSubsystem.WRIST_CORAL_GROUND) - .until(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS)) + .until( + () -> + shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS) + && wrist.isNearAngle(WristSubsystem.WRIST_CORAL_GROUND)) .andThen( Commands.parallel( shoulder.setVoltage(-1.0), @@ -870,91 +876,35 @@ private Command extendWithClearance( DoubleSupplier elevatorExtension, Supplier shoulderAngle, Supplier wristAngle) { + final AtomicReference> path = new AtomicReference<>(); + final AtomicInteger index = new AtomicInteger(0); return Commands.sequence( - // Retract shoulder + wrist - Commands.parallel( - shoulder - .run(() -> {}) - .until( - () -> - wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) - || wrist.getAngle().getDegrees() - 115.0 - > shoulder.getAngle().getDegrees()) - .andThen( - Commands.either( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - shoulder.setTargetAngle( - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get()))), - Commands.either( - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get())), - elevator.hold()) - // .unless( - // () -> - // shoulder.getAngle().getDegrees() - // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() - // && wrist.getAngle().getDegrees() < 90.0) - .until( - () -> - shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) - || wrist.getAngle().getDegrees() - 115.0 - > shoulder.getAngle().getDegrees() - && wrist.isNearTarget()) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), - // extend elevator - Commands.parallel( - Commands.either( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get())), - Commands.either( - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get())), - elevator.setExtension(elevatorExtension)) - .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), - // re-extend joints - Commands.parallel( - shoulder - .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) - // .unless( - // () -> - // shouldntTuck() - // || shoulderAngle.get().getDegrees() - // < - // ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) - .until( - () -> - wrist.isNearTarget() - && (wrist.getAngle().getDegrees() < 120.0 - || !elevator.isNearExtension(1.4, 0.25))) - .withTimeout(1.0) - .andThen(shoulder.setTargetAngle(shoulderAngle)), - wrist - .hold() - .until( - new Trigger(() -> shoulder.isNearAngle(shoulderAngle.get())) - .debounce(0.040)) - .withTimeout(0.5) - .unless( - () -> - wristAngle.get().getDegrees() < 90.0 - || shoulderAngle.get().getDegrees() - > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees() - + 5) - .andThen(wrist.setTargetAngle(wristAngle)), - elevator.setExtension(elevatorExtension))) - .finallyDo( - (interrupted) -> { - if (interrupted) { - System.out.println("interrupted clearance extend"); - } - }); + Commands.runOnce( + () -> { + index.set(0); + path.set( + ExtensionPathing.getPath( + getExtensionState(), + new ExtensionState( + elevatorExtension.getAsDouble(), shoulderAngle.get(), wristAngle.get()))); + }), + holdExtension(() -> path.get().get(index.get())) + .until( + () -> + elevator.isNearExtension( + path.get().get(index.get()).elevatorHeightMeters(), 0.2) + && shoulder.isNearAngle( + path.get().get(index.get()).shoulderAngle(), + Rotation2d.fromDegrees(10.0)) + && wrist.isNearAngle( + path.get().get(index.get()).wristAngle(), Rotation2d.fromDegrees(20.0))) + .finallyDo(() -> index.set(index.get() + 1)) + .repeatedly() + .until(() -> index.get() == path.get().size() - 1), + holdExtension( + () -> + new ExtensionState( + elevatorExtension.getAsDouble(), shoulderAngle.get(), wristAngle.get()))); } private Command holdExtension(Supplier state) { diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java index 49be3de9..03ee8bab 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java @@ -28,7 +28,7 @@ public class ShoulderIOSim implements ShoulderIO { private final ArmFeedforward feedforward = new ArmFeedforward(0.0, 0.0, 0.0); // 1.31085, 0.278); private final ProfiledPIDController pid = - new ProfiledPIDController(60.0, 0.0, 6.0, new TrapezoidProfile.Constraints(10.0, 10.0)); + new ProfiledPIDController(80.0, 0.0, 6.0, new TrapezoidProfile.Constraints(10.0, 10.0)); private double appliedVoltage = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 2d3a3db2..5ed27160 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -150,6 +150,11 @@ public boolean isNearAngle(Rotation2d target) { return MathUtil.isNear(target.getDegrees(), inputs.position.getDegrees(), 10.0); } + public boolean isNearAngle(Rotation2d target, Rotation2d tolerance) { + return MathUtil.isNear( + target.getDegrees(), inputs.position.getDegrees(), tolerance.getDegrees()); + } + public ShoulderIOInputsAutoLogged getInputs() { return inputs; } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java index 419d0f49..42e5c350 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java @@ -27,7 +27,7 @@ public class WristIOSim implements WristIO { private final ArmFeedforward feedforward = new ArmFeedforward(0.0, 1.0, 0.0); private final ProfiledPIDController pid = - new ProfiledPIDController(30.0, 0.0, 0.1, new TrapezoidProfile.Constraints(10.0, 10.0)); + new ProfiledPIDController(50.0, 0.0, 0.1, new TrapezoidProfile.Constraints(10.0, 10.0)); private double appliedVoltage = 0.0; diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 65dd63ad..f78ca024 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -43,10 +43,10 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(5); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(5); public static MotionMagicConfigs SLOW_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(3); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(3); public static MotionMagicConfigs CRAWL_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(2); @@ -129,6 +129,11 @@ public boolean isNearAngle(Rotation2d target) { return MathUtil.isNear(target.getDegrees(), inputs.position.getDegrees(), 10.0); } + public boolean isNearAngle(Rotation2d target, Rotation2d tolerance) { + return MathUtil.isNear( + target.getDegrees(), inputs.position.getDegrees(), tolerance.getDegrees()); + } + public Command currentZero(Supplier shoulderInputs) { return Commands.sequence( this.runOnce(