diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8ee2599c..034250c4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -806,7 +806,7 @@ public Robot() { AutoAim.approachAlgae( swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), - 0.75)), + 1)), Commands.waitUntil( new Trigger( () -> diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 8329ced4..3d32ba0b 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -51,6 +51,7 @@ public class ExtensionKinematics { public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = new Pose2d(new Translation2d(0.23, 2.05), Rotation2d.fromDegrees(110.0)); + public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 45ed7f3b..48eed6b7 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -776,10 +776,11 @@ private void configureStateTransitionCommands() { .whileTrue( Commands.parallel( elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS), + // Make it initially extend to the full 90 degrees + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS), wrist.setSlowTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS))) .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) - .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS)) + .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) .and(scoreReq) .onTrue(forceState(SuperState.SCORE_ALGAE_NET)); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d8ac1320..3cfb6e75 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -56,7 +56,9 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); - public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(61.5); + // 1 inch high (maybe not needed?) + public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(62.5); + public static final double ALGAE_PROCESSOR_EXTENSION = 0.01; public static final double HP_EXTENSION_METERS = Units.inchesToMeters(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..5c0d21dc 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -39,6 +39,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-20.0); + public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(105); public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); diff --git a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java b/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java index bfb2d116..105e35a4 100644 --- a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java +++ b/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.util.Units; import frc.robot.Robot; import frc.robot.Robot.AlgaeIntakeTarget; import java.util.Arrays; @@ -55,7 +56,8 @@ public static Pose2d getRobotTargetLocation(Pose2d original) { } public static Pose2d getOffsetLocation(Pose2d original) { - return original.transformBy(new Transform2d((-0.3), 0, Rotation2d.kZero)); + return original.transformBy( + new Transform2d((-0.3 - Units.inchesToMeters(6)), 0, Rotation2d.kZero)); } /** Gets the closest offset target to the given pose. */ diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index a249680a..078fa79b 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -32,7 +32,7 @@ public class AutoAim { new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); public static final Translation2d RED_REEF_CENTER = ChoreoAllianceFlipUtil.flip(BLUE_REEF_CENTER); - public static double BLUE_NET_X = 8.08; + public static double BLUE_NET_X = 8.08 + Units.inchesToMeters(4); public static double RED_NET_X = ChoreoAllianceFlipUtil.flipX(BLUE_NET_X); public static Pose2d BLUE_PROCESSOR_POS = new Pose2d(5.973, 0, Rotation2d.fromDegrees(270));